Skip to content
Snippets Groups Projects
Commit b63d36f3 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

added mtn_library files

parent 2709bef5
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
motion/include/mtn_library.h
+ 6
− 1
View file @ b63d36f3
......@@ -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
motion/src/mtn_library.c
+ 207
− 1
View file @ b63d36f3
......@@ -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;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment