From 44fc71217f0e262a501a13295932380bf0d72aee Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 27 Jul 2017 08:54:02 +0200
Subject: [PATCH] Added a new configuration parameter to let the software know
the actual orientation of the gyroscope sensor. Needed to properly balance
the robot.
---
controllers/include/cm510_cfg.h | 4 ++--
motion/include/motion_cfg.h | 4 ++++
motion/src/balance.c | 5 +++++
3 files changed, 11 insertions(+), 2 deletions(-)
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index 6175f6d..66c9016 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 a4d2118..07e1c4a 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 e5b84dd..b1d757e 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;
--
GitLab