![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2016_05_19 Auto mode: 10sec forward, 2sec stop, 2sec turn right Please change test Auto pwm
Dependencies: ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST_2016 mbed
Fork of Aigamozu_Robot_ver4 by
Diff: main.cpp
- Revision:
- 26:835042870f81
- Parent:
- 25:ae5fab0946f2
- Child:
- 27:2861016eaac6
diff -r ae5fab0946f2 -r 835042870f81 main.cpp --- a/main.cpp Sun May 17 11:38:25 2015 +0000 +++ b/main.cpp Sun May 17 14:29:44 2015 +0000 @@ -46,7 +46,7 @@ //Base ID: 'a' ~ 'a' //manager ID: '0'(数字のゼロ) // -const char MyID = 'D'; +const char MyID = 'A'; //************ID Number***************** ///////////////////////////////////////// @@ -333,21 +333,20 @@ x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]); } - void get_sigma(){ - double temp[2][2]; - for(int i=0;i<2;i++) { - for(int j=0;j<2;j++) { - for(int k=0;k<2;k++) { - temp[i][j]+=K[1][i][k]*sigma[0][k][j]; - } - } - } - for(int i = 0;i < 2;i++){ - for(int j = 0;j < 2;j++){ - sigma[1][i][j] = sigma[0][i][j]-temp[i][j]; - } - } + double temp[2][2]={0}; + for(int i=0;i<2;i++) { + for(int j=0;j<2;j++) { + for(int k=0;k<2;k++) { + temp[i][j]+=K[1][i][k]*sigma[0][k][j]; + } + } + } + for(int i = 0;i < 2;i++){ + for(int j = 0;j < 2;j++){ + sigma[1][i][j] = sigma[0][i][j]-temp[i][j]; + } + } } void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){ @@ -410,7 +409,6 @@ Timer collect_Timer; const int collect_Time = 2000; //when we collect 4 GPS point, we use int collect_flag = 0; - char collect_state = 'a'; char SenderIDc; //GPS Send Command @@ -500,10 +498,8 @@ Send_Request_Base(collect_flag); collect_flag++; - collect_state++; if(collect_flag == 4){ - collect_state = 'a'; collect_flag = 0; } }