Diff: main.cpp

Revision:

4:d9ea010641ab

Parent:

3:6a932d0aac7d

Child:

5:581b55deaeb1

--- a/main.cpp	Sat Apr 25 06:08:18 2015 +0000
+++ b/main.cpp	Sat Apr 25 06:42:51 2015 +0000
@@ -12,6 +12,8 @@
 #include "AX12.h"
 #include "MX28.h"
 
+#define DYNA_DEBUG 1
+
 DigitalOut low(p16); DigitalOut RelaySwitch(p18);
 
 AX12 linear (p13, p14, 0, 1000000);      //直動Dynamixel
@@ -27,6 +29,7 @@
 int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0; //MultiTurnモード
 
 void Dyna_init() {
+    if(DYNA_DEBUG) printf("\nDyna_init\n");
 //    low = 0; RelaySwitch = 1;
     linear.SetTorqueLimit(0);
     pitch.SetTorqueLimit(0);
@@ -41,6 +44,7 @@
 }
 
 void Dyna_GetData(char* data) {
+    if(DYNA_DEBUG) printf("\nDyna_SetData\n");
     float lP, lV, pP, pC, yP, yC;
     
     lP = linear.GetPosition(); lV = linear.GetVolts();
@@ -52,6 +56,7 @@
 }
 
 void Dyna_SetGoal(int linear_mode, int pitch_mode, int yaw_mode) {
+    if(DYNA_DEBUG) printf("\nDyna_SetGoal\n");
     switch(linear_mode){
         case 0: break;
         case 1: linear.SetTorqueLimit(1); linear_goal-=2; break;
@@ -81,6 +86,7 @@
 }
 
 void Dyna_home_position() {
+    if(DYNA_DEBUG) printf("\nDyna_home_position\n");
     linear.SetTorqueLimit(1);
     pitch.SetTorqueLimit(1);
     yaw.SetTorqueLimit(1);
@@ -94,12 +100,13 @@
 }
 
 void Dyna_reset() {
+    if(DYNA_DEBUG) printf("\nDyna_reset\n");
 //    RelaySwitch = 0;
     linear.SetTorqueLimit(0);
     pitch.SetTorqueLimit(0);
     yaw.SetTorqueLimit(0);
     
-    wait(0.1);
+    wait(1);
     
 //    RelaySwitch = 1;
     linear.SetTorqueLimit(1);
@@ -108,6 +115,7 @@
 }
 
 void Dyna_end() {
+    if(DYNA_DEBUG) printf("\nDyna_end\n");
 //    RelaySwitch = 0;
     linear.SetTorqueLimit(0);
     pitch.SetTorqueLimit(0);
@@ -251,6 +259,7 @@
 /*---------------
     MAIN ROOP
 ----------------*/
+#define DEBUG 1
 
 int main() {
     char Dyna_data[50];
@@ -268,15 +277,16 @@
         Read_data=pc.getc();
         wait_ms(1);
         Joy_mode = Read_data & 0xc0; Joy_mode = Joy_mode >> 6; //ジョイスティックのモード判定
+        if(DEBUG) printf("Joy_mode : %d\n",Joy_mode);
         
         switch(Joy_mode){
             case 0: //通常モード
                 myled1=0; myled2=0; myled3=0; myled4=0;
             /*--Dynamixel:begin---------------------------------------------------------------*/       
                 //Dynamixelへの命令を判定
-                linear_mode = Read_data & 0x30; linear_mode = linear_mode >> 4;
-                pitch_mode = Read_data & 0x0c; pitch_mode = pitch_mode >> 2;
-                yaw_mode = Read_data & 0x03;                
+                linear_mode = Read_data & 0x30; linear_mode = linear_mode >> 4; if(DEBUG) printf("linear_mode : %d\n",linear_mode);
+                pitch_mode = Read_data & 0x0c; pitch_mode = pitch_mode >> 2; if(DEBUG) printf("pitch_mode : %d\n",pitch_mode);
+                yaw_mode = Read_data & 0x03; if(DEBUG) printf("yaw_mode : %d\n",yaw_mode);
 
                 //現在の角度・電圧・電流を取得
                 myled1=1;
@@ -303,7 +313,7 @@
             
             //値を送信
                 myled4=1;
-                sprintf(Send_data,"%s%s%f",Dyna_data,Thermo_data,CO2_data);
+                sprintf(Send_data,"%s%s%4.1f",Dyna_data,Thermo_data,CO2_data);
                 printf("Send_data : %s\n",Send_data);
                 myled4=0;           
                 break;
@@ -318,4 +328,10 @@
                 myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); break;
         }
     }
-}
\ No newline at end of file
+}
+
+/* CoolTermのSend stringを使う場合 Hex : 16進数で値を送信可能 なので
+    home_position 40
+    reset         80
+    end           c0
+*/
\ No newline at end of file