Diff: main.cpp

Revision:

15:d62819013445

Parent:

14:523d184e5107

Child:

16:b01cfa36fa26

--- a/main.cpp	Sat Apr 11 05:53:45 2015 +0000
+++ b/main.cpp	Thu Apr 16 02:37:43 2015 +0000
@@ -41,6 +41,8 @@
 #include "AX12.h"
 #include "MX28.h"
 
+DigitalOut low(p19); DigitalOut RelaySwitch(p17);
+
 AX12 linear (p13, p14, 0);      //直動Dynamixel
 MX28 pitch (p13, p14, 1);       //ピッチDynamixel
 MX28 yaw (p13, p14, 2);         //ヨーDynamixel
@@ -54,6 +56,7 @@
 int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0; //MultiTurnモード
 
 void Dyna_init() {
+    low = 0; RelaySwitch = 1;
     linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); 
     linear.SetCRSpeed(linear_Speed); linear_goal=linear_Init; linear.SetGoal(linear_Init);
     pitch.SetCWLimit(linear_min); pitch.SetCCWLimit(linear_MAX); 
@@ -65,28 +68,21 @@
 
 void Dyna_home_position() {
     linear.SetGoal(linear_Init);
-    wait(1); 
+    wait(3); 
     pitch.SetGoal(pitch_Init);
     yaw.SetGoal(yaw_Init);
-    wait(1); 
+    wait(2); 
 }
 
 void Dyna_reset() {
-    Dyna_home_position();
-    linear.SetTorqueLimit(0);
-    pitch.SetTorqueLimit(0);
-    yaw.SetTorqueLimit(0);
-    wait(1); 
-    linear.SetTorqueLimit(1);
-    pitch.SetTorqueLimit(1);
-    yaw.SetTorqueLimit(1);
+    RelaySwitch = 0;
+    wait_ms(10);
+    RelaySwitch = 1;
 }
 
 void Dyna_end() {
     Dyna_home_position();
-    linear.TorqueEnable(0);
-    pitch.TorqueEnable(0);
-    yaw.TorqueEnable(0);
+    RelaySwitch = 0;
 }
 /*--Dynamixel:end-----------------------------------------------------------------------------------------*/
 
@@ -169,8 +165,8 @@
                 linear_goal=lP;
                 switch(linear_mode){
                     case 0: break;
-                    case 1: linear_goal++; break;
-                    case 2: linear_goal--; break;
+                    case 1: linear.SetTorqueLimit(1); linear_goal++; break;
+                    case 2: linear.SetTorqueLimit(1); linear_goal--; break;
                 }    
                 linear.SetGoal(linear_goal);
                 pitch_goal=pP;