modify gyrodometry

common/mb_controller.c

@@ -89,12 +89,8 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
     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);
-<<<<<<< HEAD
-   
-=======
    // 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 );
->>>>>>> origin/odometry
     return 0;
 }
 

common/mb_defs.h

@@ -19,21 +19,14 @@
 
 // TODO: Add convienient defines to define things like motor and encoder polarity here
 #define LEFT_MOTOR_POL 1
-#define RIGHT_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
-<<<<<<< HEAD
-#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.157  // wheel separation distance in meters
-=======
 #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
->>>>>>> origin/odometry
 #define MAX_FWD_VEL             0.8   // maximum forwad speed (m/s)
 #define MAX_TURN_VEL            2.5   // maximum turning speed (rad/s)
 

common/mb_odometry.c

@@ -39,13 +39,14 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
     //del_thresh Needs to be tuned
     //IF DON'T WANT GYRO, SET IT AS F!
     float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
-    float del_thesh = 0.125/DT; 
+    float del_thresh = 0.125/DT; 
+    printf("del_thresh", del_thresh);
     boolean Gyro = T;
 
     float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
     float del_right = mb_state->right_encoder_delta * 1.0 * enc2meters;
-    float del_s = (del_right - del_left) / 2;//float del_s = (del_left + del_right) / 2;
-    float del_theta = mb_clamp_radians((del_right + del_left) / WHEEL_BASE);
+    float del_s = (del_right + del_left) / 2;//float del_s = (del_left + del_right) / 2;
+    float del_theta = mb_clamp_radians((del_right - del_left) / WHEEL_BASE);
     float del_x = del_s * cos(mb_odometry->theta + del_theta/2);
     float del_y = del_s * sin(mb_odometry->theta + del_theta/2);
     mb_odometry->x += del_x;
@@ -57,7 +58,7 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
     float del_theta_gyro = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0) * DT);
     float del_go = del_theta_gyro - del_theta;
     if (Gyro == T){
-        if (abs(del_go) > del_thesh){mb_odometry->theta += del_theta_gyro * DT;}
+        if (abs(del_go) >= 0.0){mb_odometry->theta += del_theta_gyro * DT;}
         else{mb_odometry->theta += del_theta * DT;}
     }
     else{// this is after we update the del_x and y