#define KI 0.051429
#define KD 0.000378
#define PID_T 0.01
-#define STEP_PER_M 3376.1 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:34
-#define WHEEL_DIST 0.252
+#define STEP_PER_M 4171.4 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:34, calculated wheel diameter: 0.12454m
+#define WHEEL_DIST 0.36923 // Real: 0.252
enum mode {
MOTOR_MANUAL,
break;
case 0x94: // Front Handicap backward
front_handicap_bwd = TWDR;
+ cmd_vel.bUpdate = 1;
TWI_ACK;
break;
case 0x95: // Aft Handicap forward
aft_handicap_fwd = TWDR;
+ cmd_vel.bUpdate = 1;
TWI_ACK;
break;
case 0xff: // bootloader
angle_new = angle.f + angle_diff;
if (angle_new > 2*M_PI) angle_new-=2*M_PI;
- else if (angle_new < 2*M_PI) angle_new+=2*M_PI;
+ else if (angle_new < -2*M_PI) angle_new+=2*M_PI;
translation = (diff_left_m + diff_right_m)/2.0;
pos_x_new = pos_x.f + cos(angle_new)*translation;
speed_wish_left*=STEP_PER_M;
speed_wish_right*=STEP_PER_M;
- if (speed1_wish > 0 && aft_handicap_fwd > 0) {
+ if (speed_wish_left > 0 && aft_handicap_fwd > 0) {
speed1_wish = speed_wish_left * (100-aft_handicap_fwd)/100.0;
} else {
speed1_wish = speed_wish_left;
}
- if (speed2_wish > 0 && front_handicap_bwd > 0) {
+ if (speed_wish_left < 0 && front_handicap_bwd > 0) {
speed2_wish = speed_wish_left * (100-front_handicap_bwd)/100.0;
} else {
speed2_wish = speed_wish_left;
}
- if (speed3_wish > 0 && front_handicap_bwd > 0) {
+ if (speed_wish_right < 0 && front_handicap_bwd > 0) {
speed3_wish = speed_wish_right * (100-front_handicap_bwd)/100.0;
} else {
speed3_wish = speed_wish_right;
}
- if (speed4_wish > 0 && aft_handicap_fwd > 0) {
+ if (speed_wish_right > 0 && aft_handicap_fwd > 0) {
speed4_wish = speed_wish_right * (100-aft_handicap_fwd)/100.0;
} else {
speed4_wish = speed_wish_right;