+comp

src/enc.comp

@@ -6,7 +6,29 @@ HAL_PIN(pos) = 0.0;
 MEM(int e_res) = 0.0;
 
 INIT(
+  // enable pin clock
+  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+
+  // pin mode: af
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  // af mode: tim3
+  GPIO_PinAFConfig(GPIOB, GPIO_PinSource4 | GPIO_PinSource5, GPIO_AF_TIM3);
+
+  // tim3 mode: count on A and B on rising edge
+  TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
+
+  // enc res / turn
   TIM_SetAutoreload(TIM3, PIN(res) - 1);
+
+  // enable tim3
+  TIM_Cmd(TIM3, ENABLE);
 );
 
 RT_IN(

src/pid.comp

@@ -0,0 +1,202 @@
+COMP(pid);
+
+//HAL_PIN(pos_cmd_ext) = 0.0;
+//HAL_PIN(pos_fb) = 0.0;
+HAL_PIN(pos_error) = 0.0;
+
+HAL_PIN(vel_cmd) = 0.0;
+HAL_PIN(vel_ext_cmd) = 0.0;
+HAL_PIN(vel_fb) = 0.0;
+HAL_PIN(vel_error) = 0.0;
+
+HAL_PIN(acc_cmd) = 0.0;
+HAL_PIN(acc_ext_cmd) = 0.0;
+
+HAL_PIN(force_cmd) = 0.0;
+HAL_PIN(force_ext_cmd) = 0.0;
+
+HAL_PIN(cur_cmd) = 0.0;
+HAL_PIN(cur_fb) = 0.0;
+HAL_PIN(cur_error) = 0.0;
+
+HAL_PIN(volt_cmd) = 0.0;
+
+HAL_PIN(pwm_cmd) = 0.0;
+
+
+HAL_PIN(enable) = 0.0;
+
+HAL_PIN(pos_en) = 0.0;
+HAL_PIN(pos_p) = 0.0;
+HAL_PIN(pos_lp) = 0.0;
+
+HAL_PIN(vel_en) = 0.0;
+HAL_PIN(vel_p) = 0.0;
+HAL_PIN(vel_i) = 0.0;
+//HAL_PIN(vel_d) = 0.0;
+HAL_PIN(vel_ff) = 0.0;
+HAL_PIN(vel_lp) = 0.0;
+
+HAL_PIN(acc_p) = 0.0;
+HAL_PIN(acc_ff) = 0.0;
+
+HAL_PIN(force_p) = 0.0;
+
+HAL_PIN(cur_p) = 0.0;
+//HAL_PIN(cur_i) = 0.0;
+HAL_PIN(cur_d) = 0.0;
+HAL_PIN(cur_ind) = 0.0;
+HAL_PIN(cur_lp) = 0.0;
+
+HAL_PIN(volt) = 0.0;
+
+
+HAL_PIN(vel_max) = 0.0;
+HAL_PIN(vel_error_sum_max) = 0.0;
+HAL_PIN(acc_max) = 0.0;
+HAL_PIN(force_max) = 0.0;
+HAL_PIN(cur_max) = 0.0;
+HAL_PIN(volt_max) = 0.0;
+HAL_PIN(pwm_max) = 0.0;
+
+
+HAL_PIN(saturated) = 0.0;
+
+MEM(float velcmd) = 0.0;
+MEM(float acccmd) = 0.0;
+MEM(float vltcmd) = 0.0;
+
+MEM(float vel_error_sum) = 0.0;
+MEM(float cur_cmd_old) = 0.0;
+MEM(float sat) = 0.0;
+
+RT_PID(
+  float poserr = PIN(pos_error);
+
+  float velextcmd = PIN(vel_ext_cmd);
+  float velfb = PIN(vel_fb);
+  float velerr;
+
+  float accextcmd = PIN(acc_ext_cmd);
+
+  float forcecmd;
+  float forceextcmd = PIN(force_ext_cmd);
+
+  float curcmd;
+  float curfb = PIN(cur_fb);
+  float curerr;
+
+  float pwmcmd;
+
+
+  float en = PIN(enable);
+
+  float posen = PIN(pos_en);
+  float posp = PIN(pos_p);
+  float poslp = PIN(pos_lp);
+
+  float velen = PIN(vel_en);
+  float velp = PIN(vel_p);
+  float veli = PIN(vel_i);
+  float velff = PIN(vel_ff);
+  float vellp = PIN(vel_lp);
+
+  float accp = PIN(acc_p);
+  float accff = PIN(acc_ff);
+
+
+  float forcep = PIN(force_p);
+
+  float curp = PIN(cur_p);
+  float curd = PIN(cur_d);
+  float curind = PIN(cur_ind);
+  float curlp = PIN(cur_lp);
+
+  float vlt = PIN(volt);
+
+  float velmax = PIN(vel_max);
+  float velerrsummax = PIN(vel_error_sum_max);
+  float accmax = PIN(acc_max);
+  float forcemax = PIN(force_max);
+  float curmax = PIN(cur_max);
+  float vltmax = PIN(volt_max);
+  float pwmmax = PIN(pwm_max);
+
+
+  if(en > 0.0){
+    // pos -> vel
+    if(posen == 0.0){
+      posp = 0.0;
+    }
+    velcmd = LIMIT(posp * poserr + velff * velextcmd, velmax) * poslp + velcmd * (1 - poslp);
+
+    // vel -> acc
+    velerr = velcmd - velfb;
+    if(velen > 0.0){
+      velp = 0.0;
+      veli = 0.0;
+    }
+    if(veli > 0.0){
+      vel_error_sum = LIMIT(vel_error_sum + velerr * period, velerrsummax);
+    }
+    else{
+      vel_error_sum = 0.0;
+    }
+    acccmd = LIMIT(velp * velerr + veli * vel_error_sum + accff * accextcmd, accmax) * vellp + acccmd * (1 - vellp);
+
+    // acc -> force
+    forcecmd = LIMIT(accp * acccmd + forceextcmd, forcemax);
+
+    // force -> current
+    curcmd = LIMIT(forcep * forcecmd, curmax);
+
+    // current -> volt
+    curerr = curcmd - curfb;
+    vltcmd = LIMIT(curp * curerr + curd * (curcmd - cur_cmd_old) / period + curind * velfb, vltmax)  * curlp + vltcmd * (1 - curlp);
+    cur_cmd_old = curcmd;
+
+    // volt -> pwm
+    if(vlt == 0.0){
+      pwmcmd = 0.0;
+    }
+    else{
+      pwmcmd = vltcmd / vlt;
+    }
+
+    if(pwmcmd >= pwmmax || pwmcmd <= -pwmmax){
+      sat += period;
+    }
+    else{
+      sat = 0.0;
+    }
+    pwmcmd = LIMIT(pwmcmd, pwmmax);
+  }
+  else{
+    velcmd = 0.0;
+    velerr = 0.0;
+    acccmd = 0.0;
+    forcecmd = 0.0;
+    curcmd = 0.0;
+    velerr = 0.0;
+    vltcmd = 0.0;
+    pwmcmd = 0.0;
+
+    vel_error_sum = 0.0;
+
+    cur_cmd_old = 0.0;
+
+    sat = 0.0;
+  }
+
+  PIN(vel_cmd) = velcmd;
+  PIN(vel_error) = velerr;
+  PIN(acc_cmd) = acccmd;
+  PIN(force_cmd) = forcecmd;
+  PIN(cur_cmd) = curcmd;
+  PIN(cur_error) = curerr;
+  PIN(volt_cmd) = vltcmd;
+  PIN(pwm_cmd) = pwmcmd;
+  PIN(saturated) = sat;
+);
+
+ENDCOMP;

src/pos_minus.comp

@@ -0,0 +1,11 @@
+COMP(pos_minus);
+
+HAL_PIN(in0) = 0.0;
+HAL_PIN(in1) = 0.0;
+HAL_PIN(out) = 0.0;
+
+RC(out,
+  PIN(out) = minus(PIN(in0), PIN(in1));
+);
+
+ENDCOMP;

src/term.comp

@@ -0,0 +1,91 @@
+COMP(term);
+
+HAL_PIN(wave0) = 0.0;
+HAL_PIN(gain0) = 0.0;
+HAL_PIN(offset0) = 0.0;
+
+HAL_PIN(wave1) = 0.0;
+HAL_PIN(gain1) = 0.0;
+HAL_PIN(offset1) = 0.0;
+
+HAL_PIN(wave2) = 0.0;
+HAL_PIN(gain2) = 0.0;
+HAL_PIN(offset2) = 0.0;
+
+HAL_PIN(wave3) = 0.0;
+HAL_PIN(gain3) = 0.0;
+HAL_PIN(offset3) = 0.0;
+
+NRT(
+  float e = 0.0;
+  unsigned char buf[MAX_WAVE + 2];
+
+  e = PIN(wave0);
+  e = (e + PIN(offset0)) * PIN(gain0);
+  e = CLAMP(e + 128,1,255);
+  buf[1] = (int)e;
+
+  e = PIN(wave1);
+  e = (e + PIN(offset1)) * PIN(gain1);
+  e = CLAMP(e + 128,1,255);
+  buf[2] = (int)e;
+
+  e = PIN(wave2);
+  e = (e + PIN(offset2)) * PIN(gain2);
+  e = CLAMP(e + 128,1,255);
+  buf[3] = (int)e;
+
+  e = PIN(wave3);
+  e = (e + PIN(offset3)) * PIN(gain3);
+  e = CLAMP(e + 128,1,255);
+  buf[4] = (int)e;
+
+  buf[0] = 255;
+  buf[MAX_WAVE + 1] = 0;
+
+
+  #ifdef USBTERM
+    if(UB_USB_CDC_GetStatus()==USB_CDC_CONNECTED){
+      UB_USB_CDC_SendString((char*)buf, NONE);//schleppfehler senden
+
+      char source[APP_TX_BUF_SIZE];
+      char sink[APP_TX_BUF_SIZE];
+
+      int i = scanf_("%N = %N",sink ,source);
+
+      if(i == 2){ // read
+        if(is_hal_pin(sink)){
+          printf_("%s <= %s = %f\n", sink, find_hal_pin(sink)->source->name, get_hal_pin(sink));
+        }
+        else{
+          printf_("not found\n");
+        }
+      }
+      else if(i == 5){
+        if(is_hal_pin(source) && is_hal_pin(sink)){
+          link_hal_pins(source, sink);
+          printf_("OK %s <= %s = %f\n", sink, source, get_hal_pin(sink));
+        }
+        else if(is_hal_pin(sink)){
+          set_hal_pin(sink, read_float(source));
+          printf_("OK %s = %f\n", sink, get_hal_pin(sink));
+        }
+        else{
+          printf_("not found\n");
+        }
+      }
+      else if(i == -1){
+
+      }
+      else{
+        for(int i = 0; i < hal.hal_pin_count; i++){
+          printf_("%s <= %s = %f\n", hal.hal_pins[i]->name, hal.hal_pins[i]->source->name, hal.hal_pins[i]->source->value);
+          Wait(1);
+        }
+      }
+    }
+  #endif
+
+);
+
+ENDCOMP;