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

@@ -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)

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

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];

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/

motion/src/examples/get_up.c

+#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;
+  }
+}

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;
 }