Revision 10:b506d604d5f9, committed 2015-04-29

Comitter:

yusuke_kyo

Date:

Wed Apr 29 04:07:52 2015 +0000

Parent:

9:6f3963858d89

Commit message:

SetGoal add "if"

Changed in this revision

--- a/main.cpp	Tue Apr 28 06:16:12 2015 +0000
+++ b/main.cpp	Wed Apr 29 04:07:52 2015 +0000
@@ -41,7 +41,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");
+    if(DYNA_DEBUG) printf("\nDyna_init\n");  
 //    low = 0; RelaySwitch = 1;
     linear.SetTorqueLimit(0);
     pitch.SetTorqueLimit(0);
@@ -74,30 +74,33 @@
         case 1: linear.SetTorqueLimit(1); linear_goal-=10; break;
         case 2: linear.SetTorqueLimit(1); linear_goal+=20; break;
     }
-    
-    if(linear_goal<linear_min) linear_goal=linear_min;
-    if(linear_goal>linear_MAX) linear_goal=linear_MAX;
-    linear.SetGoal(linear_goal);
+    if(linear_mode!=0){
+        if(linear_goal<linear_min) linear_goal=linear_min;
+        if(linear_goal>linear_MAX) linear_goal=linear_MAX;
+        linear.SetGoal(linear_goal);
+    }
     
     switch(pitch_mode){
         case 0: break;
-        case 1: pitch_goal-=10; break;
-        case 2: pitch_goal+=10; break;
-    }    
-    
-    if(pitch_goal<pitch_min) pitch_goal=pitch_min;
-    if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
-    pitch.SetGoal(pitch_goal);
+        case 1: pitch.SetTorqueLimit(1); pitch_goal-=10; break;
+        case 2: pitch.SetTorqueLimit(1); pitch_goal+=10; break;
+    }
+    if(pitch_mode!=0){
+        if(pitch_goal<pitch_min) pitch_goal=pitch_min;
+        if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
+        pitch.SetGoal(pitch_goal);
+    }
     
     switch(yaw_mode){
         case 0: break;
         case 1: yaw_goal-=10; break;
         case 2: yaw_goal+=10; break;
     }
-
-    if(yaw_goal<-2520) yaw_goal=-2520;
-    if(yaw_goal>2520) yaw_goal=2520; 
-    yaw.SetGoal(yaw_goal);
+    if(yaw_mode!=0){
+        if(yaw_goal<-2520) yaw_goal=-2520;
+        if(yaw_goal>2520) yaw_goal=2520; 
+        yaw.SetGoal(yaw_goal);
+    }
 }
 
 void Dyna_home_position() {
@@ -226,7 +229,7 @@
         dt2[i] = 0.1 * datr2[i];
     }
     for(int i=0;i<16;i++){
-        if(dt1[i]<50 && dt1[i]>5) {
+        if(dt2[i]<50 && dt2[i]>5) {
             sprintf(con,"%3.1f ",dt2[i]);
             dt2_temp[i]=dt2[i];
         }
@@ -252,7 +255,7 @@
 /*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm] ※このセンサで測れるのは3000[ppm]まで*/
 #include "mbed.h"
 
-AnalogIn ain(p20);
+AnalogIn ain(p17);
 
 #define CO2_DEBUG 0
 
@@ -282,14 +285,14 @@
     
     char Dyna_data[50];
     char Thermo_data[200];
-    float CO2_data;
+    float CO2_data; 
     
     myled1=1; myled2=1; myled3=0; myled4=0;
     Dyna_init();
-    Dyna_home_position();
-    wait(2);
-    Dyna_home_position();
-    wait(2);
+//    Dyna_home_position();
+//    wait(2);
+//    Dyna_home_position();
+//    wait(2);
     myled1=0; myled2=0; myled3=1; myled4=0;
     
     union ArmPacket Read_data;
@@ -306,7 +309,7 @@
         //Joy_mode = Read_data & 0x03; //ジョイスティックのモード判定
         Joy_mode = Read_data.b.mode; //ジョイスティックのモード判定
         if(DEBUG) printf("Joy_mode : %d\n",Joy_mode);
-        
+
         switch(Joy_mode){
             case 0: //通常モード
                 myled1=0; myled2=0; myled3=0; myled4=0;
@@ -335,10 +338,7 @@
             case 3: //終了
                 myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); break;
         }
-                        //現在の角度・電圧・電流を取得
-                myled1=1;
-                Dyna_GetData(Dyna_data);
-                myled1=0;
+
             /*--Thermal_Sensor:begin-----------------------------------------------------------*/
                 //値を取得
                 myled3=1;
@@ -350,6 +350,11 @@
                 CO2_data=GetCO2();
                 myled3=0;
             /*--CO2_Sensor:end-----------------------------------------------------------------*/
+                
+                //現在の角度・電圧・電流を取得
+                myled1=1;
+                Dyna_GetData(Dyna_data);
+                myled1=0;            
             
             //値を送信
                 myled4=1;