Diff: main.cpp

Revision:

10:ffe65600fba2

Parent:

9:65c76b58e5b7

Child:

11:4ed9e37e5ffd

--- a/main.cpp	Wed Apr 08 08:16:14 2015 +0000
+++ b/main.cpp	Thu Apr 09 04:24:45 2015 +0000
@@ -5,10 +5,7 @@
 
 #include "mbed.h"
 
-DigitalOut myled1(LED1);
-DigitalOut myled2(LED2);
-DigitalOut myled3(LED3);
-DigitalOut myled4(LED4);
+DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4);
 
 /*--Ethernet:begin------------------------------------------------------------------------------------------*/
 #include "mbed.h"
@@ -43,18 +40,16 @@
 #include "MX28.h"
 
 AX12 linear (p13, p14, 0);      //直動Dynamixel
-//MX28 pitch (p9, p10, 1);        //ピッチDynamixel
-MX28 pitch (p13, p14, 1);        //ピッチDynamixel
-//MX28 yaw (p28, p27, 2);         //ヨーDynamixel
+MX28 pitch (p13, p14, 1);       //ピッチDynamixel
 MX28 yaw (p13, p14, 2);         //ヨーDynamixel
 
 int linear_goal, pitch_goal, yaw_goal;
 
-//最小値、最大値、初期値を指定[unit:degree]
-//MX:MultiTurnモードでは0°～5040°
+//最小値，最大値，角速度，初期値を指定[unit:degree]
+//MX:MultiTurnモードでは-2520°～2520°(SetGoalの書き換え不要)
 int linear_min = 100; int linear_MAX = 720; float linear_Speed=0.1; int linear_Init = linear_MAX;
 int pitch_min = 172; int pitch_MAX = 334; float pitch_Speed=0.2; int pitch_Init = pitch_MAX;
-int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0;
+int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0; //MultiTurnモード
 
 void Dyna_init() {
     linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); 
@@ -98,7 +93,7 @@
 /*データシート：http://www.omron.co.jp/ecb/products/sensor/special/mems/pdf/AN-D6T-01JP_r2.pdf*/
 #include "MEMS.h"
 
-MEMS MEMS1(p9, p10); // sda, scl
+MEMS MEMS1(I2C_sda, p10); // sda, scl
 MEMS MEMS2(p28, p27); // sda, scl
 
 float ThD1[16], ThD2[16];
@@ -111,7 +106,6 @@
 /*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm] ※このセンサで測れるのは5400[ppm]まで*/
 #include "mbed.h"
 
-Serial pc(USBTX, USBRX); // tx, rx
 AnalogIn ain(p20);
 
 float CO2_Sensor()
@@ -147,7 +141,7 @@
         
         sock.receiveFrom(echo_server, R_data, sizeof(R_data)); 
         wait_ms(1);
-        Joy_mode = atoi(R_data);
+        Joy_mode = atoi(R_data[0] & 0x03);
         
         switch(Joy_mode){
             case 0: //通常モード
@@ -160,9 +154,9 @@
                 myled1=0;
                                   
                 //JoyStickのモード
-                linear_mode = Joy_mode && 0x0c; linear_mode = linear_mode >> 2;
-                pitch_mode = Joy_mode && 0x30; pitch_mode = linear_mode >> 4;
-                yaw_mode = Joy_mode && 0xc0; yaw_mode = linear_mode >> 6;                
+                linear_mode = R_data[0] & 0x0c; linear_mode = linear_mode >> 2;
+                pitch_mode = R_data[0] & 0x30; pitch_mode = linear_mode >> 4;
+                yaw_mode = R_data[0] & 0xc0; yaw_mode = linear_mode >> 6;                
 
                 //目標角度へ動かす
                 myled2=1;
@@ -202,6 +196,7 @@
             /*--CO2_Sensor:end-----------------------------------------------------------------*/
             
             //値を送信
+                myled3=1;
                 sprintf(Thermo_data,
                 "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
                 ThD1[0],ThD1[1],ThD1[2],ThD1[3],ThD1[4],ThD1[5],ThD1[6],ThD1[7],
@@ -210,7 +205,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));
-            
+                myled3=0;           
                 break;
                         
             
@@ -221,7 +216,7 @@
                 Dyna_reset(); break;
             
             case 3: //終了
-                Dyna_end(); return 0;
+                myled4=1; Dyna_end(); sock.close(true); eth.disconnect(); return 0;
         }
     }
 }
\ No newline at end of file