velocity controller 1.5

common/mb_controller.c

@@ -18,12 +18,12 @@ int mb_initialize_controller(){
     left_wheel_low_pass = rc_filter_empty();
     right_wheel_low_pass = rc_filter_empty();
     //put variable for left pid and right pid
-    float kpL = 4;
-    float kiL = 8;
-    float kdL = 0.05;
-     float kpR = 4;
-    float kiR = 8;
-    float kdR = 0.05;
+    float kpL = 3;
+    float kiL = 15;
+    float kdL = 0.15;
+     float kpR = 3.5;
+    float kiR = 15;
+    float kdR = 0.15;
     rc_filter_pid(&left_wheel_velocity_pid,kpL,kiL,kdL,0.1,DT);
     rc_filter_pid(&right_wheel_velocity_pid,kpR,kiR,kdR,0.1,DT);
     rc_filter_enable_saturation(&left_wheel_velocity_pid,-1.0,1.0);
@@ -31,8 +31,6 @@ int mb_initialize_controller(){
     
     rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.06);
     rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.06);
-    left_prev = 0;
-    right_prev = 0;
     return 0;
 }
 
@@ -104,10 +102,10 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
      if(*reset == 1)
     {
         *reset = 0;
-        rc_filter_reset(&right_wheel_velocity_pid);
-        rc_filter_reset(&left_wheel_velocity_pid);   
         rc_filter_reset(&left_wheel_low_pass);
         rc_filter_reset(&right_wheel_low_pass);  
+        rc_filter_reset(&right_wheel_velocity_pid);
+        rc_filter_reset(&left_wheel_velocity_pid);   
     }
    
 
@@ -127,5 +125,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
 int mb_destroy_controller(){
     rc_filter_free(&left_wheel_velocity_pid);
     rc_filter_free(&right_wheel_velocity_pid);
+    rc_filter_reset(&left_wheel_low_pass);
+    rc_filter_reset(&right_wheel_low_pass);  
     return 0;
 }

common/mb_defs.h

@@ -15,7 +15,7 @@
 #define BEAGLEBONE_BLUE
 #define DEFAULT_PWM_FREQ        25000 // period of motor drive pwm
 #define LEFT_MOTOR              1     // id of left motor
-#define RIGHT_MOTOR             2     // id of right motor
+#define RIGHT_MOTOR             3 // id of right motor
 
 // TODO: Add convienient defines to define things like motor and encoder polarity here
 #define LEFT_MOTOR_POL 1