added professor code

common/mb_controller.c

@@ -75,6 +75,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
     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;
 }
 

common/mb_defs.h

@@ -18,14 +18,14 @@
 #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
+#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
-#define WHEEL_DIAMETER          0.0 // diameter of wheel in meters
+#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 MAX_FWD_VEL             0.8   // maximum forwad speed (m/s)
 #define MAX_TURN_VEL            2.5   // maximum turning speed (rad/s)
@@ -49,6 +49,7 @@
 #define MBOT_ENCODER_CHANNEL        "MBOT_ENCODERS"
 #define MBOT_MOTOR_COMMAND_CHANNEL  "MBOT_MOTOR_COMMAND"
 #define MBOT_TIMESYNC_CHANNEL       "MBOT_TIMESYNC"
+#define MBOT_WHEEL_CTRL_CHANNEL     "MBOT_CTRL_COMMAND"
 #define LCM_ADDRESS                 "udpm://239.255.76.67:7667?ttl=1"
 
 #define LED_OFF                 1

mobilebot/mobilebot.c

@@ -195,13 +195,14 @@ void publish_mb_msgs(){
     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);
+    mbot_wheel_ctrl_t_publish(lcm,"MBOT_CTRL_COMMAND",&wheel_ctrl_msg);
 //  odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
 }
 
@@ -265,6 +266,7 @@ void motor_command_handler(const lcm_recv_buf_t *rbuf, const char *channel,
                           const mbot_motor_command_t *msg, void *user){
 	mb_setpoints.fwd_velocity = msg->trans_v;
 	mb_setpoints.turn_velocity = msg->angular_v;
+	printf("%f,%f\n",mb_setpoints.fwd_velocity,mb_setpoints.turn_velocity);
 
 }
 

mobilebot/mobilebot.h

@@ -36,6 +36,7 @@
 #include "../lcmtypes/oled_message_t.h"
 #include "../lcmtypes/timestamp_t.h"
 #include "../lcmtypes/reset_odometry_t.h"
+#include "../lcmtypes/mbot_wheel_ctrl_t.h"
 
 #include "../common/mb_defs.h"
 #include "../common/mb_structs.h"