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@5:940424ec98a8, 2014-06-08 (annotated)

Committer:
m5171135
Date:
Sun Jun 08 11:55:07 2014 +0000
Revision:
5:940424ec98a8
Parent:
4:39d2aadf3068
Child:
6:21e2792e66c7
berfore

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 1:490b793b2e61 17 #include "VNH5019.h"
m5171135 2:95955f38f47a 18 #include "agzIDLIST.h"
m5171135 3:1229ca3df855 19 #include "aigamozuSetting.h"
m5171135 1:490b793b2e61 20
m5171135 0:7d55d6ace996 21 /////////////////////////////////////////
m5171135 0:7d55d6ace996 22 //
m5171135 0:7d55d6ace996 23 //Connection Setting
m5171135 0:7d55d6ace996 24 //
m5171135 0:7d55d6ace996 25 /////////////////////////////////////////
m5171135 4:39d2aadf3068 26
m5171135 2:95955f38f47a 27 //Serial Connect Setting: PC <--> mbed
m5171135 2:95955f38f47a 28 Serial pc(USBTX, USBRX);
m5171135 2:95955f38f47a 29
m5171135 2:95955f38f47a 30 //Serial Connect Setting: GPS <--> mbed
m5171135 0:7d55d6ace996 31 Serial * gps_Serial;
m5171135 2:95955f38f47a 32
m5171135 2:95955f38f47a 33 //Serial Connect Setting: XBEE <--> mbed
m5171135 2:95955f38f47a 34 XBee xbee(p13,p14);
m5171135 0:7d55d6ace996 35 ZBRxResponse zbRx = ZBRxResponse();
m5171135 2:95955f38f47a 36 XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L);
m5171135 0:7d55d6ace996 37
m5171135 0:7d55d6ace996 38 /////////////////////////////////////////
m5171135 0:7d55d6ace996 39 //
m5171135 0:7d55d6ace996 40 //Pin Setting
m5171135 0:7d55d6ace996 41 //
m5171135 0:7d55d6ace996 42 /////////////////////////////////////////
m5171135 4:39d2aadf3068 43
m5171135 2:95955f38f47a 44 //Motor Contorol Pin Setting
m5171135 1:490b793b2e61 45 VNH5019 motorShield(p21,p22,p23,p24,p25,p26);
m5171135 5:940424ec98a8 46
m5171135 3:1229ca3df855 47
m5171135 2:95955f38f47a 48 //Interrupt Setting
m5171135 0:7d55d6ace996 49 Ticker axis;
m5171135 0:7d55d6ace996 50 Ticker auth_axis;
m5171135 0:7d55d6ace996 51
m5171135 0:7d55d6ace996 52 /////////////////////////////////////////
m5171135 0:7d55d6ace996 53 //
m5171135 0:7d55d6ace996 54 //Each Value Setting
m5171135 0:7d55d6ace996 55 //
m5171135 0:7d55d6ace996 56 /////////////////////////////////////////
m5171135 2:95955f38f47a 57 int count = 0;
m5171135 2:95955f38f47a 58
m5171135 0:7d55d6ace996 59 //my status
m5171135 0:7d55d6ace996 60 //0: StndbyMode
m5171135 0:7d55d6ace996 61 //1: ManualMode
m5171135 0:7d55d6ace996 62 //2: AuthmaticMode(Random)
m5171135 0:7d55d6ace996 63 unsigned char my_status = 0;
m5171135 0:7d55d6ace996 64
m5171135 0:7d55d6ace996 65 //0 bit: Motor Satatus
m5171135 0:7d55d6ace996 66 //1 bit: GPS Status
m5171135 0:7d55d6ace996 67 //2 bit: Sensor Status
m5171135 0:7d55d6ace996 68 //3 bit: Battery Status
m5171135 0:7d55d6ace996 69 unsigned char my_mode = 0;
m5171135 0:7d55d6ace996 70
m5171135 0:7d55d6ace996 71 //ManualValue
m5171135 0:7d55d6ace996 72 int manual_count = 0;
m5171135 0:7d55d6ace996 73 int manual_flag = 0;
m5171135 0:7d55d6ace996 74
m5171135 0:7d55d6ace996 75 /////////////////////////////////////////
m5171135 0:7d55d6ace996 76 //
m5171135 0:7d55d6ace996 77 //Interruption processing 1: time -> 0.1s
m5171135 0:7d55d6ace996 78 //
m5171135 0:7d55d6ace996 79 /////////////////////////////////////////
m5171135 0:7d55d6ace996 80 void axisRenovation(){
m5171135 0:7d55d6ace996 81
m5171135 0:7d55d6ace996 82 if(manual_count > 100){
m5171135 1:490b793b2e61 83 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 84 manual_count = 0;
m5171135 0:7d55d6ace996 85 }
m5171135 2:95955f38f47a 86 if(my_mode == 1) manual_count++;
m5171135 0:7d55d6ace996 87 }
m5171135 0:7d55d6ace996 88
m5171135 0:7d55d6ace996 89 /////////////////////////////////////////
m5171135 0:7d55d6ace996 90 //
m5171135 0:7d55d6ace996 91 //Interruption processing: time -> 1.0s
m5171135 0:7d55d6ace996 92 //
m5171135 0:7d55d6ace996 93 /////////////////////////////////////////
m5171135 0:7d55d6ace996 94
m5171135 5:940424ec98a8 95 void normalRandomMode(){
m5171135 0:7d55d6ace996 96
m5171135 2:95955f38f47a 97 if(count < 20){
m5171135 1:490b793b2e61 98 motorShield.changeSpeed(1,127,1,127);
m5171135 0:7d55d6ace996 99 }
m5171135 0:7d55d6ace996 100 else{
m5171135 2:95955f38f47a 101 motorShield.changeSpeed(1,64,2,64);
m5171135 2:95955f38f47a 102 if(count > 21) {
m5171135 2:95955f38f47a 103 count = 0;
m5171135 2:95955f38f47a 104 motorShield.changeSpeed(1,127,1,127);
m5171135 2:95955f38f47a 105 }
m5171135 0:7d55d6ace996 106 }
m5171135 0:7d55d6ace996 107 count++;
m5171135 5:940424ec98a8 108 }
m5171135 5:940424ec98a8 109
m5171135 5:940424ec98a8 110 void gpsRandomMode(){
m5171135 5:940424ec98a8 111
m5171135 5:940424ec98a8 112
m5171135 5:940424ec98a8 113
m5171135 5:940424ec98a8 114
m5171135 0:7d55d6ace996 115 }
m5171135 0:7d55d6ace996 116
m5171135 4:39d2aadf3068 117
m5171135 4:39d2aadf3068 118
m5171135 5:940424ec98a8 119
m5171135 5:940424ec98a8 120
m5171135 0:7d55d6ace996 121 /////////////////////////////////////////
m5171135 0:7d55d6ace996 122 //
m5171135 0:7d55d6ace996 123 //Main Processing
m5171135 0:7d55d6ace996 124 //
m5171135 0:7d55d6ace996 125 /////////////////////////////////////////
m5171135 0:7d55d6ace996 126 int main() {
m5171135 0:7d55d6ace996 127 //start up time
m5171135 0:7d55d6ace996 128 wait(3);
m5171135 0:7d55d6ace996 129 //set pc frequency to 57600bps
m5171135 2:95955f38f47a 130 pc.baud(PC_BAUD_RATE);
m5171135 0:7d55d6ace996 131 //set xbee frequency to 57600bps
m5171135 2:95955f38f47a 132 xbee.begin(XBEE_BAUD_RATE);
m5171135 0:7d55d6ace996 133
m5171135 0:7d55d6ace996 134 //GPS setting
m5171135 2:95955f38f47a 135 gps_Serial = new Serial(p28,p27);
m5171135 2:95955f38f47a 136 Adafruit_GPS myGPS(gps_Serial);
m5171135 2:95955f38f47a 137 Timer refresh_Timer;
m5171135 0:7d55d6ace996 138 const int refresh_Time = 2000; //refresh time in ms
m5171135 2:95955f38f47a 139 myGPS.begin(GPS_BAUD_RATE);
m5171135 0:7d55d6ace996 140
m5171135 2:95955f38f47a 141 //GPS Send Command
m5171135 2:95955f38f47a 142 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
m5171135 0:7d55d6ace996 143 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
m5171135 0:7d55d6ace996 144 myGPS.sendCommand(PGCMD_ANTENNA);
m5171135 0:7d55d6ace996 145
m5171135 0:7d55d6ace996 146 wait(2);
m5171135 0:7d55d6ace996 147
m5171135 0:7d55d6ace996 148 //interrupt start
m5171135 0:7d55d6ace996 149 axis.attach(&axisRenovation, 0.1);
m5171135 5:940424ec98a8 150 AigamozuControlPackets agz(axis);
m5171135 0:7d55d6ace996 151 refresh_Timer.start();
m5171135 4:39d2aadf3068 152
m5171135 4:39d2aadf3068 153
m5171135 2:95955f38f47a 154 while (true) {
m5171135 0:7d55d6ace996 155
m5171135 4:39d2aadf3068 156 //Check Xbee Buffer Available
m5171135 0:7d55d6ace996 157 xbee.readPacket();
m5171135 0:7d55d6ace996 158 if (xbee.getResponse().isAvailable()) {
m5171135 0:7d55d6ace996 159 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
m5171135 0:7d55d6ace996 160 xbee.getResponse().getZBRxResponse(zbRx);
m5171135 5:940424ec98a8 161 uint8_t *buf = zbRx.getFrameData();
m5171135 0:7d55d6ace996 162
m5171135 2:95955f38f47a 163
m5171135 4:39d2aadf3068 164 //Check Command Type
m5171135 3:1229ca3df855 165 switch(agz.checkCommnadType(buf)){
m5171135 4:39d2aadf3068 166 //CommandType -> ChanegeMode
m5171135 4:39d2aadf3068 167 case CHANGE_MODE :{
m5171135 5:940424ec98a8 168 //Change Mode
m5171135 5:940424ec98a8 169 agz.changeMode(buf);
m5171135 5:940424ec98a8 170 //Motor Stop
m5171135 1:490b793b2e61 171 motorShield.changeSpeed(0,0,0,0);
m5171135 5:940424ec98a8 172 //case: AUTO1
m5171135 5:940424ec98a8 173 if(agz.nowMode == AUTO_MODE) auth_axis.attach(&normalRandomMode, 1.0);
m5171135 5:940424ec98a8 174 else if(agz.nowMode ==AUTO_GPS_MODE) auth_axis.attach(&gpsRandomMode, 1.0);
m5171135 0:7d55d6ace996 175 else auth_axis.detach();
m5171135 5:940424ec98a8 176
m5171135 5:940424ec98a8 177 //case:AUTO2
m5171135 5:940424ec98a8 178
m5171135 5:940424ec98a8 179
m5171135 5:940424ec98a8 180
m5171135 0:7d55d6ace996 181 break;
m5171135 0:7d55d6ace996 182 }
m5171135 4:39d2aadf3068 183
m5171135 4:39d2aadf3068 184 //CommandType -> Manual
m5171135 3:1229ca3df855 185 case MANUAL:{
m5171135 4:39d2aadf3068 186 //Check now Mode
m5171135 4:39d2aadf3068 187 if(agz.nowMode == MANUAL_MODE){
m5171135 4:39d2aadf3068 188 //manual_count = 0;
m5171135 4:39d2aadf3068 189 //Change Motor Behavior
m5171135 4:39d2aadf3068 190 motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 0:7d55d6ace996 191 }
m5171135 0:7d55d6ace996 192 break;
m5171135 0:7d55d6ace996 193 }
m5171135 0:7d55d6ace996 194
m5171135 4:39d2aadf3068 195 //CommandType -> Send Status
m5171135 3:1229ca3df855 196 case STATUS_REQUEST:{
m5171135 4:39d2aadf3068 197 //Create GPS Infomation Packet
m5171135 5:940424ec98a8 198 agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
m5171135 4:39d2aadf3068 199 //Select Destination
m5171135 5:940424ec98a8 200 ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength());
m5171135 5:940424ec98a8 201
m5171135 4:39d2aadf3068 202 //Send -> Base
m5171135 0:7d55d6ace996 203 xbee.send(tx64request);
m5171135 4:39d2aadf3068 204 break;
m5171135 4:39d2aadf3068 205
m5171135 0:7d55d6ace996 206 }
m5171135 4:39d2aadf3068 207
m5171135 0:7d55d6ace996 208 default:
m5171135 0:7d55d6ace996 209 {
m5171135 0:7d55d6ace996 210 break;
m5171135 0:7d55d6ace996 211 }
m5171135 0:7d55d6ace996 212 }
m5171135 0:7d55d6ace996 213 }
m5171135 0:7d55d6ace996 214 }
m5171135 0:7d55d6ace996 215
m5171135 0:7d55d6ace996 216 myGPS.read();
m5171135 0:7d55d6ace996 217 //recive gps module
m5171135 0:7d55d6ace996 218 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 219 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 220 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 221 continue;
m5171135 0:7d55d6ace996 222 }
m5171135 0:7d55d6ace996 223 }
m5171135 0:7d55d6ace996 224
m5171135 0:7d55d6ace996 225 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 226 refresh_Timer.reset();
m5171135 4:39d2aadf3068 227 if (myGPS.fix) agz.nowStatus = GPS_AVAIL;
m5171135 4:39d2aadf3068 228 else agz.nowStatus = GPS_UNAVAIL;
m5171135 0:7d55d6ace996 229 }
m5171135 0:7d55d6ace996 230 }
m5171135 0:7d55d6ace996 231 }