sinyuu masahito / Mbed 2 deprecated YOZAKURA_ARM_USB_0427

Modify ota's code

Dependencies:   Dynamixel SerialHalfDuplex mbed

Fork of YOZAKURA_ARM_USB_0425 by 雄介 太田

Files at this revision

API Documentation at this revision

Revision 9:6f3963858d89, committed 2015-04-28

Comitter:
masasin
Date:
Tue Apr 28 06:16:12 2015 +0000
Parent:
8:bbea423c9206
Commit message:
Modify limits

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Mon Apr 27 11:04:34 2015 +0000
+++ b/main.cpp	Tue Apr 28 06:16:12 2015 +0000
@@ -71,44 +71,32 @@
     if(DYNA_DEBUG) printf("\nDyna_SetGoal\n");
     switch(linear_mode){
         case 0: break;
-        case 1: 
-            linear.SetTorqueLimit(1); linear_goal-=10;  
-            if(linear_goal<linear_min) linear_goal=linear_min;
-            break;
-        case 2: 
-            linear.SetTorqueLimit(1); linear_goal+=20;
-            if(linear_goal>linear_MAX) linear_goal=linear_MAX;
-            break;
+        case 1: linear.SetTorqueLimit(1); linear_goal-=10; break;
+        case 2: linear.SetTorqueLimit(1); linear_goal+=20; break;
     }
     
+    if(linear_goal<linear_min) linear_goal=linear_min;
+    if(linear_goal>linear_MAX) linear_goal=linear_MAX;
     linear.SetGoal(linear_goal);
     
     switch(pitch_mode){
         case 0: break;
-        case 1:
-            pitch_goal-=10;
-            if(pitch_goal<pitch_min) pitch_goal=pitch_min;
-            break;
-        case 2:
-            pitch_goal+=10;
-            if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
-            break;
+        case 1: pitch_goal-=10; break;
+        case 2: pitch_goal+=10; break;
     }    
     
+    if(pitch_goal<pitch_min) pitch_goal=pitch_min;
+    if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
     pitch.SetGoal(pitch_goal);
     
     switch(yaw_mode){
         case 0: break;
-        case 1:
-            yaw_goal-=10;
-            if(pitch_goal<-2520) pitch_goal=-2520;
-            break;
-        case 2:
-            yaw_goal+=10;
-            if(pitch_goal>2520) pitch_goal=2520; 
-            break;
+        case 1: yaw_goal-=10; break;
+        case 2: yaw_goal+=10; break;
     }
 
+    if(yaw_goal<-2520) yaw_goal=-2520;
+    if(yaw_goal>2520) yaw_goal=2520; 
     yaw.SetGoal(yaw_goal);
 }