diff --git a/motion/include/mtn_library.h b/motion/include/mtn_library.h
index 925bf9611c6642aeb667b49db7773be67b5def9d..3a2fc57eed029e5b005ab246f5acadc306f65cec 100644
--- a/motion/include/mtn_library.h
+++ b/motion/include/mtn_library.h
@@ -8,6 +8,9 @@ void mtn_lib_init(void);
void mtn_lib_stop_mtn(void);
void mtn_lib_set_start_foot(foot_t foot);
+void reset_steps(void);
+int get_steps (void);
+
uint8_t walk_forward(void);
uint8_t walk_backward(void);
uint8_t turn_left(void);
@@ -37,10 +40,12 @@ uint8_t fast_walk_backward_turn_right(void);
uint8_t walk_forward_compensating(int comp_ini, int current);
+uint8_t walk_backward_compensating(int comp_ini, int current);
uint8_t get_up_process(fallen_t fall_state);
uint8_t stairs_up_process (fallen_t fall_state);
uint8_t stairs_down_process(void);
uint8_t walk_left_compensating (int comp_ini, int comp_act);
uint8_t walk_right_compensating (int comp_ini, int comp_act);
-
+uint8_t turn_angle (int angle);
+//uint8_t fast_walk_forward_compensating(int comp_ini, int current);
#endif
diff --git a/motion/src/mtn_library.c b/motion/src/mtn_library.c
index 3a61e8fdb9c46983353ad03c84b9fcb80687909c..b7c143db0d3eac71b8244642b120ccbc6e7c69c0 100644
--- a/motion/src/mtn_library.c
+++ b/motion/src/mtn_library.c
@@ -6,12 +6,16 @@
#include <stdlib.h>
#include "cm510.h"
// private variables
+
+#define err 100 // the error allowed for when we turn (angle) now set at 10º = 100
typedef enum {mtn_fwd=0,mtn_bwd=1,mtn_turn_left=2,mtn_turn_right=3,mtn_left=4,mtn_right=5,mtn_fwd_turn_left=6,mtn_fwd_turn_right=7,
mtn_bwd_turn_left=8,mtn_bwd_turn_right=9,mtn_fwd_left=10,mtn_fwd_right=11,mtn_bwd_left=12,mtn_bwd_right=13,
mtn_fast_fwd=14,mtn_fast_bwd=15} mtn_t;
typedef enum {idle,wait_start,wait_middle,wait_end} full_states;
typedef enum {wf_idle,wf_middle,wf_adjust,wf_end} wf_states;
+typedef enum {wb_idle,wb_middle,wb_adjust,wb_end} wb_states;
+typedef enum {t_init,t_middle,t_left,t_right,t_wait_end} turn_states;
typedef enum {wrc_idle,wrc_middle,wrc_wait_stop,wrc_adjust,wrc_end} wrc_states;
typedef enum {wlc_idle,wlc_middle,wlc_wait_stop,wlc_adjust,wlc_end} wlc_states;
@@ -27,7 +31,7 @@ typedef struct{
uint8_t end_l;
uint8_t end_r;
}TPages;
-
+int step_count;
uint8_t mtn_lib_stop;
foot_t mtn_lib_start_foot;
foot_t mtn_lib_current_foot;
@@ -331,6 +335,16 @@ int compass_deviation (int ini, int actual){ //Calculates the difference between
return inc;
} //return value is between -1800 and 1800
+int add_angles (int a, int b){
+ int res = a+b;
+ if (res < 0 ) res+=360;
+ else if (res>360){
+ res-=360;
+ }
+ return res;
+
+}
+
/* public functions */
void mtn_lib_init(void)
@@ -338,6 +352,7 @@ void mtn_lib_init(void)
mtn_lib_stop=0x00;
mtn_lib_start_foot=left_foot;
mtn_lib_current_foot=left_foot;
+ step_count = 0;
}
void mtn_lib_stop_mtn(void)
@@ -350,6 +365,15 @@ void mtn_lib_set_start_foot(foot_t foot)
mtn_lib_start_foot=foot;
}
+void reset_steps(void)
+{
+ step_count = 0;
+}
+
+int get_steps (void){
+ return step_count;
+}
+
uint8_t walk_forward(void)
{
return mtn_lib_full(&mtn_pages[mtn_fwd]);
@@ -480,6 +504,7 @@ uint8_t walk_forward_compensating(int comp_ini, int current){ //el primer cop qu
case wf_middle:
if (is_action_running()) state = wf_middle;
else {
+ step_count++;
if (mtn_lib_stop==0x01){
if (mtn_lib_current_foot==left_foot) action_set_page (F_E_L);
else action_set_page (F_E_R);
@@ -520,6 +545,7 @@ uint8_t walk_forward_compensating(int comp_ini, int current){ //el primer cop qu
case wf_adjust:
if (is_action_running()) state = wf_adjust;
else {
+ step_count++;
if (mtn_lib_stop==0x01){
if (diff > 0){ // si estamos girando a la izq solo podemos acabar con la derecha
if (mtn_lib_current_foot==right_foot){
@@ -569,6 +595,7 @@ uint8_t walk_forward_compensating(int comp_ini, int current){ //el primer cop qu
case wf_end:
if (is_action_running()) state = wf_end;
else {
+ step_count++;
mtn_lib_stop =0x00;
state = wf_idle;
done = 0x01;
@@ -580,6 +607,125 @@ uint8_t walk_forward_compensating(int comp_ini, int current){ //el primer cop qu
}
+uint8_t walk_backward_compensating(int comp_ini, int current){ //el primer cop que es crida ha d'estar en walk_ready
+ static wb_states state = wb_idle;
+ uint8_t done=0x00;
+ int diff = compass_deviation(comp_ini,current);
+ switch (state) {
+ case wb_idle:
+ if (mtn_lib_start_foot==left_foot) action_set_page (B_S_L);
+ else action_set_page (B_S_R);
+ action_start_page();
+ mtn_lib_current_foot = (mtn_lib_start_foot + 1 )%2;
+ state = wb_middle;
+ break;
+ case wb_middle:
+ if (is_action_running()) state = wb_middle;
+ else {
+ step_count--;
+ if (mtn_lib_stop==0x01){
+ if (mtn_lib_current_foot==left_foot) action_set_page (B_E_L);
+ else action_set_page (B_E_R);
+ action_start_page();
+ state = wb_end;
+ }
+ else {
+ //si no se ha desviado o se ha desviado pero toca andar con el otro pie
+ if (abs (diff)<100 || (diff>100 && mtn_lib_current_foot==left_foot)|| (diff<-100 && mtn_lib_current_foot==right_foot )){
+ if (mtn_lib_current_foot==left_foot) action_set_page (B_M_L);
+ else action_set_page (B_M_R);
+ action_start_page();
+ state = wb_middle;
+ }
+ else { //se ha desviado
+ if (diff>0){ //si tenemos que corregir hacia la derecha solo podemos empezar con el pie derecho
+ if (mtn_lib_current_foot==right_foot) {//--> quizas este if no es necesario
+ action_set_page (BRT_M_R);
+ state = wb_adjust;
+ action_start_page();
+ }
+ else state = wb_middle;
+ }
+ else { //si tenemos que corregir hacia la izquierda solo podemos empezar con el pie izquierdo
+ if (mtn_lib_current_foot==left_foot) {
+ state = wb_adjust;
+ action_set_page (BLT_M_L);
+ action_start_page();
+ }
+ else state = wb_middle;
+ }
+ }
+
+ }
+ change_foot();
+ }
+ break;
+ case wb_adjust:
+ if (is_action_running()) state = wb_adjust;
+ else {
+ step_count--;
+ if (mtn_lib_stop==0x01){
+ if (diff < 0){ // si estamos girando a la izq solo podemos acabar con la derecha
+ if (mtn_lib_current_foot==right_foot){
+ action_set_page (BLT_E_R);
+ state=wb_end;
+ }
+ else {
+ action_set_page (BLT_M_L);
+ state = wb_adjust;
+ }
+ }
+ else {// si estamos girando a la derecha solo podemos acabar con la izq
+ if (mtn_lib_current_foot==left_foot) {
+ action_set_page (BRT_E_L);
+ state = wb_end;
+ }
+ else {
+ action_set_page (BRT_M_R);
+ state = wb_adjust;
+ }
+ }
+ action_start_page();
+ change_foot();
+
+ }
+ else {
+ if (abs (diff) > 100) {
+ if (diff < 0){ //Girar a la izq
+ if (mtn_lib_current_foot==left_foot) action_set_page (BLT_M_L);//Forward Left Turn - wb_middle -Left foot
+ else action_set_page (BLT_M_R);//Forward Left Turn - wb_middle -Right foot
+ }
+ else { //Girar a la derecha
+ if (mtn_lib_current_foot==left_foot) action_set_page (BRT_M_L);//Forward Right Turn - wb_middle -Left foot
+ else action_set_page (BRT_M_R);//Forward Right Turn - wb_middle -Right foot
+ }
+ action_start_page();
+ state = wb_adjust;
+ change_foot();
+
+ }
+ else { //podemos volver a andar normal
+ state = wb_middle;
+ }
+ }
+ }
+ break;
+ case wb_end:
+ if (is_action_running()) state = wb_end;
+ else {
+ step_count--;
+ mtn_lib_stop =0x00;
+ state = wb_idle;
+ done = 0x01;
+ }
+ break;
+ }
+ return done;
+
+
+}
+
+
uint8_t get_up_process(fallen_t fall_state)
{
static get_up_states state;
@@ -945,4 +1091,64 @@ uint8_t walk_right_compensating (int comp_ini, int comp_act){
}
return done;
}
+uint8_t turn_angle (int angle){
+ static turn_states s = t_init;
+ static int comp_ini = 0;
+ static int comp_end = 0;
+ int done = 0;
+
+ switch (s){
+ case t_init:
+ comp_ini = exp_compass_get_avg_heading();
+ comp_end = add_angles (comp_ini,angle*10);
+
+ s=t_middle;
+ break;
+ case t_middle:
+ if (abs (compass_deviation (exp_compass_get_avg_heading(),comp_end))>err){
+ // ("diff = %d\n",compass_deviation (exp_compass_get_avg_heading(),comp_end));
+ if (compass_deviation (exp_compass_get_avg_heading(),comp_end)>err){
+ s =t_right;
+ }
+ else if (compass_deviation (exp_compass_get_avg_heading(),comp_end)<-err){
+ s=t_left;
+ }
+ }
+ else {
+ s = t_wait_end;
+ }
+ break;
+ case t_right:
+ if (turn_right()){
+ s = t_wait_end;
+ }
+ else {
+ if (compass_deviation (exp_compass_get_avg_heading(),comp_end)<err){
+ mtn_lib_stop_mtn();
+ }
+ else s = t_right;
+
+ }
+ break;
+
+ case t_left:
+ if (turn_left()){
+ s = t_wait_end;
+ }
+ else {
+ if (compass_deviation (exp_compass_get_avg_heading(),comp_end)>-err){
+ mtn_lib_stop_mtn();
+ }
+ else s = t_left;
+
+ }
+ break;
+ case t_wait_end:
+ done =0x01;
+ s = t_init;
+ break;
+
+ }
+ return done;
+}