Diff: main.cpp

Revision:

1:37af6a93195e

Parent:

0:6a1be4584c5b

Child:

2:3a4f83edb424

--- a/main.cpp	Fri Apr 10 03:18:03 2015 +0000
+++ b/main.cpp	Wed Apr 15 15:02:19 2015 +0000
@@ -8,11 +8,12 @@
 Serial pc(USBTX, USBRX); // tx, rx
 DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4);
 
-
 /*--Dynamixel:begin-----------------------------------------------------------------------------------------*/
 #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
@@ -26,44 +27,33 @@
 int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0; //MultiTurnモード
 
 void Dyna_init() {
-    pc.printf("Dynamixel Init : start\n");
+    low = 0; RelaySwitch = 1;
     linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX);
     linear.SetCRSpeed(linear_Speed); linear_goal=linear_Init; linear.SetGoal(linear_Init);
-    pc.printf("Dynamixel Init : linear OK!\n");
     pitch.SetCWLimit(linear_min); pitch.SetCCWLimit(linear_MAX); 
     pitch.SetCRSpeed(pitch_Speed); pitch_goal=linear_Init; pitch.SetGoal(pitch_Init);
-    pc.printf("Dynamixel Init : pitch OK!\n");
     yaw.SetCWLimit(linear_min); yaw.SetCCWLimit(linear_MAX); 
     yaw.SetCRSpeed(yaw_Speed); yaw_goal=yaw_Init; yaw.SetGoal(yaw_Init);
-    pc.printf("Dynamixel Init : yaw OK!\n");
-    pc.printf("Dynamixel Init : end\n");
     wait(1);
 }
 
 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-----------------------------------------------------------------------------------------*/
 
@@ -118,7 +108,7 @@
     while(1) {
         
 //        sock.receiveFrom(echo_server, R_data, sizeof(R_data)); //オペステからデータを取得
-        pc.getc();
+        R_data=pc.getc();
         wait_ms(1);
         int J = atoi(R_data);
         Joy_mode = J & 0x03; //ジョイスティックのモード判定
@@ -143,8 +133,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;
@@ -185,7 +175,7 @@
                 ThD2[8],ThD2[9],ThD2[10],ThD2[11],ThD2[12],ThD2[13],ThD2[14],ThD2[15]);
                 sprintf(S_data, "%f %f %f %f %f %f %s %f",lP,lV,pP,pC,yP,yC,Thermo_data,CO2_data);
 //                sock.sendTo(echo_server, S_data, sizeof(S_data));
-                pc.printf("%s\n",S_data)
+                printf("%s\n",S_data)
                 myled3=0;           
                 break;