aigamozu / Mbed 2 deprecated aigamozu_program_ver2_yokokawa

yokokawa

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

Fork of aigamozu_program_ver2 by aigamozu

main.cpp@6:21e2792e66c7, 2014-06-08 (annotated)

Committer:
m5171135
Date:
Sun Jun 08 15:09:17 2014 +0000
Revision:
6:21e2792e66c7
Parent:
5:940424ec98a8
Child:
7:bfc65eac624e
ver 2.0

Who changed what in which revision?

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