aigamozu / Mbed 2 deprecated aigamozu_auto_ver1

auto

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common

main.cpp@0:d8f3aa3b6876, 2015-04-11 (annotated)

Committer:
kityann
Date:
Sat Apr 11 11:57:26 2015 +0000
Revision:
0:d8f3aa3b6876
Child:
1:a5f98c7e1feb
auto program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:d8f3aa3b6876 1 /**********************************************/
kityann 0:d8f3aa3b6876 2 //
kityann 0:d8f3aa3b6876 3 //
kityann 0:d8f3aa3b6876 4 //
kityann 0:d8f3aa3b6876 5 // Program name: Aigamozu Robot Control
kityann 0:d8f3aa3b6876 6 // Author: Atsunori Maruyama
kityann 0:d8f3aa3b6876 7 // Ver -> 1.3
kityann 0:d8f3aa3b6876 8 // Day -> 2014/06/09
kityann 0:d8f3aa3b6876 9 //
kityann 0:d8f3aa3b6876 10 //
kityann 0:d8f3aa3b6876 11 /**********************************************/
kityann 0:d8f3aa3b6876 12
kityann 0:d8f3aa3b6876 13 #include "mbed.h"
kityann 0:d8f3aa3b6876 14 #include "XBee.h"
kityann 0:d8f3aa3b6876 15 #include "MBed_Adafruit_GPS.h"
kityann 0:d8f3aa3b6876 16 #include "AigamozuControlPackets.h"
kityann 0:d8f3aa3b6876 17 #include "agzIDLIST.h"
kityann 0:d8f3aa3b6876 18 #include "aigamozuSetting.h"
kityann 0:d8f3aa3b6876 19
kityann 0:d8f3aa3b6876 20 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 21 //
kityann 0:d8f3aa3b6876 22 //Connection Setting
kityann 0:d8f3aa3b6876 23 //
kityann 0:d8f3aa3b6876 24 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 25
kityann 0:d8f3aa3b6876 26 //Serial Connect Setting: PC <--> mbed
kityann 0:d8f3aa3b6876 27 Serial pc(USBTX, USBRX);
kityann 0:d8f3aa3b6876 28
kityann 0:d8f3aa3b6876 29 //Serial Connect Setting: GPS <--> mbed
kityann 0:d8f3aa3b6876 30 Serial * gps_Serial;
kityann 0:d8f3aa3b6876 31
kityann 0:d8f3aa3b6876 32 //Serial Connect Setting: XBEE <--> mbed
kityann 0:d8f3aa3b6876 33 XBee xbee(p13,p14);
kityann 0:d8f3aa3b6876 34 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:d8f3aa3b6876 35 XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L);
kityann 0:d8f3aa3b6876 36
kityann 0:d8f3aa3b6876 37 //Timer
kityann 0:d8f3aa3b6876 38 Timer t;
kityann 0:d8f3aa3b6876 39 int flag=0;
kityann 0:d8f3aa3b6876 40
kityann 0:d8f3aa3b6876 41 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 42 //
kityann 0:d8f3aa3b6876 43 //Pin Setting
kityann 0:d8f3aa3b6876 44 //
kityann 0:d8f3aa3b6876 45 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 46 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:d8f3aa3b6876 47
kityann 0:d8f3aa3b6876 48 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 49 //
kityann 0:d8f3aa3b6876 50 //Main Processing
kityann 0:d8f3aa3b6876 51 //
kityann 0:d8f3aa3b6876 52 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 53 int main() {
kityann 0:d8f3aa3b6876 54 int count = 0;
kityann 0:d8f3aa3b6876 55 //start up time
kityann 0:d8f3aa3b6876 56 wait(3);
kityann 0:d8f3aa3b6876 57 //set pc frequency to 57600bps
kityann 0:d8f3aa3b6876 58 pc.baud(PC_BAUD_RATE);
kityann 0:d8f3aa3b6876 59 //set xbee frequency to 57600bps
kityann 0:d8f3aa3b6876 60 xbee.begin(XBEE_BAUD_RATE);
kityann 0:d8f3aa3b6876 61
kityann 0:d8f3aa3b6876 62 //GPS setting
kityann 0:d8f3aa3b6876 63 gps_Serial = new Serial(p28,p27);
kityann 0:d8f3aa3b6876 64 Adafruit_GPS myGPS(gps_Serial);
kityann 0:d8f3aa3b6876 65 Timer refresh_Timer;
kityann 0:d8f3aa3b6876 66 const int refresh_Time = 2000; //refresh time in ms
kityann 0:d8f3aa3b6876 67 myGPS.begin(GPS_BAUD_RATE);
kityann 0:d8f3aa3b6876 68
kityann 0:d8f3aa3b6876 69 //GPS Send Command
kityann 0:d8f3aa3b6876 70 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:d8f3aa3b6876 71 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:d8f3aa3b6876 72 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:d8f3aa3b6876 73
kityann 0:d8f3aa3b6876 74 wait(2);
kityann 0:d8f3aa3b6876 75
kityann 0:d8f3aa3b6876 76 //interrupt start
kityann 0:d8f3aa3b6876 77 AigamozuControlPackets agz(agz_motorShield);
kityann 0:d8f3aa3b6876 78 refresh_Timer.start();
kityann 0:d8f3aa3b6876 79
kityann 0:d8f3aa3b6876 80
kityann 0:d8f3aa3b6876 81 while (true) {
kityann 0:d8f3aa3b6876 82
kityann 0:d8f3aa3b6876 83 //Check Xbee Buffer Available
kityann 0:d8f3aa3b6876 84 xbee.readPacket();
kityann 0:d8f3aa3b6876 85 if (xbee.getResponse().isAvailable()) {
kityann 0:d8f3aa3b6876 86 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:d8f3aa3b6876 87 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:d8f3aa3b6876 88 uint8_t *buf = zbRx.getFrameData();
kityann 0:d8f3aa3b6876 89
kityann 0:d8f3aa3b6876 90
kityann 0:d8f3aa3b6876 91 //Check Command Type
kityann 0:d8f3aa3b6876 92 switch(agz.checkCommnadType(buf)){
kityann 0:d8f3aa3b6876 93
kityann 0:d8f3aa3b6876 94 //CommandType -> ChanegeMode
kityann 0:d8f3aa3b6876 95 case CHANGE_MODE :{
kityann 0:d8f3aa3b6876 96 agz.changeMode(buf);
kityann 0:d8f3aa3b6876 97 break;
kityann 0:d8f3aa3b6876 98 }
kityann 0:d8f3aa3b6876 99
kityann 0:d8f3aa3b6876 100 //CommandType -> Manual
kityann 0:d8f3aa3b6876 101 case MANUAL:{
kityann 0:d8f3aa3b6876 102 //Check now Mode
kityann 0:d8f3aa3b6876 103 if(agz.nowMode == MANUAL_MODE){
kityann 0:d8f3aa3b6876 104 agz.changeSpeed(buf);
kityann 0:d8f3aa3b6876 105 }
kityann 0:d8f3aa3b6876 106 break;
kityann 0:d8f3aa3b6876 107 }
kityann 0:d8f3aa3b6876 108
kityann 0:d8f3aa3b6876 109 //CommandType -> Send Status
kityann 0:d8f3aa3b6876 110 case STATUS_REQUEST:{
kityann 0:d8f3aa3b6876 111 //Create GPS Infomation Packet
kityann 0:d8f3aa3b6876 112 agz.createReceiveStatusCommand(1,6,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:d8f3aa3b6876 113 //Select Destination
kityann 0:d8f3aa3b6876 114 ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength());
kityann 0:d8f3aa3b6876 115 //Send -> Base
kityann 0:d8f3aa3b6876 116 xbee.send(tx64request);
kityann 0:d8f3aa3b6876 117 break;
kityann 0:d8f3aa3b6876 118
kityann 0:d8f3aa3b6876 119 }
kityann 0:d8f3aa3b6876 120
kityann 0:d8f3aa3b6876 121 default:
kityann 0:d8f3aa3b6876 122 {
kityann 0:d8f3aa3b6876 123 break;
kityann 0:d8f3aa3b6876 124 }
kityann 0:d8f3aa3b6876 125 }
kityann 0:d8f3aa3b6876 126 }
kityann 0:d8f3aa3b6876 127 }
kityann 0:d8f3aa3b6876 128
kityann 0:d8f3aa3b6876 129 myGPS.read();
kityann 0:d8f3aa3b6876 130 //recive gps module
kityann 0:d8f3aa3b6876 131 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:d8f3aa3b6876 132 if ( myGPS.newNMEAreceived() ) {
kityann 0:d8f3aa3b6876 133 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:d8f3aa3b6876 134 continue;
kityann 0:d8f3aa3b6876 135 }
kityann 0:d8f3aa3b6876 136 }
kityann 0:d8f3aa3b6876 137
kityann 0:d8f3aa3b6876 138 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:d8f3aa3b6876 139 refresh_Timer.reset();
kityann 0:d8f3aa3b6876 140 if (myGPS.fix) {
kityann 0:d8f3aa3b6876 141 agz.nowStatus = GPS_AVAIL;
kityann 0:d8f3aa3b6876 142 agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:d8f3aa3b6876 143 }
kityann 0:d8f3aa3b6876 144 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:d8f3aa3b6876 145 }
kityann 0:d8f3aa3b6876 146
kityann 0:d8f3aa3b6876 147
kityann 0:d8f3aa3b6876 148
kityann 0:d8f3aa3b6876 149
kityann 0:d8f3aa3b6876 150 }
kityann 0:d8f3aa3b6876 151 }