Diff: main.cpp

Revision:

5:a6d9a1dec391

Parent:

4:dd8c3543adc7

Child:

6:fc49395cd7e5

--- a/main.cpp	Fri Apr 17 02:39:00 2015 +0000
+++ b/main.cpp	Tue Apr 21 16:08:58 2015 +0000
@@ -14,9 +14,9 @@
 
 DigitalOut low(p19); DigitalOut RelaySwitch(p17);
 
-AX12 linear (p13, p14, 0);      //直動Dynamixel
-MX28 pitch (p13, p14, 1);       //ピッチDynamixel
-MX28 yaw (p13, p14, 2);         //ヨーDynamixel
+AX12 linear (p13, p14, 0, 1000000);      //直動Dynamixel
+MX28 pitch (p13, p14, 1, 1000000);       //ピッチDynamixel
+MX28 yaw (p13, p14, 2, 1000000);         //ヨーDynamixel
 
 int linear_goal, pitch_goal, yaw_goal;
 
@@ -30,14 +30,14 @@
     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); 
+    pitch.SetCWLimit(pitch_min); pitch.SetCCWLimit(pitch_MAX); 
     pitch.SetCRSpeed(pitch_Speed); pitch_goal=linear_Init; pitch.SetGoal(pitch_Init);
-    yaw.SetCWLimit(linear_min); yaw.SetCCWLimit(linear_MAX); 
+    yaw.SetCWLimit(yaw_min); yaw.SetCCWLimit(yaw_MAX); 
     yaw.SetCRSpeed(yaw_Speed); yaw_goal=yaw_Init; yaw.SetGoal(yaw_Init);
     wait(1);
 }
 
-void GetDyna(char* data) {
+void Dyna_GetData(char* data) {
     float lP, lV, pP, pC, yP, yC;
     
     lP = linear.GetPosition(); lV = linear.GetVolts();
@@ -49,22 +49,22 @@
     sprintf(data,"%f %f %f %f %f %f ",lP,lV,pP,pC,yP,yC);  
 }
 
-void SetGoalDyna(int l_mode, int p_mode, int y_mode) {
-    switch(l_mode){
+void Dyna_SetGoal(int linear_mode, int pitch_mode, int yaw_mode) {
+    switch(linear_mode){
         case 0: break;
         case 1: linear.SetTorqueLimit(1); linear_goal++; break;
         case 2: linear.SetTorqueLimit(1); linear_goal--; break;
-    }    
+    }
     linear.SetGoal(linear_goal);
     
-    switch(p_mode){
+    switch(pitch_mode){
         case 0: break;
         case 1: pitch_goal++; break;
         case 2: pitch_goal--; break;
     }    
     pitch.SetGoal(pitch_goal);
     
-    switch(y_mode){
+    switch(yaw_mode){
         case 0: break;
         case 1: yaw_goal++; break;
         case 2: yaw_goal--; break;
@@ -102,6 +102,8 @@
 MEMS MEMS1(p9, p10); // sda, scl
 MEMS MEMS2(p28, p27); // sda, scl
 
+#define THERMO_DEBUG 0
+
 void GetThermo(char* data) {
     float ThD1[16], ThD2[16];
     char con[10];
@@ -113,10 +115,25 @@
         sprintf(con,"%3.1f ",ThD1[i]);
         strcat(data,con);
     }
+    
+    if(THERMO_DEBUG) {
+        for(int i=0;i<16;i++){
+            if(i%4==0) printf("\n");
+            printf("%3.1f ",ThD1[i]);
+        }
+    }
+    
     for(int i=0;i<16;i++){
         sprintf(con,"%3.1f ",ThD2[i]);
         strcat(data,con);
     }
+    
+    if(THERMO_DEBUG) {
+        for(int i=0;i<16;i++){
+            if(i%4==0) printf("\n");
+            printf("%3.1f ",ThD2[i]);
+        }
+    }
 }
 /*--Thermal_Sensor:end------------------------------------------------------------------------------------*/
 
@@ -129,6 +146,8 @@
 
 AnalogIn ain(p20);
 
+#define CO2_DEBUG 0
+
 float GetCO2() {
     float v; //生データ：電圧
     float sensor_v,CO2;
@@ -136,6 +155,8 @@
     v = ain.read()*3.3;
     sensor_v = v * 5.0/3.3; //電圧レベルを合わせる
     CO2 = sensor_v * 1000 + 400;  //データシートより
+    
+    if(CO2_DEBUG) printf("\n%4.1f",CO2);
 
     return(CO2);
 }
@@ -168,36 +189,38 @@
             /*--Dynamixel:begin---------------------------------------------------------------*/       
                 //Dynamixelへの命令を判定
                 linear_mode = R_data & 0x0c; linear_mode = linear_mode >> 2;
-                pitch_mode = R_data & 0x30; pitch_mode = linear_mode >> 4;
-                yaw_mode = R_data & 0xc0; yaw_mode = linear_mode >> 6;                
+                pitch_mode = R_data & 0x30; pitch_mode = pitch_mode >> 4;
+                yaw_mode = R_data & 0xc0; yaw_mode = yaw_mode >> 6;                
 
                 //現在の角度・電圧・電流を取得
                 myled1=1;
-                GetDyna(Dyna_data);
+                Dyna_GetData(Dyna_data);
                 myled1=0;
                 
                 //目標角度を変更
                 myled2=1;
-                SetGoalDyna(linear_mode, pitch_mode, yaw_mode);
+                Dyna_SetGoal(linear_mode, pitch_mode, yaw_mode);
                 myled2=0;
             /*--Dynamixel:end------------------------------------------------------------------*/       
         
             /*--Thermal_Sensor:begin-----------------------------------------------------------*/
                 //値を取得
+                myled3=1;
                 GetThermo(Thermo_data);
             /*--Thermal_Sensor:end-------------------------------------------------------------*/
         
             /*--CO2_Sensor:begin---------------------------------------------------------------*/
                 //値を取得
                 CO2_data=GetCO2();
+                myled3=0;
             /*--CO2_Sensor:end-----------------------------------------------------------------*/
             
             //値を送信
-                myled3=1;
-                sprintf(S_data, "%s%s%f",Dyna_data,Thermo_data,CO2_data);
+                myled4=1;
+                sprintf(S_data,"%s%s%f",Dyna_data,Thermo_data,CO2_data);
 //                sock.sendTo(echo_server, S_data, sizeof(S_data));
-                pc.printf("%s\n",S_data);
-                myled3=0;           
+                printf("%s\n",S_data);
+                myled4=0;           
                 break;
             
             case 1: //ホームポジション
@@ -207,7 +230,7 @@
                 Dyna_reset(); break;
             
             case 3: //終了
-                myled4=1; Dyna_end(); return 0;
+                myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); return 0;
         }
     }
 }
\ No newline at end of file