From c1ec19c00b63172139a190ae878ce2713a9b58c2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sat, 16 Apr 2016 21:10:18 +0200
Subject: [PATCH] Added a function to reset the fallen state. Swaped the values
of the fallen thresholds. Added an example to get up when the robot falls.
Removed the balance_init() function from the manager_init() function.
---
controllers/include/cm510_cfg.h | 4 +-
motion/include/balance.h | 1 +
motion/src/balance.c | 5 ++
motion/src/examples/Makefile | 2 +-
motion/src/examples/get_up.c | 100 ++++++++++++++++++++++++++++++++
motion/src/motion_manager.c | 2 -
6 files changed, 109 insertions(+), 5 deletions(-)
create mode 100644 motion/src/examples/get_up.c
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index cebc829..a83f031 100644
--- a/controllers/include/cm510_cfg.h
+++ b/controllers/include/cm510_cfg.h
@@ -31,8 +31,8 @@
#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON 10
#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF 100
#define BALANCE_MAX_CAL_GYRO_ERROR 20.0
-#define BALANCE_FORWARD_FALL_THD_VALUE (-200)
-#define BALANCE_BACKWARD_FALL_THD_VALUE 200
+#define BALANCE_FORWARD_FALL_THD_VALUE 200
+#define BALANCE_BACKWARD_FALL_THD_VALUE (-200)
#define BALANCE_KNEE_GAIN (2.0/54.0)
#define BALANCE_ANKLE_PITCH_GAIN (2.0/18.0)
#define BALANCE_ANKLE_ROLL_GAIN (2.0/20.0)
diff --git a/motion/include/balance.h b/motion/include/balance.h
index 05a44eb..0b9fdef 100644
--- a/motion/include/balance.h
+++ b/motion/include/balance.h
@@ -20,6 +20,7 @@ void balance_init(void);
* back to 0x00
*/
fallen_t balance_robot_has_fallen(void);
+void balance_reset_fall_state(void);
uint8_t balance_calibrate_gyro(void);
/**
* \brief Function to enable the internal gyros
diff --git a/motion/src/balance.c b/motion/src/balance.c
index b9e2e3a..9caf2a9 100644
--- a/motion/src/balance.c
+++ b/motion/src/balance.c
@@ -84,6 +84,11 @@ fallen_t balance_robot_has_fallen(void)
return state;
}
+void balance_reset_fall_state(void)
+{
+ balance_robot_fallen_state = robot_standing;
+}
+
uint8_t balance_calibrate_gyro(void)
{
uint16_t x_gyro_values[BALANCE_GYRO_CAL_NUM_SAMPLES];
diff --git a/motion/src/examples/Makefile b/motion/src/examples/Makefile
index 289a744..1cb7fe8 100644
--- a/motion/src/examples/Makefile
+++ b/motion/src/examples/Makefile
@@ -2,7 +2,7 @@ PROJECT=manager_ex
########################################################
# afegir tots els fitxers que s'han de compilar aquÃ
########################################################
-SOURCES=main.c
+SOURCES=get_up.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
diff --git a/motion/src/examples/get_up.c b/motion/src/examples/get_up.c
new file mode 100644
index 0000000..c44c72b
--- /dev/null
+++ b/motion/src/examples/get_up.c
@@ -0,0 +1,100 @@
+#include <util/delay.h>
+#include <stdio.h>
+#include "cm510.h"
+#include "balance.h"
+#include "exp_board.h"
+
+typedef enum {wait_start,check_fallen,get_up} main_states;
+typedef enum {stop_motion,wait_stop_motion,wait_getting_up,wait_stabilize} get_up_states;
+
+uint8_t get_up_process(fallen_t fall_state)
+{
+ static get_up_states state;
+ uint8_t done=0x00;
+
+ switch(state)
+ {
+ case stop_motion: if(is_action_running())
+ {
+ action_stop_page();
+ state=wait_stop_motion;
+ }
+ else
+ {
+ if(fall_state==robot_face_down)
+ action_set_page(27);
+ else
+ action_set_page(28);
+ action_start_page();
+ state=wait_getting_up;
+ }
+ break;
+ case wait_stop_motion: if(!is_action_running())
+ {
+ if(fall_state==robot_face_down)
+ action_set_page(27);
+ else
+ action_set_page(28);
+ action_start_page();
+ state=wait_getting_up;
+ }
+ else
+ state= wait_stop_motion;
+ break;
+ case wait_getting_up: if(!is_action_running())
+ {
+ state=wait_stabilize;
+ user_time_set_one_time(1000);
+ }
+ else
+ state=wait_getting_up;
+ break;
+ case wait_stabilize: if(user_time_is_done())
+ {
+ balance_reset_fall_state();
+ state=stop_motion;
+ done=0x01;
+ }
+ else
+ state=wait_stabilize;
+ break;
+ }
+
+ return done;
+}
+
+void user_init(void)
+{
+ serial_console_init(57600);
+ balance_init();
+ balance_calibrate_gyro();
+ balance_enable_gyro();
+}
+
+void user_loop(void)
+{
+ static main_states state=wait_start;
+ static fallen_t fall_state;
+
+ switch(state)
+ {
+ case wait_start: if(is_button_rising_edge(BTN_START))
+ state=check_fallen;
+ else
+ state=wait_start;
+ break;
+ case check_fallen: fall_state=balance_robot_has_fallen();
+ if(fall_state!=robot_standing)
+ {
+ state=get_up;
+ }
+ else
+ state=check_fallen;
+ break;
+ case get_up: if(get_up_process(fall_state)==0x01)
+ state=check_fallen;
+ else
+ state=get_up;
+ break;
+ }
+}
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 2f4fe00..9465347 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -215,8 +215,6 @@ uint8_t manager_init(uint8_t num_servos)
manager_timer_init();
/* initialize the action module */
action_init();
- /* initialize the balance module */
- balance_init();
return num;
}
--
GitLab