added controller

common/mb_controller.c

@@ -17,11 +17,14 @@ 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 kp = 0.65;
-    float ki = 0.02;
-    float kd = 0.15;
-    rc_filter_pid(&left_wheel_velocity_pid,kp,ki,kd,0.1,DT);
-    rc_filter_pid(&right_wheel_velocity_pid,kp,ki,kd,0.1,DT);
+    float kpL = 2;
+    float kiL = 7;
+    float kdL = 0.2;
+     float kpR = 2;
+    float kiR = 7;
+    float kdR = 0.2;
+    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);
     rc_filter_enable_saturation(&right_wheel_velocity_pid,-1.0,1.0);
     
@@ -76,14 +79,15 @@ int mb_load_controller_config(){
 *******************************************************************************/
 
 int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
-    //mb_state->left_velocity = rc_filter_march(&left_wheel_low_pass,mb_state->left_velocity);
-    //mb_state->right_velocity = rc_filter_march(&right_wheel_low_pass, mb_state->right_velocity);
-    float left_error = mb_setpoints->fwd_velocity - LEFT_MOTOR_POL*mb_state->left_velocity;
-    float right_error = mb_setpoints->fwd_velocity -  RIGHT_MOTOR_POL*mb_state->right_velocity;
+    mb_state->left_velocity = rc_filter_march(&left_wheel_low_pass,mb_state->left_velocity);
+    mb_state->right_velocity = rc_filter_march(&right_wheel_low_pass, mb_state->right_velocity);
+    float left_vel = (2*mb_setpoints->fwd_velocity  - mb_setpoints->turn_velocity*WHEEL_BASE)/2;
+    float right_vel = (2*mb_setpoints->fwd_velocity + mb_setpoints->turn_velocity*WHEEL_BASE)/2;
+    float left_error = left_vel - mb_state->left_velocity;
+    float right_error = right_vel -  mb_state->right_velocity;
     mb_state->left_cmd = rc_filter_march(&left_wheel_velocity_pid, left_error);
     mb_state->right_cmd = rc_filter_march(&right_wheel_velocity_pid, right_error);
-    mb_state->left_cmd = rc_filter_march(&left_wheel_low_pass,mb_state->left_cmd);
-    mb_state->right_cmd = rc_filter_march(&right_wheel_low_pass, mb_state->right_cmd );
+   
     return 0;
 }
 

common/mb_defs.h

@@ -20,13 +20,13 @@
 // TODO: Add convienient defines to define things like motor and encoder polarity here
 #define LEFT_MOTOR_POL 1
 #define RIGHT_MOTOR_POL -1
-#define LEFT_ENC_POL 1
-#define RIGHT_ENC_POL -1
+#define LEFT_ENC_POL -1
+#define RIGHT_ENC_POL 1
 // TODO: Fill in physical propeties of robot
 #define GEAR_RATIO              78  // gear ratio of motor
 #define ENCODER_RES             20 // encoder counts per motor shaft revolution
 #define WHEEL_DIAMETER          0.08 // diameter of wheel in meters
-#define WHEEL_BASE              0.0  // wheel separation distance in meters
+#define WHEEL_BASE              0.157  // wheel separation distance in meters
 #define MAX_FWD_VEL             0.8   // maximum forwad speed (m/s)
 #define MAX_TURN_VEL            2.5   // maximum turning speed (rad/s)
 

mobilebot/mobilebot.c

@@ -145,14 +145,14 @@ void read_mb_sensors(){
     }
 
     // Read encoders    
-    mb_state.left_encoder_delta = rc_encoder_read(LEFT_MOTOR);
-    mb_state.right_encoder_delta = rc_encoder_read(RIGHT_MOTOR);
+    mb_state.left_encoder_delta =  LEFT_ENC_POL*rc_encoder_read(LEFT_MOTOR);
+    mb_state.right_encoder_delta = RIGHT_ENC_POL*rc_encoder_read(RIGHT_MOTOR);
     mb_state.left_encoder_total += mb_state.left_encoder_delta;
     mb_state.right_encoder_total += mb_state.right_encoder_delta;
     rc_encoder_write(LEFT_MOTOR,0);
     rc_encoder_write(RIGHT_MOTOR,0);
     float enc2meters = (WHEEL_DIAMETER*3.1415) / (GEAR_RATIO*ENCODER_RES);
-    mb_state.left_velocity = enc2meters * mb_state.left_encoder_delta / DT;
+    mb_state.left_velocity =enc2meters * mb_state.left_encoder_delta / DT;
     mb_state.right_velocity = enc2meters * mb_state.right_encoder_delta / DT;
     //unlock state mutex
     pthread_mutex_unlock(&state_mutex);
@@ -193,8 +193,8 @@ void publish_mb_msgs(){
     wheel_ctrl_msg.right_motor_pwm_cmd = mb_state.right_cmd;
     wheel_ctrl_msg.left_motor_vel_cmd =mb_setpoints.fwd_velocity;
     wheel_ctrl_msg.right_motor_vel_cmd =mb_setpoints.fwd_velocity;
-    wheel_ctrl_msg.left_motor_vel =  -1*mb_state.left_velocity;
-    wheel_ctrl_msg.right_motor_vel =  mb_state.right_velocity;
+    wheel_ctrl_msg.left_motor_vel =  LEFT_MOTOR_POL*mb_state.left_velocity;
+    wheel_ctrl_msg.right_motor_vel =  RIGHT_ENC_POL*mb_state.right_velocity;
     
 
     //TODO: Create Odometry LCM message