Diff: main.cpp

Revision:

3:6a932d0aac7d

Parent:

2:f0efe1d10f8a

Child:

4:d9ea010641ab

--- a/main.cpp	Sat Apr 25 03:36:16 2015 +0000
+++ b/main.cpp	Sat Apr 25 06:08:18 2015 +0000
@@ -54,8 +54,8 @@
 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;
+        case 1: linear.SetTorqueLimit(1); linear_goal-=2; break;
+        case 2: linear.SetTorqueLimit(1); linear_goal+=2; break;
     }
     if(linear_goal>linear_MAX) linear_goal=linear_MAX; 
     if(linear_goal<linear_min) linear_goal=linear_min;
@@ -63,8 +63,8 @@
     
     switch(pitch_mode){
         case 0: break;
-        case 1: pitch_goal-=5; break;
-        case 2: pitch_goal+=5; break;
+        case 1: pitch_goal-=3; break;
+        case 2: pitch_goal+=3; break;
     }    
     if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
     if(pitch_goal<pitch_min) pitch_goal=pitch_min;
@@ -72,8 +72,8 @@
     
     switch(yaw_mode){
         case 0: break;
-        case 1: yaw_goal-=5; break;
-        case 2: yaw_goal+=5; break;
+        case 1: yaw_goal-=3; break;
+        case 2: yaw_goal+=3; break;
     }
     if(pitch_goal>2520) pitch_goal=2520; 
     if(pitch_goal<-2520) pitch_goal=-2520;
@@ -126,18 +126,27 @@
 #define  D6T_addr  0x14
 #define  D6T_cmd   0x4c
 
-#define THERMO_DEBUG 0
+#define THERMO_DEBUG 1
+
+char   I2C_rd1[64]; // 生データ
+short  datr1[16]; // 16点 温度データ（10倍整数）
+short  PTAT1; // センサ内部PTAT温度データ（10倍整数）
+double dt1[16]; // 16点 温度データ
+double dt1_temp[16];
+short  d_PTAT1; // センサ内部PTAT温度データ
+
+char   I2C_rd2[64]; // 生データ
+short  datr2[16]; // 16点 温度データ（10倍整数）
+short  PTAT2; // センサ内部PTAT温度データ（10倍整数）
+double dt2[16]; // 16点 温度データ
+double dt2_temp[16];
+short  d_PTAT2; // センサ内部PTAT温度データ
 
 void GetThermo(char* data) {
-    char con[10];
+    char con[6];
+    sprintf(data,"");
     
     /*MEMS1*/
-    char  I2C_rd1[64]; // 生データ
-    short  datr1[16]; // 16点 温度データ（10倍整数）
-    short  PTAT1; // センサ内部PTAT温度データ（10倍整数）
-    double   dt1[16]; // 16点 温度データ
-    short   d_PTAT1; // センサ内部PTAT温度データ
-
     int  i,j;
     int  itemp;
     
@@ -158,11 +167,15 @@
         dt1[i] = 0.1 * datr1[i];
     }
     for(int i=0;i<16;i++){
-        sprintf(con,"%3.1f ",dt1[i]);
+        if(dt1[i]<50 && dt1[i]>5) {
+            sprintf(con,"%3.1f ",dt1[i]);
+            dt1_temp[i]=dt1[i];
+        }
+        else sprintf(con,"%3.1f ",dt1_temp[i]);
         strcat(data,con);
     }
     if(THERMO_DEBUG){
-        printf("Thermal_Sensor 1");
+        printf("\nThermal_Sensor 1");
         for(i=0;i<16;i++){
             if(i%4==0) printf("\n");
             printf("%3.1f ",dt1[i]);
@@ -172,13 +185,7 @@
     wait(0.1);
     /*MEMS1*/
         
-    /*MEMS2*/
-    char  I2C_rd2[64]; // 生データ
-    short  datr2[16]; // 16点 温度データ（10倍整数）
-    short  PTAT2; // センサ内部PTAT温度データ（10倍整数）
-    double   dt2[16]; // 16点 温度データ
-    short   d_PTAT2; // センサ内部PTAT温度データ
-    
+    /*MEMS2*/  
     //// measure
     MEMS2.start();
     MEMS2.write(D6T_addr);
@@ -196,11 +203,15 @@
         dt2[i] = 0.1 * datr2[i];
     }
     for(int i=0;i<16;i++){
-        sprintf(con,"%3.1f ",dt2[i]);
+        if(dt1[i]<50 && dt1[i]>5) {
+            sprintf(con,"%3.1f ",dt2[i]);
+            dt2_temp[i]=dt2[i];
+        }
+        else sprintf(con,"%3.1f ",dt2_temp[i]);
         strcat(data,con);
     }
     if(THERMO_DEBUG){
-        printf("Thermal_Sensor 2");
+        printf("\nThermal_Sensor 2");
         for(i=0;i<16;i++){
             if(i%4==0) printf("\n");
             printf("%3.1f ",dt2[i]);
@@ -212,7 +223,6 @@
 }
 /*--Thermal_Sensor:end------------------------------------------------------------------------------------*/
 
-
 /*--CO2_Sensor:begin--------------------------------------------------------------------------------------*/
 /*CO2センサモジュール：A051020-AQ6B-01*/
 /*データシート：http://www.fisinc.co.jp/common/pdf/A051020-AQ6.pdf*/
@@ -221,7 +231,7 @@
 
 AnalogIn ain(p20);
 
-#define CO2_DEBUG 0
+#define CO2_DEBUG 1
 
 float GetCO2() {
     float v; //生データ：電圧
@@ -243,12 +253,13 @@
 ----------------*/
 
 int main() {
-    char Dyna_data[100];
+    char Dyna_data[50];
+    char Thermo_data[200];
     float CO2_data;
     
     Dyna_init();
     
-    char Send_data[200];
+    char Send_data[250];
     int Read_data;
     int Joy_mode, linear_mode, pitch_mode, yaw_mode;
     
@@ -281,7 +292,6 @@
             /*--Thermal_Sensor:begin-----------------------------------------------------------*/
                 //値を取得
                 myled3=1;
-                char Thermo_data[100];
                 GetThermo(Thermo_data);
             /*--Thermal_Sensor:end-------------------------------------------------------------*/
         
@@ -294,7 +304,7 @@
             //値を送信
                 myled4=1;
                 sprintf(Send_data,"%s%s%f",Dyna_data,Thermo_data,CO2_data);
-                printf("%s\n",Send_data);
+                printf("Send_data : %s\n",Send_data);
                 myled4=0;           
                 break;