From f161a83c4042719843ffa9cd77d9224a0deddda7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sat, 16 Apr 2016 23:45:26 +0200
Subject: [PATCH] Solved a bug when stopping the action in the middle of a page
 which does not have a direct exit page. Now the stop flag is only reset when
 the action is actually stopped. Added an example to walk straight.

---
 motion/src/action.c                 |   4 +-
 motion/src/examples/Makefile        |   2 +-
 motion/src/examples/walk_straight.c | 132 ++++++++++++++++++++++++++++
 3 files changed, 134 insertions(+), 4 deletions(-)
 create mode 100644 motion/src/examples/walk_straight.c

diff --git a/motion/src/action.c b/motion/src/action.c
index bcdc50a..622be3d 100644
--- a/motion/src/action.c
+++ b/motion/src/action.c
@@ -208,6 +208,7 @@ void action_process(void)
       if( action_finished == 0x01 ) // ¸ð¼ÇÀÌ ³¡³µ´Ù¸é
       {
         action_is_running=0x00;
+        action_stop = 0x00;
         return;
       }
       action_step_count++;
@@ -225,10 +226,7 @@ void action_process(void)
       {
         // ´ÙÀ½ ÆäÀÌÁö ·Îµù
         if( action_stop == 0x01 ) // ¸ð¼Ç Á¤Áö ¸í·ÉÀÌ ÀÖ´Ù¸é
-        {
           action_next_index = action_current_page.header.exit; // ´ÙÀ½ ÆäÀÌÁö´Â Exit ÆäÀÌÁö·Î
-          action_stop = 0x00;
-        }
         else
         {
           bPlayRepeatCount--;
diff --git a/motion/src/examples/Makefile b/motion/src/examples/Makefile
index 1cb7fe8..4dacfd5 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=get_up.c
+SOURCES=walk_straight.c
 
 OBJS=$(SOURCES:.c=.o)
 SRC_DIR=./src/
diff --git a/motion/src/examples/walk_straight.c b/motion/src/examples/walk_straight.c
new file mode 100644
index 0000000..358f50b
--- /dev/null
+++ b/motion/src/examples/walk_straight.c
@@ -0,0 +1,132 @@
+#include <util/delay.h>
+#include <stdio.h>
+#include "cm510.h"
+#include "balance.h"
+#include "exp_board.h"
+#include <stdlib.h>
+
+typedef enum {wait_start,wait_walk_ready,check_compass,turn} main_states;
+typedef enum {stop_motion,wait_stop_motion,check_angle,wait_stop_turn} turn_states;
+
+uint16_t get_avg_heading_inc(uint16_t initial_compass)
+{
+  int16_t actual_compass;
+  int16_t increase;
+  actual_compass = exp_compass_get_avg_heading();
+  increase=actual_compass-(int16_t)initial_compass;
+
+  if(increase<-1800){
+    increase=increase+3600;
+  }else if (increase>1800){
+    increase=increase-3600;
+  }
+  return increase;
+}
+
+uint8_t turn_angle(int16_t angle)
+{
+  static turn_states state=stop_motion;
+  uint8_t done=0x00,turn_error;
+  static uint16_t initial_heading;
+
+  switch(state)
+  {
+    case stop_motion: if(is_action_running())
+                      {
+                        action_stop_page();
+                        state=wait_stop_motion;
+                      }
+                      else
+                      {
+                        if(angle>0)
+                          action_set_page(68);
+                        else
+                          action_set_page(56);
+                        action_start_page();
+                        initial_heading=exp_compass_get_avg_heading();
+                        state=check_angle;
+                      }
+                      break;
+    case wait_stop_motion: if(is_action_running())
+                             state=wait_stop_motion;
+                           else
+                           {
+                             if(angle>0)
+                               action_set_page(68);
+                             else
+                               action_set_page(56);
+                             action_start_page();
+                             initial_heading=exp_compass_get_avg_heading();
+                             state=check_angle;
+                           }
+                           break;
+    case check_angle: turn_error=abs(get_avg_heading_inc(initial_heading)-angle);
+                      if(turn_error<100)
+                      {
+                        action_stop_page();
+                        state=wait_stop_turn;
+                      }
+                      else
+                        state=check_angle;
+                      break;
+    case wait_stop_turn: if(is_action_running())
+                           state=wait_stop_turn;
+                         else
+                         {
+                           state=stop_motion;
+                           done=0x01;
+                         }
+                         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 uint16_t initial_heading;
+  static int16_t increment_heading;
+
+  switch(state)
+  {
+    case wait_start: if(is_button_rising_edge(BTN_START))
+                     {
+                       action_set_page(31);
+                       action_start_page();
+                       state=wait_walk_ready;
+                     }
+                     else
+                       state=wait_start;
+                     break;
+    case wait_walk_ready: if(is_action_running())
+                            state=wait_walk_ready;
+                          else
+                          {
+                            initial_heading=exp_compass_get_avg_heading();
+                            action_set_page(32);
+                            action_start_page();
+                            state=check_compass;
+                          }
+                          break;
+    case check_compass: if((increment_heading=get_avg_heading_inc(initial_heading))>200 || (increment_heading=get_avg_heading_inc(initial_heading))<-200)
+                          state=turn;
+                        else
+                          state=check_compass;
+                        break;
+    case turn: if(turn_angle(increment_heading))
+                 state=wait_walk_ready;
+               else
+                 state=turn;
+               break;
+  }
+}
+
-- 
GitLab