Diff: main.cpp

Revision:

2:f0efe1d10f8a

Parent:

1:bf4d524625f5

Child:

3:6a932d0aac7d

--- a/main.cpp	Fri Apr 24 17:14:48 2015 +0000
+++ b/main.cpp	Sat Apr 25 03:36:16 2015 +0000
@@ -63,12 +63,8 @@
     
     switch(pitch_mode){
         case 0: break;
-        case 1: pitch_goal-=5; 
-                 if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX;
-                 break;
-        case 2: pitch_goal+=5; 
-                 if(pitch_goal<pitch_min) pitch_goal=pitch_min;
-                 break;
+        case 1: pitch_goal-=5; break;
+        case 2: pitch_goal+=5; break;
     }    
     if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
     if(pitch_goal<pitch_min) pitch_goal=pitch_min;
@@ -103,7 +99,7 @@
     pitch.SetTorqueLimit(0);
     yaw.SetTorqueLimit(0);
     
-    wait_ms(10);
+    wait(0.1);
     
 //    RelaySwitch = 1;
     linear.SetTorqueLimit(1);
@@ -130,7 +126,7 @@
 #define  D6T_addr  0x14
 #define  D6T_cmd   0x4c
 
-#define THERMO_DEBUG 1
+#define THERMO_DEBUG 0
 
 void GetThermo(char* data) {
     char con[10];
@@ -165,6 +161,13 @@
         sprintf(con,"%3.1f ",dt1[i]);
         strcat(data,con);
     }
+    if(THERMO_DEBUG){
+        printf("Thermal_Sensor 1");
+        for(i=0;i<16;i++){
+            if(i%4==0) printf("\n");
+            printf("%3.1f ",dt1[i]);
+        } printf("\n");
+    }
     d_PTAT1 = 0.1 * PTAT1;
     wait(0.1);
     /*MEMS1*/
@@ -191,13 +194,18 @@
     }
     for(i=0;i<16;i++){
         dt2[i] = 0.1 * datr2[i];
-        if(i%4==0) printf("\n");
-        printf("%3.1f ",dt2[i]);    
     }
     for(int i=0;i<16;i++){
         sprintf(con,"%3.1f ",dt2[i]);
         strcat(data,con);
     }
+    if(THERMO_DEBUG){
+        printf("Thermal_Sensor 2");
+        for(i=0;i<16;i++){
+            if(i%4==0) printf("\n");
+            printf("%3.1f ",dt2[i]);
+        } printf("\n");
+    }
     d_PTAT2 = 0.1 * PTAT2;
     wait(0.1);
     /*MEMS2*/
@@ -213,7 +221,7 @@
 
 AnalogIn ain(p20);
 
-#define CO2_DEBUG 1
+#define CO2_DEBUG 0
 
 float GetCO2() {
     float v; //生データ：電圧
@@ -248,16 +256,16 @@
         
         Read_data=pc.getc();
         wait_ms(1);
-        Joy_mode = Read_data & 0x03; //ジョイスティックのモード判定
+        Joy_mode = Read_data & 0xc0; Joy_mode = Joy_mode >> 6; //ジョイスティックのモード判定
         
         switch(Joy_mode){
             case 0: //通常モード
                 myled1=0; myled2=0; myled3=0; myled4=0;
             /*--Dynamixel:begin---------------------------------------------------------------*/       
                 //Dynamixelへの命令を判定
-                linear_mode = Read_data & 0x0c; linear_mode = linear_mode >> 2;
-                pitch_mode = Read_data & 0x30; pitch_mode = pitch_mode >> 4;
-                yaw_mode = Read_data & 0xc0; yaw_mode = yaw_mode >> 6;                
+                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;                
 
                 //現在の角度・電圧・電流を取得
                 myled1=1;