aigamozu
ver2
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of
aigamozu_auto_ver1
by 
aigamozu
Revision 1:a5f98c7e1feb, committed 2015-04-11
- Comitter:
- kityann
- Date:
- Sat Apr 11 12:25:34 2015 +0000
- Parent:
- 0:d8f3aa3b6876
- Child:
- 2:e9a8664b52ff
- Commit message:
- auto
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AigamozuControlPackets.lib Sat Apr 11 12:25:34 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#e77c664ee5e2
--- a/AigamozuMotorControl.lib Sat Apr 11 11:57:26 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#200ce5c1f486
--- a/agzIDLIST.lib Sat Apr 11 11:57:26 2015 +0000 +++ b/agzIDLIST.lib Sat Apr 11 12:25:34 2015 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/teams/aigamozu/code/agzIDLIST/#6d3d25a89e91 +http://mbed.org/teams/aigamozu/code/agzIDLIST/#f80f54b11696
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/agz_common.lib Sat Apr 11 12:25:34 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/aigamozu/code/agz_common/#54e62ef6d287
--- a/main.cpp Sat Apr 11 11:57:26 2015 +0000
+++ b/main.cpp Sat Apr 11 12:25:34 2015 +0000
@@ -16,6 +16,7 @@
#include "AigamozuControlPackets.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.h"
+#include "agz_common.h"
/////////////////////////////////////////
//
@@ -32,11 +33,9 @@
//Serial Connect Setting: XBEE <--> mbed
XBee xbee(p13,p14);
ZBRxResponse zbRx = ZBRxResponse();
-XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L);
+XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
-//Timer
-Timer t;
-int flag=0;
+AGZ_ROBOT robot[4];
/////////////////////////////////////////
//
@@ -51,7 +50,6 @@
//
/////////////////////////////////////////
int main() {
- int count = 0;
//start up time
wait(3);
//set pc frequency to 57600bps
@@ -66,6 +64,16 @@
const int refresh_Time = 2000; //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[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
+ XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
+ XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
+ int id;
+
+
//GPS Send Command
myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
@@ -77,16 +85,19 @@
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 *buf = zbRx.getFrameData();
+ uint8_t *buf1 = &buf[11];
+
//Check Command Type
switch(agz.checkCommnadType(buf)){
@@ -109,22 +120,40 @@
//CommandType -> Send Status
case STATUS_REQUEST:{
//Create GPS Infomation Packet
- agz.createReceiveStatusCommand(1,6,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+ agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
//Select Destination
- ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength());
+ ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
//Send -> Base
xbee.send(tx64request);
break;
}
+
+ case RECEIVE_STATUS:{
+
+ printf("Receive Status\n");
+
+ id = buf1[6] - '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]);
+
+ printf("%d\n", id);
+ printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
+ break;
+ }
- default:
- {
+ default:{
break;
- }
}
}
+
+
+ }
}
+
myGPS.read();
//recive gps module
@@ -134,18 +163,37 @@
continue;
}
}
-
if (refresh_Timer.read_ms() >= refresh_Time) {
refresh_Timer.reset();
if (myGPS.fix) {
agz.nowStatus = GPS_AVAIL;
agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+
+ printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
}
else agz.nowStatus = GPS_UNAVAIL;
+
}
-
-
+ //get base GPS
+ if( collect_Timer.read_ms() >= collect_Time){
+ collect_Timer.reset();
- }
+ printf("Send Request\n");
+
+ 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 == 4){
+ collect_state = 'a';
+ collect_flag = 0;
+ }
+ }
+ }
}
\ No newline at end of file
--- a/timer.bld Sat Apr 11 11:57:26 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/82220227f4fa \ No newline at end of file