aigamozu
20150511, Nakazawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of
aigamozu_auto_ver3_3_2
by 
aigamozu
Revision 7:209c07cc2b80, committed 2015-05-11
- Comitter:
- s1210160
- Date:
- Mon May 11 13:23:52 2015 +0000
- Parent:
- 6:11a8ae7ab71e
- Commit message:
- 20150511, Nakazawa
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Apr 26 16:52:54 2015 +0000
+++ b/main.cpp Mon May 11 13:23:52 2015 +0000
@@ -9,6 +9,16 @@
//
//
/**********************************************/
+//
+// 中澤遥菜
+// 変更点：メイン関数の中の処理を関数化しました。
+// setting(), interrupt_start(VNH5019 motorShield),
+// GPS_Setting(int refresh_Time, int collect_Time),
+// GPS_SendCommand(), check_CommandType(utf8_t command_type),
+// LatLong_afterFiltering(double longitude, double latitude, double x0, double x1),
+// send_GPSdata(), get_GPSdata(), getKalman()
+//
+/**********************************************/
#include "mbed.h"
#include "XBee.h"
@@ -106,65 +116,72 @@
get_sigma();
}
-
/////////////////////////////////////////
//
-//Main Processing
+//setting
//
/////////////////////////////////////////
-int main() {
- //start up time
+void setting(){
+ //start up time
wait(3);
//set pc frequency to 57600bps
pc.baud(PC_BAUD_RATE);
//set xbee frequency to 57600bps
xbee.begin(XBEE_BAUD_RATE);
+ }
+
+/////////////////////////////////////////
+//
+//interrupt start
+//
+/////////////////////////////////////////
+void interrupt_start(VNH5019 motorShield, Timer refresh){
+ AigamozuControlPackets agz(motorShield);
+ refresh.start();
- //GPS setting
+ printf("start\n");
+}
+
+/////////////////////////////////////////
+//
+//GPS Setting
+//
+/////////////////////////////////////////
+XBeeAddress64 collect_Address[5];
+XBeeAddress64 robot_Address
+
+void GPS_Setting(Timer refresh_Timer, Timer collect_Timer){
gps_Serial = new Serial(p28,p27);
Adafruit_GPS myGPS(gps_Serial);
Timer refresh_Timer;
- const int refresh_Time = 2000; //refresh time in ms
+ const int refresh_Time = refresh_Timer; //refresh time in ms
myGPS.begin(GPS_BAUD_RATE);
Timer collect_Timer;
- const int collect_Time = 200; //when we collect 4 GPS point, we use
- int collect_flag = 0;
- char collect_state = 'a';
- XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
+ const int collect_Time = collect_Timer; //when we collect 4 GPS point, we use
+ collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)};
- XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
- int id;
- bool flag = true;
-
- //GPS Send Command
+ robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
+ }
+
+/////////////////////////////////////////
+//
+//GPS Send Command
+//
+/////////////////////////////////////////
+void GPS_SendCommand(){
myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
myGPS.sendCommand(PGCMD_ANTENNA);
-
- wait(2);
-
- //interrupt start
- AigamozuControlPackets agz(agz_motorShield);
- refresh_Timer.start();
-
- printf("test\n");
-
+ }
- while (true) {
-
- //Check Xbee Buffer Available
- xbee.readPacket();
- if (xbee.getResponse().isAvailable()) {
- if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
- xbee.getResponse().getZBRxResponse(zbRx);
- uint8_t *buf = zbRx.getFrameData();
- uint8_t *buf1 = &buf[11];
- id = buf1[5] - 'A';
-
-
- //Check Command Type
- switch(agz.checkCommnadType(buf)){
+/////////////////////////////////////////
+//
+//Check Command Type
+//
+/////////////////////////////////////////
+void check_CommandType(utf8_t command_type, AigamozuControlPackets ai(motorShield), ){
+ switch(command_type){
//CommandType -> ChanegeMode
case CHANGE_MODE :{
@@ -183,26 +200,12 @@
//CommandType -> Send Status
case STATUS_REQUEST:{
+
//send normal data
//Create GPS Infomation Packet
agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
//Select Destination
- ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
- //Send -> Base
- xbee.send(tx64request);
-
- //send Kalman data
- agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
- //Select Destination
- ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
- //Send -> Base
- xbee.send(tx64request);
-
- //send normal data
- //Create GPS Infomation Packet
- agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
- //Select Destination
- ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength());
+ ZBTxRequest tx64request2(collect_Address[id],ai.packetData,ai.getPacketLength());
//Send -> Base
xbee.send(tx64request);
@@ -215,47 +218,109 @@
break;
}
-
- case RECEIVE_STATUS:{
-
- //printf("Receive Status\n");
-
- id = buf1[5] - 'A';
- robot[id].set_state(buf1[9]);
- robot[id].set_LatitudeH(&buf1[13]);
- robot[id].set_LatitudeL(&buf1[17]);
- robot[id].set_LongitudeH(&buf1[21]);
- robot[id].set_LongitudeL(&buf1[25]);
-
- agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
- /*
- printf("%d,", buf1[5]);
- printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
- */
- break;
- }
- case RECEIVE_KALMAN:{
- id = buf1[5] - 'A';
- robot[id].set_state(buf1[9]);
- robot[id].set_LatitudeKH(&buf1[13]);
- robot[id].set_LatitudeKL(&buf1[17]);
- robot[id].set_LongitudeKH(&buf1[21]);
- robot[id].set_LongitudeKL(&buf1[25]);
-
- agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL());
-
- break;
- }
+
default:{
break;
}
}
-
-
- }
}
+/////////////////////////////////////////
+//
+//latitude and longitude after filtering
+//
+/////////////////////////////////////////
+void LatLong_afterFiltering(double longitude, double latitude, double x0, double x1){
+ myGPS.longitudeKH=longitude;//longitude after filtering
+ myGPS.latitudeKH=latitude;//latitude after filtering
+ myGPS.longitudeKL=(long)x0;//longitude after filtering
+ myGPS.latitudeKL=(long)x1;//latitude after filtering
+}
+
+/////////////////////////////////////////
+//
+//send_GPSdata()
+//
+/////////////////////////////////////////
+void send_GPSdata(){
+ if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
+ xbee.getResponse().getZBRxResponse(zbRx);
+ uint8_t *buf = zbRx.getFrameData();
+ uint8_t *buf1 = &buf[11];
+ id = buf1[5] - 'A';+
+
+ //Check Command Type
+ check_CommandType(agz.checkCommnadType(buf));
+ }
+}
+
+/////////////////////////////////////////
+//
+//get_GPSdata
+//
+/////////////////////////////////////////
+void get_GPSdata(){
+ if(flag){
+ if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
+ flag = false;
+ x[0][0]=(double)myGPS.longitudeL;
+ x[0][1]=(double)myGPS.latitudeL;
+ }
+ }
+ if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
+}
+
+/////////////////////////////////////////
+//
+//get_Kalman
+//
+/////////////////////////////////////////
+void get_Kalman(){
+ //Kalman Filter
+ Kalman(myGPS.longitudeL,myGPS.latitudeL);
+ //kousinn
+ for(int i = 0;i < 2;i++){
+ for(int j = 0;j < 2;j++){
+ K[0][i][j]=K[1][i][j];
+ x[0][i]=x[1][i];
+ sigma[0][i][j]=sigma[1][i][j];
+ }
+ }
+
+ LatLong_afterFiltering(myGPS.longitudeH, myGPS.latitudeH, x[1][0], x[1][1]);
+ }
+/////////////////////////////////////////
+//
+//Main Processing
+//
+/////////////////////////////////////////
+int main() {
+ setting();
+
+ //GPS setting
+ GPS_Setting(2000, 200);
+ int collect_flag = 0;
+ char collect_state = 'a';
+ int id;
+ bool flag = true;
+
+ //GPS Send Command
+ GPS_SendCommand();
+
+ wait(2);
+
+ //interrupt start
+ interrupt_start(agz_motorShield, refreshTime);
+
+ while (true) {
+
+ //Check Xbee Buffer Available
+ xbee.readPacket();
+ if (xbee.getResponse().isAvailable()) {
+ send_GPSdata();
+ }
+
myGPS.read();
//recive gps module
//check if we recieved a new message from GPS, if so, attempt to parse it,
@@ -269,30 +334,9 @@
if (myGPS.fix) {
agz.nowStatus = GPS_AVAIL;
- if(flag){
- if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
- flag = false;
- x[0][0]=(double)myGPS.longitudeL;
- x[0][1]=(double)myGPS.latitudeL;
- }
- }
- if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
+ get_GPSdata();
- //Kalman Filter
- Kalman(myGPS.longitudeL,myGPS.latitudeL);
- //kousinn
- for(int i = 0;i < 2;i++){
- for(int j = 0;j < 2;j++){
- K[0][i][j]=K[1][i][j];
- x[0][i]=x[1][i];
- sigma[0][i][j]=sigma[1][i][j];
- }
- }
-
- myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering
- myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering
- myGPS.longitudeKL=(long)x[1][0];//longitude after filtering
- myGPS.latitudeKL=(long)x[1][1];//latitude after filtering
+ get_Kalman();
agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
@@ -303,26 +347,6 @@
}
-
- //get base GPS
- if( collect_Timer.read_ms() >= collect_Time){
- collect_Timer.reset();
-
- //printf("Send Request:%d,%d\n",collect_flag,collect_state);
-
- agz.createRequestCommand('A', collect_state);
- //Select Destination
- ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
- //Send -> Base
- xbee.send(tx64request);
-
- collect_flag++;
- collect_state++;
-
- if(collect_flag == 5){
- collect_state = 'a';
- collect_flag = 0;
- }
- }
+
}
}
\ No newline at end of file