Merge branch 'withlivecode' into odometry

common/mb_controller.c

@@ -14,14 +14,19 @@ int mb_initialize_controller(){
     //mb_load_controller_config();
     left_wheel_velocity_pid = rc_filter_empty();
     right_wheel_velocity_pid = rc_filter_empty();
+    left_wheel_low_pass = rc_filter_empty();
+    right_wheel_low_pass = rc_filter_empty();
     //put variable for left pid and right pid
-    float kp = 1.0;
-    float ki = 0.0;
-    float kd = 0.0;
+    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);
     rc_filter_enable_saturation(&left_wheel_velocity_pid,-1.0,1.0);
     rc_filter_enable_saturation(&right_wheel_velocity_pid,-1.0,1.0);
+    
+    rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.1);
+    rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.1);
     return 0;
 }
 
@@ -70,11 +75,15 @@ 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;
+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_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_controller.h

@@ -18,6 +18,8 @@ int mb_destroy_controller();
 *************/
 rc_filter_t left_wheel_velocity_pid;
 rc_filter_t right_wheel_velocity_pid;
+rc_filter_t left_wheel_low_pass;
+rc_filter_t right_wheel_low_pass;
 /***********
 * For each PID filter you want to load from settings
 * add a pid_parameter_t or filter_parameter_t

common/mb_defs.h

@@ -23,10 +23,17 @@
 #define LEFT_ENC_POL 1
 #define RIGHT_ENC_POL -1
 // TODO: Fill in physical propeties of robot
+<<<<<<< HEAD
 #define GEAR_RATIO              78.0  // gear ratio of motor
 #define ENCODER_RES             20.0  // encoder counts per motor shaft revolution
 #define WHEEL_DIAMETER          0.084 // diameter of wheel in meters
 #define WHEEL_BASE              0.158  // wheel separation distance 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
+>>>>>>> withlivecode
 #define MAX_FWD_VEL             0.8   // maximum forwad speed (m/s)
 #define MAX_TURN_VEL            2.5   // maximum turning speed (rad/s)
 
@@ -49,6 +56,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

measure_motor_params/measure_motors.c

@@ -20,11 +20,11 @@
 #include <rc/motor.h>
 #include "../common/mb_motor.h"
 #include "../common/mb_defs.h"
-
+#include "../common/mb_structs.h"
 
 float enc2meters = (WHEEL_DIAMETER * M_PI) / (GEAR_RATIO * ENCODER_RES);
 
-void test_speed(float du, float dtime_s);
+void test_speed(float duty, float dtime_s,mb_state_t* mb);
 
 /*******************************************************************************
 * int main() 
@@ -59,11 +59,10 @@ int main(){
     }
 #endif
 
-    if(rc_encoder_eqep_init()<0){
+    if(rc_encoder_init()<0){
         fprintf(stderr,"ERROR: failed to initialze encoders\n");
         return -1;
     }
-    
     // make PID file to indicate your project is running
 	// due to the check made on the call to rc_kill_existing_process() above
 	// we can be fairly confident there is no PID file already and we can
@@ -75,15 +74,43 @@ int main(){
 
 	if(rc_get_state()==RUNNING){
 		rc_nanosleep(1E9); //sleep for 1s
-        //TODO: write routine here
 	}
-	
-	// TODO: Plase exit routine here
+    rc_encoder_eqep_write(1,0);
+    rc_encoder_eqep_write(2,0);
+    mb_state_t mb;
+    mb.left_encoder_delta = 0;
+    mb.right_encoder_delta = 0;
 
+    //set up file for output
+    FILE *fpL;
+    FILE *fpR;
+    fpL = fopen("left_speed.txt", "w+");
+    fpR = fopen("right_speed.txt", "w+");
+    for (float pwm = 0.0; pwm <=1.0; pwm += 0.05){
+       rc_encoder_eqep_write(1,0);
+       rc_encoder_eqep_write(2,0);
+       test_speed(pwm,1E9,&mb);
+       fprintf(fpL,"%f\t%f\n",pwm,mb.left_velocity);
+       fprintf(fpR,"%f\t%f\n",pwm,mb.right_velocity);
+    }
+	// TODO: Plase exit routine here
+    fclose(fpL);
+    fclose(fpR);
+    rc_encoder_cleanup();
+    rc_motor_cleanup();
+    rc_set_state(EXITING);
     // remove pid file LAST
 	rc_remove_pid_file();   
 	return 0;
 }
-
-void test_speed(float duty, float dtime_s){
-}
\ No newline at end of file
+void test_speed(float duty, float dtime_s,mb_state_t* mb){
+    rc_motor_set(1,duty);
+    rc_motor_set(2,-duty);
+    rc_nanosleep(2E8);
+    
+    rc_nanosleep(dtime_s);
+    mb->left_encoder_delta= rc_encoder_eqep_read(1);
+    mb->right_encoder_delta= rc_encoder_eqep_read(2);
+    mb->left_velocity = mb->left_encoder_delta*1.0 * enc2meters / (dtime_s/1E9);
+    mb->right_velocity = mb->right_encoder_delta*1.0 * enc2meters / (dtime_s/1E9);
+}

mobilebot/mobilebot.c

@@ -195,10 +195,11 @@ void publish_mb_msgs(){
     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;
+    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;
+    
 
     //TODO: Create Odometry LCM message
     odo_msg.utime = now;
@@ -283,6 +284,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;
+	
 
 }
 

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"

python/plot_step.py

@@ -33,6 +33,7 @@ for event in log:
 cmd = data[:,3]
 start = np.where(cmd>=0.1)[0][0]-5
 end = start + 500 #plot 10s worth of data
+plt.figure(0)
 p1 = plt.plot(data[start:end,0], data[start:end,3], 'g', label="vel cmd")
 p2 = plt.plot(data[start:end,0], data[start:end,1], 'r', label="pwm cmd")
 p3 = plt.plot(data[start:end,0], data[start:end,5], 'b', label="velocity")
@@ -40,3 +41,8 @@ plt.legend(loc="upper right")
 filename, file_extension = os.path.splitext(file)
 plt.savefig(filename + '.png')
 plt.show()
+plt.figure(1)
+p1 = plt.plot(data[start:end,0], data[start:end,4], 'g', label="vel cmd")
+p2 = plt.plot(data[start:end,0], data[start:end,2], 'r', label="pwm cmd")
+p3 = plt.plot(data[start:end,0], data[start:end,6], 'b', label="velocity")
+plt.show()