controller updated

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,12 +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

@@ -18,10 +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
+#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              78  // gear ratio of motor
 #define ENCODER_RES             20 // encoder counts per motor shaft revolution

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){
-}
+void test_speed(float duty, float dtime_s,mb_state_t* mb){
+    rc_motor_set(1,duty);
+    rc_motor_set(2,-duty);
\ No newline at end of file
+    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

@@ -191,10 +191,10 @@ 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
@@ -202,7 +202,7 @@ void publish_mb_msgs(){
     //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_CTRL_COMMAND",&wheel_ctrl_msg);
+    mbot_wheel_ctrl_t_publish(lcm,"MBOT_WHEEL_CTRL",&wheel_ctrl_msg);
 //  odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
 }
 
@@ -266,7 +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);
+	
 
 }