diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h index 6175f6d4e5bd87f73dbb2f67322dc645270eb903..66c9016f30a296f38f0bca3f50ba87a9557e9eb2 100644 --- a/controllers/include/cm510_cfg.h +++ b/controllers/include/cm510_cfg.h @@ -26,8 +26,8 @@ #define MANAGER_MISSING_SERVOS_ALARM_TIME_ON 10 #define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF 100 #define BALANCE_GYRO_CAL_NUM_SAMPLES 10 -#define BALANCE_GYRO_X_CHANNEL 4 -#define BALANCE_GYRO_Y_CHANNEL 3 +#define BALANCE_GYRO_X_CHANNEL 3 +#define BALANCE_GYRO_Y_CHANNEL 4 #define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE NOTE_SOL #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON 10 #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF 100 diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h index a4d211884c84f21573f590a2c2cc1650812af696..07e1c4a9327e00a0149590db7b1622da04a732d6 100644 --- a/motion/include/motion_cfg.h +++ b/motion/include/motion_cfg.h @@ -31,6 +31,10 @@ #define BALANCE_GYRO_Y_CHANNEL 4 #endif +#ifndef BALANCE_GYRO_Y_FWD + #define BALANCE_GYRO_Y_BWD +#endif + #ifndef BALANCE_GYRO_CAL_FAILED_ALARM_NOTE #define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE NOTE_SOL #endif diff --git a/motion/src/balance.c b/motion/src/balance.c index e5b84dda8125c9287d8301d0ae6948c009eedd04..b1d757ed6b975a304351a16d591352973048cbe8 100644 --- a/motion/src/balance.c +++ b/motion/src/balance.c @@ -37,6 +37,11 @@ void balance_loop(void) gyro_x=get_adc_channel(BALANCE_GYRO_X_CHANNEL)-balance_x_gyro_center; gyro_y=get_adc_channel(BALANCE_GYRO_Y_CHANNEL)-balance_y_gyro_center; + #ifdef BALANCE_GYRO_Y_BWD + gyro_x=-gyro_x; + gyro_y=-gyro_y; + #endif + if(gyro_x < BALANCE_BACKWARD_FALL_THD_VALUE) { balance_robot_fallen_state = robot_face_up;