From 6f1cd7c4b707ffa89a180badfd58da2054104b9c Mon Sep 17 00:00:00 2001
From: humanoids <lfreixas@iri.upc.edu>
Date: Tue, 6 Sep 2016 15:29:37 +0200
Subject: [PATCH] solved bug in turning around
---
motion/src/mtn_library.c | 46 +++++++++++++++++++++-------------------
1 file changed, 24 insertions(+), 22 deletions(-)
diff --git a/motion/src/mtn_library.c b/motion/src/mtn_library.c
index d0aa135..c79fd82 100644
--- a/motion/src/mtn_library.c
+++ b/motion/src/mtn_library.c
@@ -330,13 +330,15 @@ void change_foot(void){
}
int compass_deviation (int ini, int actual){ //Calculates the difference between initial orientation and actual orientation of compass.
short int inc = actual - ini;
- if (inc<-1800) inc+=3600;
- else if (inc>1800) inc-=3600;
+ 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;
@@ -728,21 +730,21 @@ uint8_t walk_backward_compensating(int comp_ini, int current){ //el primer cop q
uint8_t get_up_process(fallen_t fall_state)
{
- static get_up_states 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
+ 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
@@ -750,17 +752,19 @@ uint8_t get_up_process(fallen_t fall_state)
state=wait_stabilize;
user_time_set_one_time(1000); //we set a timer for the robot to stabilize
}
- else
+ 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
+ else{
state=wait_stabilize;
+ }
break;
}
@@ -798,7 +802,7 @@ uint8_t stairs_up_process(fallen_t fall_state)
state = up_wait_step1;
else
{
- user_time_set_one_time(1500);
+ user_time_set_one_time(200);
state = up_delay1;
}
break;
@@ -893,9 +897,9 @@ uint8_t stairs_down_process(void)
state = down_wait_step2;
else
{
- action_set_page(230);
+ action_set_page(31);
action_start_page();
- state = down_wait_step3;
+ state = wait_ready_down;
}
break;
case down_wait_step3: if(is_action_running())
@@ -1096,7 +1100,6 @@ uint8_t turn_angle (int angle){
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();
@@ -1106,7 +1109,7 @@ uint8_t turn_angle (int angle){
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));
+ mtn_lib_stop = 0x00;
if (compass_deviation (exp_compass_get_avg_heading(),comp_end)>err){
s =t_right;
}
@@ -1123,11 +1126,10 @@ uint8_t turn_angle (int angle){
s = t_wait_end;
}
else {
- if (compass_deviation (exp_compass_get_avg_heading(),comp_end)<err){
+ if (abs (compass_deviation (exp_compass_get_avg_heading(),comp_end))<err){
mtn_lib_stop_mtn();
}
else s = t_right;
-
}
break;
@@ -1136,15 +1138,15 @@ uint8_t turn_angle (int angle){
s = t_wait_end;
}
else {
- if (compass_deviation (exp_compass_get_avg_heading(),comp_end)>-err){
+ 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;
--
GitLab