solved bug in turning around

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;