Merge branch 'master' of github.com:rene-dev/stmbl

Makefile

@@ -1,6 +1,6 @@
 # put your *.o targets here, make should handle the rest!
 
-SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c printf.c scanf.c param.c setup.c stm32_ub_encoder_tim3.c pid.c input.c
+SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c printf.c scanf.c param.c setup.c stm32_ub_encoder_tim3.c pid.c hal.c
 #USB
 SRCS +=  ub_lib/stm32_ub_usb_cdc.c ub_lib/usb_cdc_lolevel/usb_core.c ub_lib/usb_cdc_lolevel/usb_dcd_int.c ub_lib/usb_cdc_lolevel/usbd_req.c ub_lib/usb_cdc_lolevel/usbd_cdc_core.c ub_lib/usb_cdc_lolevel/usbd_core.c ub_lib/usb_cdc_lolevel/usb_dcd.c ub_lib/usb_cdc_lolevel/usbd_cdc_vcp.c ub_lib/usb_cdc_lolevel/usbd_desc.c ub_lib/usb_cdc_lolevel/usbd_ioreq.c ub_lib/usb_cdc_lolevel/usb_bsp.c ub_lib/usb_cdc_lolevel/usbd_usr.c
 #SRCS = main.c system.c

src/hal.c

@@ -0,0 +1,119 @@
+#include "hal.h"
+
+void init_hal(){
+  hal.hal_pin_count = 0;
+  hal.fast_rt_lock = 0;
+  hal.rt_lock = 0;
+  hal.nrt_lock = 0;
+}
+
+void init_hal_pin(HPNAME name, struct hal_pin* pin, float value){
+  strncpy(pin->name, name, MAX_HPNAME);
+  pin->value = value;
+  pin->source = pin;
+  register_hal_pin(pin);
+}
+
+int register_hal_pin(struct hal_pin* pin){
+  if(hal.hal_pin_count >= MAX_HAL_PINS){
+    return(0);
+  }
+
+  for(int i = 0; i < hal.hal_pin_count; i++){
+    if(!strcmp(hal.hal_pins[i]->name, pin->name)){
+      return(0);
+    }
+  }
+
+  hal.hal_pins[hal.hal_pin_count] = pin;
+  hal.hal_pin_count++;
+
+  return(1);
+}
+
+int set_hal_pin(HPNAME name, float value){
+  for(int i = 0; i < hal.hal_pin_count; i++){
+    if(!strcmp(hal.hal_pins[i]->name, name)){
+      hal.hal_pins[i]->value = value;
+      return(1);
+    }
+  }
+  return(0);
+}
+
+int is_hal_pin(HPNAME name){
+  for(int i = 0; i < hal.hal_pin_count; i++){
+    if(!strcmp(hal.hal_pins[i]->name, name)){
+      return(1);
+    }
+  }
+  return(0);
+}
+
+float get_hal_pin(HPNAME name){
+  for(int i = 0; i < hal.hal_pin_count; i++){
+    if(!strcmp(hal.hal_pins[i]->name, name)){
+      return(hal.hal_pins[i]->source->value);
+    }
+  }
+  return(0.0);
+}
+
+void write_hal_pin(struct hal_pin* pin, float value){
+  pin->value = value;
+}
+
+float read_hal_pin(struct hal_pin* pin){
+  return(pin->source->value);
+}
+
+struct hal_pin* find_hal_pin(HPNAME name){
+  for(int i = 0; i < hal.hal_pin_count; i++){
+    if(!strcmp(hal.hal_pins[i]->name, name)){
+      return(hal.hal_pins[i]);
+    }
+  }
+  return(0);
+}
+
+int link_hal_pins(HPNAME source, HPNAME sink){
+  struct hal_pin* d;
+  struct hal_pin* s;
+  d = find_hal_pin(source);
+  s = find_hal_pin(sink);
+
+  if(d != 0 && s != 0){
+    s->value = s->source->value;
+    s->source = d->source;
+    return(1);
+  }
+  return(0);
+}
+
+float read_float(char* buf){
+  int buffer_pos = 0;
+  float f = 0;
+  float tf = 10;
+  float invert = 1;
+
+  if(buf[buffer_pos] && buf[buffer_pos] == '-'){
+    invert = -1;
+    buffer_pos++;
+  }
+  else if(buf[buffer_pos] && buf[buffer_pos] == '+'){
+    buffer_pos++;
+  }
+  while(buf[buffer_pos] && isDecDigit(buf[buffer_pos])){
+    f *= 10;
+    f += buf[buffer_pos++] - '0';
+  }
+  if(buf[buffer_pos] && buf[buffer_pos] == '.'){
+    buffer_pos++;
+    while(buf[buffer_pos] && isDecDigit(buf[buffer_pos])){
+      f += (buf[buffer_pos++] - '0') / tf;
+      tf *= 10;
+    }
+  }
+  f *= invert;
+  return(f);
+}

src/hal.h

@@ -0,0 +1,46 @@
+#include "param.h"
+#include "scanf.h"
+
+#pragma once
+
+#define MAX_HAL_PINS 128
+#define MAX_HPNAME 16
+
+typedef char HPNAME[MAX_HPNAME];
+
+struct hal_pin{
+  HPNAME name;
+  float value;
+  struct hal_pin* source;
+};
+
+struct hal_struct{
+  struct hal_pin* hal_pins[MAX_HAL_PINS];
+  int hal_pin_count;
+
+  int fast_rt_lock;
+  int rt_lock;
+  int nrt_lock;
+} hal;
+
+void init_hal();
+
+void init_hal_pin(HPNAME name, struct hal_pin* pin, float value);
+
+int register_hal_pin(struct hal_pin* pin);
+
+int set_hal_pin(HPNAME name, float value);
+
+int is_hal_pin(HPNAME name);
+
+float get_hal_pin(HPNAME name);
+
+void write_hal_pin(struct hal_pin* pin, float value);
+
+float read_hal_pin(struct hal_pin* pin);
+
+struct hal_pin* find_hal_pin(HPNAME name);
+
+int link_hal_pins(HPNAME source, HPNAME sink);
+
+float read_float(char* buf);

src/param.c

@@ -21,11 +21,7 @@ int strcmp(const char* s1, const char* s2){
 }
 
 void strncpy(char* dst, char* src, int n){
-	for(int i = 0; i < n && *src; i++){
-		*dst = *src;
-		dst++;
-		src++;
-	}
+  while (n-- && (*dst++ = *src++));
 }
 
 void param_init(){

src/param.h

@@ -6,6 +6,8 @@
 //  Copyright (c) 2013 Rene Hopf. All rights reserved.
 //
 
+#pragma once
+
 #define MAX_PARAMS 64
 #define MAX_PNAME 8
 typedef char PNAME[MAX_PNAME];

src/pid.c

@@ -236,8 +236,141 @@ void calc_pid(hal_pid_t *arg, float period)
             (pid->saturated_count) ++;
     } else {
         pid->saturated = 0;
-        pid->saturated_s = 0;
+        pid->saturated = 0;
         pid->saturated_count = 0;
     }
     /* done */
 }
+
+
+
+void pid2(pid2p* pid){
+	if(pid->enable > 0.5){
+		// pos -> vel
+		pid->pos_error = minus(pid->ext_cmd_pos, pid->feedback_pos);
+		pid->cmd_vel = LIMIT(pid->pos_p * pid->pos_error + pid->ff1 * pid->ext_cmd_vel, pid->max_vel);
+
+		// vel -> acc
+		pid->vel_error = pid->cmd_vel - pid->feedback_vel;
+		if(pid->vel_i > 0.0){
+			pid->vel_error_sum = LIMIT(pid->vel_error_sum + pid->vel_error * pid->period, pid->max_vel_error_sum);
+		}
+		else{
+			pid->vel_error_sum = 0.0;
+		}
+		pid->cmd_acc = LIMIT(pid->vel_p * pid->vel_error + pid->vel_i * pid->vel_error_sum + pid->ff2 * pid->ext_cmd_acc, pid->max_acc);
+
+		// acc -> force
+		pid->cmd_force = LIMIT(pid->acc_p * pid->cmd_acc + pid->ext_cmd_force, pid->max_force);
+
+		// force -> current
+		pid->cmd_cur = LIMIT(pid->force_p * pid->cmd_force, pid->max_cur);
+
+		// current -> volt
+		pid->cur_error = pid->cmd_cur - pid->feedback_cur;
+		pid->cmd_volt = LIMIT(pid->cur_p * pid->cur_error + pid->cur_d * (pid->cmd_cur - pid->cmd_cur_old) / pid->period + pid->ind_p * pid->feedback_vel, pid->max_volt);
+		pid->cmd_cur_old = pid->cmd_cur;
+
+		// volt -> pwm
+		pid->cmd_pwm = pid->cmd_volt / pid->volt;
+		if(pid->cmd_pwm >= pid->max_pwm || pid->cmd_pwm <= -pid->max_pwm){
+			pid->saturated_s += pid->period;
+		}
+		else{
+			pid->saturated_s = 0.0;
+		}
+		pid->cmd_pwm = LIMIT(pid->cmd_pwm, pid->max_pwm);
+	}
+	else{
+		pid->cmd_vel = 0.0;
+		pid->cmd_acc = 0.0;
+		pid->cmd_force = 0.0;
+		pid->cmd_cur = 0.0;
+		pid->cmd_volt = 0.0;
+		pid->cmd_vel = 0.0;
+
+		pid->vel_error_sum = 0.0;
+
+		pid->cmd_cur_old = 0.0;
+
+		pid->saturated_s = 0.0;
+	}
+}
+
+
+void pid2_init(pid2p* pid){
+	pid->ext_cmd_pos = 0.0;
+	pid->feedback_pos = 0.0;
+
+	pid->cmd_vel = 0.0;
+	pid->ext_cmd_vel = 0.0;
+	pid->feedback_vel = 0.0;
+
+	pid->cmd_acc = 0.0;
+	pid->ext_cmd_acc = 0.0;
+	//pid->feedback_acc = 0.0;
+
+	pid->cmd_force = 0.0;
+	pid->ext_cmd_force = 0.0;
+	//pid->feedback_force = 0.0;
+
+	pid->cmd_cur = 0.0;
+	pid->feedback_cur = 0.0;
+
+	pid->cmd_volt = 0.0;
+
+	pid->cmd_pwm = 0.0;
+
+
+	pid->pos_error = 0.0;
+	pid->vel_error = 0.0;
+	pid->cur_error = 0.0;
+
+	pid->enable = 1.0;
+
+	pid->pos_p = 30.0;
+	pid->ff1 = 0.95;
+
+	pid->vel_p = 1.0;
+	pid->vel_i = 40.0;
+	pid->ff2 = 0.002;
+
+	pid->acc_p = 0.1;
+
+	pid->force_p = 3.667;
+
+	pid->cur_p = 15.0;
+	pid->cur_d = 0.01;
+	pid->ind_p = 0.57;
+
+	pid->volt = 130.0;
+
+	pid->period = 0.001;
+
+
+	pid->max_vel = 62.9;
+	pid->max_vel_error_sum = 2.5;
+
+	pid->max_acc = 1200;
+
+	pid->max_force = 100.0;
+
+	pid->max_cur = 6.0;
+
+	pid->max_volt = 130.0;
+
+	pid->max_pwm = 0.9;
+
+	pid->i0 = 1.0;
+
+
+	pid->vel_error_sum = 0.0;
+
+	pid->cmd_cur_old = 0.0;
+
+	pid->saturated_s = 0.0;
+
+	pid->i2t = 0.0;
+
+	pid->minus = minus;
+}

src/pid.h

@@ -7,6 +7,7 @@ extern "C" {
 
 #define ABS(a)	   (((a) < 0) ? -(a) : (a))
 #define CLAMP(x, low, high)  (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
+#define LIMIT(x, lowhigh)  (((lowhigh) > 0.0) ? (((x) > (lowhigh)) ? (lowhigh) : (((x) < (-lowhigh)) ? (-lowhigh) : (x))) : (x))
 #define MIN(a, b)  (((a) < (b)) ? (a) : (b))
 #define MAX(a, b)  (((a) > (b)) ? (a) : (b))
 #define DEG(a) ((a)*M_PI/180.0)
@@ -58,6 +59,105 @@ typedef struct {
     //char prev_ie;
 } hal_pid_t;
 
+typedef struct {
+  // pos
+  float ext_cmd_pos;
+  float feedback_pos;
+
+  // vel
+  float cmd_vel;
+  float ext_cmd_vel;
+  float feedback_vel;
+
+  // acc
+  float cmd_acc;
+  float ext_cmd_acc;
+  //float feedback_acc;
+
+  // force
+  float cmd_force;
+  float ext_cmd_force;
+  //float feedback_force;
+
+  // current
+  float cmd_cur;
+  float feedback_cur;
+
+  // volt
+  float cmd_volt;
+
+  // pwm output
+  float cmd_pwm;
+
+
+  // error
+  float pos_error;
+  float vel_error;
+  float cur_error;
+
+
+  // parameters
+  float enable;
+
+  // pos -> vel
+  float pos_p;
+  float ff1;
+
+  // vel -> acc
+  float vel_p;
+  float vel_i;
+  float ff2;
+
+  // acc -> force
+  float acc_p;
+
+  // force -> current
+  float force_p;
+
+  // current -> volt
+  float cur_p;
+  float cur_d;
+  float ind_p;
+
+  // voltage -> pwm
+  float volt;
+
+  // time
+  float period;
+
+  // limits
+  float max_vel;
+  float max_vel_error_sum;
+
+  float max_acc;
+
+  float max_force;
+
+  float max_cur;
+
+  float max_volt;
+
+  float max_pwm;
+
+  float i0;
+
+
+  // state
+  float vel_error_sum;
+
+  float cmd_cur_old;
+
+  float saturated_s;
+
+  float i2t;
+
+  // operator
+  // pos_error = minus(cmd_pos, feedback_pos)
+  float (*minus)(float a, float b);
+}pid2p;
+
+void pid2(pid2p* pid);
+void pid2_init(pid2p* pid);
 
 void calc_pid(hal_pid_t *arg, float period);
 void pid_init(hal_pid_t *pid);

src/scanf.c

@@ -45,6 +45,10 @@ int isChar(char c){
 	return((c <= 'Z' && c >= 'A') || (c <= 'z' && c >= 'a') || isDecDigit(c));
 }
 
+int isNameChar(char c){
+	return(isChar(c) || c == '_' || c == '-' || c == '.');
+}
+
 int isDecDigit(char c){
 	return(c >= '0' && c <= '9');
 }
@@ -125,7 +129,20 @@ int vfsscanf_(const char *buf, const char *format, va_list arg){
 					case 's':
 						string_pos = 0;
 						c = va_arg(arg, char *);
-						while(isChar(buf[buffer_pos])){
+						//while(isChar(buf[buffer_pos])){
+						while(!isWhitespace(buf[buffer_pos])){
+							c[string_pos] = buf[buffer_pos++];
+							string_pos++;
+							c[string_pos] = '\0';
+							found++;
+						}
+						break;
+
+					case 'N':
+						string_pos = 0;
+						c = va_arg(arg, char *);
+						//while(isChar(buf[buffer_pos])){
+						while(isNameChar(buf[buffer_pos])){
 							c[string_pos] = buf[buffer_pos++];
 							string_pos++;
 							c[string_pos] = '\0';

src/setup.c

@@ -72,7 +72,8 @@ void setup(){
     NVIC_SetPriority(SysTick_IRQn, 14);
 
     pid_init(&pid);
-    
+    pid2_init(&pid2ps);
+
     #ifdef USBTERM
     UB_USB_CDC_Init();
     #endif

src/setup.h

@@ -55,6 +55,8 @@ void SysTick_Handler(void);
 
 volatile int time;
 hal_pid_t pid;
+pid2p pid2ps;
+
 volatile uint32_t ADC2_DMA_Buffer[ADC2d_ANZ];
 
 TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;