-
Laia Freixas Mateu authoredLaia Freixas Mateu authored
mtn_library.c 32.98 KiB
#include <avr/io.h>
#include "mtn_library.h"
#include "action_id.h"
#include "action.h"
#include <stdio.h>
#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;
typedef enum {wait_stop_motion,wait_getting_up,wait_stabilize} get_up_states;
typedef enum {up_idle,up_wait_stop,up_wait_step1,up_delay1,up_wait_step2,up_wait_step3,up_wait_step4,wait_ready_up} stairs_up_states;
typedef enum {down_idle,down_wait_stop,down_wait_step1,down_wait_step2,down_wait_step3,wait_ready_down} stairs_down_states;
typedef struct{
uint8_t start_l;
uint8_t start_r;
uint8_t middle_l;
uint8_t middle_r;
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;
TPages mtn_pages[]={{F_S_L,F_S_R,F_M_L,F_M_R,F_E_L,F_E_R}, //mtn_fwd
{B_S_L,B_S_R,B_M_L,B_M_R,B_E_L,B_E_R}, //mtn_bwd
{LT_S_L,LT_S_R,LT_M_L,LT_M_R,LT_E_L,LT_E_R},
{RT_S_L,RT_S_R,RT_M_L,RT_M_R,RT_E_L,RT_E_R},
{L_S_L,L_S_R,L_M_L,L_M_R,L_E_L,L_E_R},
{R_S_L,R_S_R,R_M_L,R_M_R,R_E_L,R_E_R},
{FLT_S_L,FLT_S_R,FLT_M_L,FLT_M_R,FLT_E_L,FLT_E_R},
{FRT_S_L,FRT_S_R,FRT_M_L,FRT_M_R,FRT_E_L,FRT_E_R},
{BLT_S_L,BLT_S_R,BLT_M_L,BLT_M_R,BLT_E_L,BLT_E_R},
{BRT_S_L,BRT_S_R,BRT_M_L,BRT_M_R,BRT_E_L,BRT_E_R},
{FL_S_L,FL_S_R,FL_M_L,FL_M_R,FL_E_L,FL_E_R},
{FR_S_L,FR_S_R,FR_M_L,FR_M_R,FR_E_L,FR_E_R},
{BL_S_L,BL_S_R,BL_M_L,BL_M_R,BL_E_L,BL_E_R},
{BR_S_L,BR_S_R,BR_M_L,BR_M_R,BR_E_L,BR_E_R},
{fst_F_L_S,fst_F_R_S,fst_F_R_L_M,fst_F_L_R_M,fst_F_R_E,fst_F_L_E}, //fast forward
{fst_B_L_S,fst_B_R_S,fst_B_L_M,fst_B_R_M,fst_B_L_E,fst_B_R_E}}; //fast backward
/* private functions */
uint8_t mtn_lib_full(TPages *pages)
{
static full_states state=idle;
uint8_t done=0x00;
switch(state)
{
case idle: if(mtn_lib_start_foot==left_foot)
action_set_page(pages->start_l);
else
action_set_page(pages->start_r);
action_start_page();
mtn_lib_current_foot=mtn_lib_start_foot;
state=wait_start;
break;
case wait_start: if(is_action_running())
state=wait_start;
else
{
if(mtn_lib_stop==0x01)
{
if(mtn_lib_current_foot==left_foot)
{
action_set_page(pages->end_r);
mtn_lib_current_foot=right_foot;
}
else
{
action_set_page(pages->end_l);
mtn_lib_current_foot=left_foot;
}
action_start_page();
state=wait_end;
}
else
{
if(mtn_lib_current_foot==left_foot)
{
action_set_page(pages->middle_r);
mtn_lib_current_foot=right_foot;
}
else
{
action_set_page(pages->middle_l);
mtn_lib_current_foot=left_foot;
}
action_start_page();
state=wait_middle;
}
}
break;
case wait_middle: if(is_action_running())
state=wait_middle;
else
{
if(mtn_lib_stop==0x01)
{
if(mtn_lib_current_foot==left_foot)
{
action_set_page(pages->end_r);
mtn_lib_current_foot=right_foot;
}
else
{
action_set_page(pages->end_l);
mtn_lib_current_foot=left_foot;
}
action_start_page();
state=wait_end;
}
else
{
if(mtn_lib_current_foot==left_foot)
{
action_set_page(pages->middle_r);
mtn_lib_current_foot=right_foot;
}
else
{
action_set_page(pages->middle_l);
mtn_lib_current_foot=left_foot;
}
action_start_page();
state=wait_middle;
}
}
break;
case wait_end: if(is_action_running())
state=wait_end;
else
{
mtn_lib_stop=0x00;
state=idle;
done=0x01;
}
break;
}
return done;
}
uint8_t mtn_lib_left(TPages *pages)
{
static full_states state=idle;
uint8_t done=0x00;
switch(state)
{
case idle: if(mtn_lib_start_foot==left_foot)
action_set_page(pages->start_l);
else
action_set_page(pages->start_r);
action_start_page();
mtn_lib_current_foot=mtn_lib_start_foot;
state=wait_start;
break;
case wait_start: if(is_action_running())
state=wait_start;
else
{
if(mtn_lib_current_foot==left_foot)
{
if(mtn_lib_stop==0x01)
{
action_set_page(pages->end_r);
state=wait_end;
}
else
{
action_set_page(pages->middle_r);
state=wait_middle;
}
mtn_lib_current_foot=right_foot;
action_start_page();
}
else
{
action_set_page(pages->middle_l);
mtn_lib_current_foot=left_foot;
action_start_page();
state=wait_middle;
}
}
break;
case wait_middle: if(is_action_running())
state=wait_middle;
else
{
if(mtn_lib_current_foot==left_foot)
{
if(mtn_lib_stop==0x01)
{
action_set_page(pages->end_r);
state=wait_end;
}
else
{
action_set_page(pages->middle_r);
state=wait_middle;
}
mtn_lib_current_foot=right_foot;
action_start_page();
}
else
{
action_set_page(pages->middle_l);
mtn_lib_current_foot=left_foot;
action_start_page();
state=wait_middle;
}
}
break;
case wait_end: if(is_action_running())
state=wait_end;
else
{
mtn_lib_stop=0x00;
state=idle;
done=0x01;
}
break;
}
return done;
}
uint8_t mtn_lib_right(TPages *pages)
{
static full_states state=idle;
uint8_t done=0x00;
switch(state)
{
case idle: if(mtn_lib_start_foot==left_foot)
action_set_page(pages->start_l);
else
action_set_page(pages->start_r);
action_start_page();
mtn_lib_current_foot=mtn_lib_start_foot;
state=wait_start;
break;
case wait_start: if(is_action_running())
state=wait_start;
else
{
if(mtn_lib_current_foot==right_foot)
{
if(mtn_lib_stop==0x01)
{
action_set_page(pages->end_l);
state=wait_end;
}
else
{
action_set_page(pages->middle_l);
state=wait_middle;
}
mtn_lib_current_foot=left_foot;
action_start_page();
}
else
{
action_set_page(pages->middle_r);
mtn_lib_current_foot=right_foot;
action_start_page();
state=wait_middle;
}
}
break;
case wait_middle: if(is_action_running())
state=wait_middle;
else
{
if(mtn_lib_current_foot==right_foot)
{
if(mtn_lib_stop==0x01)
{
action_set_page(pages->end_l);
state=wait_end;
}
else
{
action_set_page(pages->middle_l);
state=wait_middle;
}
mtn_lib_current_foot=left_foot;
action_start_page();
}
else
{
action_set_page(pages->middle_r);
mtn_lib_current_foot=right_foot;
action_start_page();
state=wait_middle;
}
}
break;
case wait_end: if(is_action_running())
state=wait_end;
else
{
mtn_lib_stop=0x00;
state=idle;
done=0x01;
}
break;
}
return done;
}
void change_foot(void){
if (mtn_lib_current_foot==left_foot) mtn_lib_current_foot = right_foot;
else mtn_lib_current_foot = left_foot;
}
int compass_deviation (int ini, int actual){ //Calculates the difference between initial orientation and actual orientation of compass.
short int inc = actual - ini;
inc = (inc%3600 + 3600)%3600;
//if (inc<-1800) inc+=3600;
if (inc>1800) inc-=3600;
return inc;
} //return value is between -1800 and 1800
int add_angles (int a, int b){
int res = a+b;
//res = (res%3600+3600)%3600;
if (res < 0 ) res+=3600;
else if (res>3600){
res-=3600;
}
return res;
}
/* public functions */
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)
{
mtn_lib_stop=0x01;
}
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]);
}
uint8_t walk_backward(void)
{
return mtn_lib_full(&mtn_pages[mtn_bwd]);
}
uint8_t turn_left(void)
{
return mtn_lib_left(&mtn_pages[mtn_turn_left]);
}
uint8_t turn_right(void)
{
return mtn_lib_right(&mtn_pages[mtn_turn_right]);
}
uint8_t walk_left(void)
{
return mtn_lib_left(&mtn_pages[mtn_left]);
}
uint8_t walk_right(void)
{
return mtn_lib_right(&mtn_pages[mtn_right]);
}
uint8_t walk_forward_turn_left(void)
{
return mtn_lib_left(&mtn_pages[mtn_fwd_turn_left]);
}
uint8_t walk_forward_turn_right(void)
{
return mtn_lib_right(&mtn_pages[mtn_fwd_turn_right]);
}
uint8_t walk_backward_turn_left(void)
{
return mtn_lib_left(&mtn_pages[mtn_bwd_turn_left]);
}
uint8_t walk_backward_turn_right(void)
{
return mtn_lib_right(&mtn_pages[mtn_bwd_turn_right]);
}
uint8_t walk_forward_left(void)
{
return mtn_lib_left(&mtn_pages[mtn_fwd_left]);
}
uint8_t walk_forward_right(void)
{
return mtn_lib_right(&mtn_pages[mtn_fwd_right]);
}
uint8_t walk_backward_left(void)
{
return mtn_lib_left(&mtn_pages[mtn_bwd_left]);
}
uint8_t walk_backward_right(void)
{
return mtn_lib_right(&mtn_pages[mtn_bwd_right]);
}
uint8_t fast_walk_forward(void)
{
return mtn_lib_full(&mtn_pages[mtn_fast_fwd]);
}
uint8_t fast_walk_backward(void)
{
return mtn_lib_full(&mtn_pages[mtn_fast_bwd]);
}
uint8_t fast_turn_left(void)
{
//return mtn_lib_left (&mtn_pages[mtn_]);
}
uint8_t fast_turn_right(void)
{
}
uint8_t fast_walk_left(void)
{
}
uint8_t fast_walk_right(void)
{
}
uint8_t fast_walk_forward_turn_left(void)
{
}
uint8_t fast_walk_forward_turn_right(void)
{
}
uint8_t fast_walk_backward_turn_left(void)
{
}
uint8_t fast_walk_backward_turn_right(void)
{
}
uint8_t walk_forward_compensating(int comp_ini, int current){ //el primer cop que es crida ha d'estar en walk_ready
static wf_states state = wf_idle;
uint8_t done=0x00;
int diff = compass_deviation(comp_ini,current);
switch (state) {
case wf_idle:
if (mtn_lib_start_foot==left_foot) action_set_page (F_S_L);
else action_set_page (F_S_R);
action_start_page();
mtn_lib_current_foot = (mtn_lib_start_foot + 1 )%2;
state = wf_middle;
break;
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);
action_start_page();
state = wf_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 (F_M_L);
else action_set_page (F_M_R);
action_start_page();
state = wf_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 (FRT_M_R);
state = wf_adjust;
action_start_page();
}
else state = wf_middle;
}
else { //si tenemos que corregir hacia la izquierda solo podemos empezar con el pie izquierdo
if (mtn_lib_current_foot==left_foot) {
state = wf_adjust;
action_set_page (FLT_M_L);
action_start_page();
}
else state = wf_middle;
}
}
}
change_foot();
}
break;
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){
action_set_page (FLT_E_R);
state=wf_end;
}
else {
action_set_page (FLT_M_L);
state = wf_adjust;
}
}
else {// si estamos girando a la derecha solo podemos acabar con la izq
if (mtn_lib_current_foot==left_foot) {
action_set_page (FRT_E_L);
state = wf_end;
}
else {
action_set_page (FRT_M_R);
state = wf_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 (FLT_M_L);//Forward Left Turn - wf_middle -Left foot
else action_set_page (FLT_M_R);//Forward Left Turn - wf_middle -Right foot
}
else { //Girar a la derecha
if (mtn_lib_current_foot==left_foot) action_set_page (FRT_M_L);//Forward Right Turn - wf_middle -Left foot
else action_set_page (FRT_M_R);//Forward Right Turn - wf_middle -Right foot
}
action_start_page();
state = wf_adjust;
change_foot();
}
else { //podemos volver a andar normal
state = wf_middle;
}
}
}
break;
case wf_end:
if (is_action_running()) state = wf_end;
else {
step_count++;
mtn_lib_stop =0x00;
state = wf_idle;
done = 0x01;
}
break;
}
return done;
}
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=wait_stop_motion;
uint8_t done=0x00;
switch(state)
{
case wait_stop_motion:
if(!is_action_running()) {
if(fall_state==robot_face_down)
action_set_page(F_getup);
else
action_set_page(B_getup);
action_start_page();
state=wait_getting_up;
}
else//if the robot was moving we wait until it stops
state= wait_stop_motion;
break;
case wait_getting_up: if(!is_action_running()) //if it stopped getting up
{
state=wait_stabilize;
user_time_set_one_time(1000); //we set a timer for the robot to stabilize
}
else
state=wait_getting_up;
break;
case wait_stabilize: if(user_time_is_done())
{
printf ("user time is done\n");
balance_reset_fall_state();
state=wait_stop_motion;
done=0x01;
}
else{
state=wait_stabilize;
}
break;
}
return done;
}
uint8_t stairs_up_process(fallen_t fall_state)
{
static stairs_up_states state=up_idle;
uint8_t done=0x00;
switch(state)
{
case up_idle: if(is_action_running())
{
action_stop_page();
state = up_wait_stop;
}
else
{
action_set_page(227);
action_start_page();
state = up_wait_step1;
}
break;
case up_wait_stop: if(is_action_running())
state = up_wait_stop;
else
{
action_set_page(227);
action_start_page();
state = up_wait_step1;
}
break;
case up_wait_step1: if(is_action_running())
state = up_wait_step1;
else
{
user_time_set_one_time(200);
state = up_delay1;
}
break;
case up_delay1: if(user_time_is_done())
{
action_set_page(224);
action_start_page();
state = up_wait_step2;
}
else
state = up_delay1;
break;
case up_wait_step2: if(is_action_running())
state = up_wait_step2;
else
{
action_set_page(225);
action_start_page();
state = up_wait_step3;
}
break;
case up_wait_step3: if(is_action_running())
state = up_wait_step3;
else
{
action_set_page(226);
action_start_page();
state = up_wait_step4;
}
break;
case up_wait_step4: if(is_action_running())
state = up_wait_step4;
else
{
action_set_page(31);
action_start_page();
state = wait_ready_up;
}
break;
case wait_ready_up: if(is_action_running())
state = wait_ready_up;
else
{
state=up_idle;
done=0x01;
}
break;
}
if (fall_state!=robot_standing) state=up_idle;
return done;
}
uint8_t stairs_down_process(void)
{
static stairs_down_states state=down_idle;
uint8_t done=0x00;
switch(state)
{
case down_idle: if(is_action_running())
{
action_stop_page();
state = down_wait_stop;
}
else
{
action_set_page(228);
action_start_page();
state = down_wait_step1;
}
break;
case down_wait_stop: if(is_action_running())
state = down_wait_stop;
else
{
action_set_page(228);
action_start_page();
state = down_wait_step1;
}
break;
case down_wait_step1: if(is_action_running())
state = down_wait_step1;
else
{
action_set_page(229);
action_start_page();
state = down_wait_step2;
}
break;
case down_wait_step2: if(is_action_running())
state = down_wait_step2;
else
{
action_set_page(31);
action_start_page();
state = wait_ready_down;
}
break;
case down_wait_step3: if(is_action_running())
state = down_wait_step3;
else
{
action_set_page(31);
action_start_page();
state = wait_ready_down;
}
break;
case wait_ready_down: if(is_action_running())
state = wait_ready_down;
else
{
state=down_idle;
done=0x01;
}
break;
}
return done;
}
uint8_t walk_left_compensating (int comp_ini, int comp_act){
static wlc_states s = wlc_idle;
int done =0;
int diff = compass_deviation (comp_ini,comp_act);
switch (s){
case wlc_idle:
if (mtn_lib_start_foot==left_foot) action_set_page (L_S_L);
else action_set_page (L_S_R);
action_start_page();
mtn_lib_current_foot = (mtn_lib_start_foot + 1 )%2;
s = wlc_middle;
break;
case wlc_middle:
if (is_action_running()) s = wlc_middle;
else {
if (mtn_lib_stop==0x01){
if (mtn_lib_current_foot==right_foot) {
action_set_page (L_E_R);
s = wlc_end;
}
else action_set_page (L_M_L); //we cant end with left foot
action_start_page();
}
else { //si no paramos
if (abs (diff)<200){//si no se ha desviado
if (mtn_lib_current_foot==left_foot) action_set_page (L_M_L);
else action_set_page (L_M_R);
action_start_page();
s = wlc_middle;
}
else { //se ha desviado
//paramos el movimiento y pasamos a ajustar -> solo podemos acabar con REL
if (mtn_lib_current_foot==right_foot) {
action_set_page (L_E_R);
action_start_page();
s= wlc_wait_stop;
}
else {
action_set_page (L_M_L);
action_start_page();
s= wlc_middle;
}
}
}
change_foot();
}
break;
case wlc_wait_stop:
if (is_action_running()) s = wlc_wait_stop;
else s = wlc_adjust;
case wlc_adjust:
if (diff<0) //tenemos que girar a la derecha
{
if (turn_right()) {
s = wlc_idle;
mtn_lib_stop = 0;
}
else if (diff>-100) mtn_lib_stop_mtn();
}
else {
if (turn_left()) {
s = wlc_idle;
mtn_lib_stop = 0;
}
else if (diff<100) mtn_lib_stop_mtn();
}
break;
case wlc_end:
if (is_action_running()) s = wlc_end;
else {
mtn_lib_stop = 0;
done = 0x01;
s = wlc_idle;
}
break;
}
return done;
}
uint8_t walk_right_compensating (int comp_ini, int comp_act){
static wrc_states s = wrc_idle;
int done =0;
int diff = compass_deviation (comp_ini,comp_act);
switch (s){
case wrc_idle:
if (mtn_lib_start_foot==left_foot) action_set_page (R_S_L);
else action_set_page (R_S_R);
action_start_page();
mtn_lib_current_foot = (mtn_lib_start_foot + 1 )%2;
s = wrc_middle;
break;
case wrc_middle:
if (is_action_running()) s = wrc_middle;
else {
if (mtn_lib_stop==0x01){
if (mtn_lib_current_foot==left_foot) {
action_set_page (R_E_L);
s = wrc_end;
}
else action_set_page (R_M_R); //we cant end with Right foot
action_start_page();
}
else { //si no paramos
if (abs (diff)<200){//si no se ha desviado
if (mtn_lib_current_foot==left_foot) action_set_page (R_M_L);
else action_set_page (R_M_R);
action_start_page();
s = wrc_middle;
}
else { //se ha desviado
//paramos el movimiento y pasamos a ajustar -> solo podemos acabar con REL
if (mtn_lib_current_foot==left_foot) {
action_set_page (R_E_L);
action_start_page();
s= wrc_wait_stop;
}
else {
action_set_page (R_M_R);
action_start_page();
s= wrc_middle;
}
}
}
change_foot();
}
break;
case wrc_wait_stop:
if (is_action_running()) s = wrc_wait_stop;
else s = wrc_adjust;
case wrc_adjust:
if (diff<0) //tenemos que girar a la derecha
{
if (turn_right()) {
s = wrc_idle;
mtn_lib_stop = 0;
}
else if (diff>-100) mtn_lib_stop_mtn();
}
else {
if (turn_left()) {
s = wrc_idle;
mtn_lib_stop = 0;
}
else if (diff<100) mtn_lib_stop_mtn();
}
break;
case wrc_end:
if (is_action_running()) s = wrc_end;
else {
mtn_lib_stop = 0;
done = 0x01;
s = wrc_idle;
}
break;
}
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){
mtn_lib_stop = 0x00;
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 (abs (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 (abs (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;
mtn_lib_stop = 0x00;
s = t_init;
break;
}
return done;
}