YOZAKURA ARM program, usb serial
Dependencies: Dynamixel SerialHalfDuplex mbed
Diff: main.cpp
- Revision:
- 3:6a932d0aac7d
- Parent:
- 2:f0efe1d10f8a
- Child:
- 4:d9ea010641ab
diff -r f0efe1d10f8a -r 6a932d0aac7d main.cpp --- 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;