Diff: main.cpp

Revision:

6:6d9dac9ff798

Parent:

5:581b55deaeb1

Child:

7:58eb1e1e9143

--- a/main.cpp	Sat Apr 25 06:43:37 2015 +0000
+++ b/main.cpp	Sun Apr 26 08:40:50 2015 +0000
@@ -1,6 +1,6 @@
 /*YOZAKURAのアームコード
   シリアル通信で送るデータ S_Data={linearD[4],pitchD[4],yawD[4],ThermoD1[16],ThermoD2[16],CO2[4]} 計48bytes
-  シリアル通信で受け取るデータ　R_Data=｛mode[2bit],linear_ref[2bit],pitch_ref[2bit],yaw_ref[2bit]} 計1byte
+  シリアル通信で受け取るデータ　R_Data=｛linear_ref[2bit],pitch_ref[2bit],yaw_ref[2bit]m,ode[2bit]} 計1byte
 */
 
 #include "mbed.h"
@@ -52,7 +52,7 @@
     yP = yaw.GetPosition(); yC = yaw.GetCurrent();
     
     linear_goal=lP; pitch_goal=pP; yaw_goal=yP; //現在角度を目標値に設定
-    sprintf(data,"%4.1f %4.1f %4.1f %4.1f %4.1f %4.1f ",lP,lV,pP,pC,yP,yC);  
+    sprintf(data,"%4.1f %4.1f %4.1f %4.1f %4.1f %4.1f ",lP,pP,yP,lV,pC,yC);  
 }
 
 void Dyna_SetGoal(int linear_mode, int pitch_mode, int yaw_mode) {
@@ -268,6 +268,9 @@
     
     Dyna_init();
     
+    unsigned char a = pc.getc();   
+    if(a==7) printf("arm\n");
+    
     char Send_data[250];
     int Read_data;
     int Joy_mode, linear_mode, pitch_mode, yaw_mode;
@@ -276,7 +279,7 @@
         
         Read_data=pc.getc();
         wait_ms(1);
-        Joy_mode = Read_data & 0xc0; Joy_mode = Joy_mode >> 6; //ジョイスティックのモード判定
+        Joy_mode = Read_data & 0x03;; //ジョイスティックのモード判定
         if(DEBUG) printf("Joy_mode : %d\n",Joy_mode);
         
         switch(Joy_mode){
@@ -284,9 +287,9 @@
                 myled1=0; myled2=0; myled3=0; myled4=0;
             /*--Dynamixel:begin---------------------------------------------------------------*/       
                 //Dynamixelへの命令を判定
-                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);
+                linear_mode = Read_data & 0xc0; linear_mode = linear_mode >> 6; if(DEBUG) printf("linear_mode : %d\n",linear_mode);
+                pitch_mode = Read_data & 0x30; pitch_mode = pitch_mode >> 4; if(DEBUG) printf("pitch_mode : %d\n",pitch_mode);
+                yaw_mode = Read_data & 0x0c; yaw_mode = yaw_mode >> 2; if(DEBUG) printf("yaw_mode : %d\n",yaw_mode);
 
                 //現在の角度・電圧・電流を取得
                 myled1=1;
@@ -314,7 +317,7 @@
             //値を送信
                 myled4=1;
                 sprintf(Send_data,"%s%s%4.1f",Dyna_data,Thermo_data,CO2_data);
-                printf("Send_data : %s\n",Send_data);
+                pc.printf("%s\n",Send_data);
                 myled4=0;           
                 break;