add live code result

common/mb_controller.c

@@ -11,7 +11,17 @@
 *******************************************************************************/
 
 int mb_initialize_controller(){
-    mb_load_controller_config();
+    //mb_load_controller_config();
+    left_wheel_velocity_pid = rc_filter_empty();
+    right_wheel_velocity_pid = rc_filter_empty();
+    //put variable for left pid and right pid
+    float kp = 1.0;
+    float ki = 0.0;
+    float kd = 0.0;
+    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);
+    rc_filter_enable_saturation(&left_wheel_velocity_pid,-1.0,1.0);
+    rc_filter_enable_saturation(&right_wheel_velocity_pid,-1.0,1.0);
     return 0;
 }
 
@@ -61,6 +71,10 @@ int mb_load_controller_config(){
 *******************************************************************************/
 
 int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){  
+    float left_error = mb_setpoints->fwd_velocity - mb_state->left_velocity;
+    float right_error = mb_setpoints->fwd_velocity - 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);
     return 0;
 }
 
@@ -75,5 +89,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
 *******************************************************************************/
 
 int mb_destroy_controller(){
+    rc_filter_free(&left_wheel_velocity_pid);
+    rc_filter_free(&right_wheel_velocity_pid);
     return 0;
 }

common/mb_controller.h

@@ -16,7 +16,8 @@ int mb_destroy_controller();
 * rc_filter_t left_wheel_speed_pid;
 * rc_filter_t fwd_vel_sp_lpf;
 *************/
-
+rc_filter_t left_wheel_velocity_pid;
+rc_filter_t right_wheel_velocity_pid;
 /***********
 * For each PID filter you want to load from settings
 * add a pid_parameter_t or filter_parameter_t

common/mb_defs.h

@@ -18,7 +18,10 @@
 #define RIGHT_MOTOR             2     // id of right motor
 
 // 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
 // TODO: Fill in physical propeties of robot
 #define GEAR_RATIO              0.0  // gear ratio of motor
 #define ENCODER_RES             0.0  // encoder counts per motor shaft revolution

common/mb_odometry.c

@@ -19,6 +19,7 @@
 *
 *******************************************************************************/
 void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float theta){
+
 }
 
 

mobilebot/mobilebot.c

@@ -151,7 +151,9 @@ void read_mb_sensors(){
     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.right_velocity = enc2meters * mb_state.right_encoder_delta / DT;
     //unlock state mutex
     pthread_mutex_unlock(&state_mutex);
 
@@ -166,6 +168,7 @@ void read_mb_sensors(){
 void publish_mb_msgs(){
     mbot_imu_t imu_msg;
     mbot_encoder_t encoder_msg;
+    mbot_wheel_ctrl_t wheel_ctrl_msg; 
 //  odometry_t odo_msg;
 
     //Create IMU LCM Message
@@ -185,11 +188,20 @@ void publish_mb_msgs(){
     encoder_msg.leftticks = mb_state.left_encoder_total;
     encoder_msg.rightticks = mb_state.right_encoder_total;
 
+    wheel_ctrl_msg.utime = now;
+    wheel_ctrl_msg.left_motor_pwm_cmd = mb_state.left_cmd;
+    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 = mb_state.left_velocity;
+    wheel_ctrl_msg.right_motor_vel = mb_state.right_velocity;
+
     //TODO: Create Odometry LCM message
 
     //publish IMU & Encoder Data to LCM
     mbot_imu_t_publish(lcm, MBOT_IMU_CHANNEL, &imu_msg);
     mbot_encoder_t_publish(lcm, MBOT_ENCODER_CHANNEL, &encoder_msg);
+    mbot_wheel_ctrl_t_publish(lcm,"MBOT_WHEEL_CTRL",&wheel_ctrl_msg);
 //  odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
 }
 
@@ -209,7 +221,10 @@ void publish_mb_msgs(){
 void mobilebot_controller(){
     update_now();
     read_mb_sensors();
+    mb_controller_update(&mb_state,&mb_setpoints);
     publish_mb_msgs();
+    rc_motor_set(LEFT_MOTOR,LEFT_MOTOR_POL*mb_state.left_cmd);
+    rc_motor_set(RIGHT_MOTOR,RIGHT_MOTOR_POL*mb_state.right_cmd);
 
 }