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@2:95955f38f47a, 2014-05-28 (annotated)

Committer:
m5171135
Date:
Wed May 28 11:54:22 2014 +0000
Revision:
2:95955f38f47a
Parent:
1:490b793b2e61
Child:
3:1229ca3df855
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m5171135 2:95955f38f47a 1 /**********************************************/
m5171135 2:95955f38f47a 2 //
m5171135 2:95955f38f47a 3 //
m5171135 2:95955f38f47a 4 // Program name: Aigamozu Robot Control
m5171135 2:95955f38f47a 5 // author: Atsunori Maruyama
m5171135 2:95955f38f47a 6 //
m5171135 2:95955f38f47a 7 //
m5171135 2:95955f38f47a 8 // ver -> 1.0
m5171135 2:95955f38f47a 9 //
m5171135 2:95955f38f47a 10 //
m5171135 2:95955f38f47a 11 /**********************************************/
m5171135 2:95955f38f47a 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 1:490b793b2e61 16 #include "VNH5019.h"
m5171135 2:95955f38f47a 17 #include "agzIDLIST.h"
m5171135 2:95955f38f47a 18
m5171135 2:95955f38f47a 19
m5171135 2:95955f38f47a 20 #define GPS_BAUD_RATE 9600
m5171135 2:95955f38f47a 21 #define XBEE_BAUD_RATE 57600
m5171135 2:95955f38f47a 22 #define PC_BAUD_RATE 57600
m5171135 2:95955f38f47a 23 #define IMU_BAUD_RATE 400000
m5171135 1:490b793b2e61 24
m5171135 0:7d55d6ace996 25 /////////////////////////////////////////
m5171135 0:7d55d6ace996 26 //
m5171135 0:7d55d6ace996 27 //Connection Setting
m5171135 0:7d55d6ace996 28 //
m5171135 0:7d55d6ace996 29 /////////////////////////////////////////
m5171135 2:95955f38f47a 30
m5171135 2:95955f38f47a 31 //Serial Connect Setting: PC <--> mbed
m5171135 2:95955f38f47a 32 Serial pc(USBTX, USBRX);
m5171135 2:95955f38f47a 33
m5171135 2:95955f38f47a 34 //Serial Connect Setting: GPS <--> mbed
m5171135 0:7d55d6ace996 35 Serial * gps_Serial;
m5171135 2:95955f38f47a 36
m5171135 2:95955f38f47a 37 //Serial Connect Setting: XBEE <--> mbed
m5171135 2:95955f38f47a 38 XBee xbee(p13,p14);
m5171135 0:7d55d6ace996 39 ZBRxResponse zbRx = ZBRxResponse();
m5171135 2:95955f38f47a 40 XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L);
m5171135 0:7d55d6ace996 41
m5171135 0:7d55d6ace996 42 /////////////////////////////////////////
m5171135 0:7d55d6ace996 43 //
m5171135 0:7d55d6ace996 44 //Pin Setting
m5171135 0:7d55d6ace996 45 //
m5171135 0:7d55d6ace996 46 /////////////////////////////////////////
m5171135 0:7d55d6ace996 47
m5171135 2:95955f38f47a 48 //Motor Contorol Pin Setting
m5171135 1:490b793b2e61 49 VNH5019 motorShield(p21,p22,p23,p24,p25,p26);
m5171135 0:7d55d6ace996 50
m5171135 2:95955f38f47a 51 //Interrupt Setting
m5171135 0:7d55d6ace996 52 Ticker axis;
m5171135 0:7d55d6ace996 53 Ticker auth_axis;
m5171135 0:7d55d6ace996 54
m5171135 0:7d55d6ace996 55 /////////////////////////////////////////
m5171135 0:7d55d6ace996 56 //
m5171135 0:7d55d6ace996 57 //Each Value Setting
m5171135 0:7d55d6ace996 58 //
m5171135 0:7d55d6ace996 59 /////////////////////////////////////////
m5171135 2:95955f38f47a 60 int count = 0;
m5171135 2:95955f38f47a 61
m5171135 0:7d55d6ace996 62 //my status
m5171135 0:7d55d6ace996 63 //0: StndbyMode
m5171135 0:7d55d6ace996 64 //1: ManualMode
m5171135 0:7d55d6ace996 65 //2: AuthmaticMode(Random)
m5171135 0:7d55d6ace996 66 unsigned char my_status = 0;
m5171135 0:7d55d6ace996 67
m5171135 0:7d55d6ace996 68 //0 bit: Motor Satatus
m5171135 0:7d55d6ace996 69 //1 bit: GPS Status
m5171135 0:7d55d6ace996 70 //2 bit: Sensor Status
m5171135 0:7d55d6ace996 71 //3 bit: Battery Status
m5171135 0:7d55d6ace996 72 unsigned char my_mode = 0;
m5171135 0:7d55d6ace996 73
m5171135 0:7d55d6ace996 74 //ManualValue
m5171135 0:7d55d6ace996 75 int manual_count = 0;
m5171135 0:7d55d6ace996 76 int manual_flag = 0;
m5171135 0:7d55d6ace996 77
m5171135 0:7d55d6ace996 78 //test value
m5171135 0:7d55d6ace996 79 long sub_latH = 12345;
m5171135 0:7d55d6ace996 80 long sub_latL = 67890;
m5171135 0:7d55d6ace996 81 long sub_lonH = 98765;
m5171135 0:7d55d6ace996 82 long sub_lonL = 43211;
m5171135 0:7d55d6ace996 83
m5171135 0:7d55d6ace996 84
m5171135 0:7d55d6ace996 85 union UNI_TEST_T
m5171135 0:7d55d6ace996 86 {
m5171135 0:7d55d6ace996 87 long a;
m5171135 0:7d55d6ace996 88 uint8_t b[4];
m5171135 0:7d55d6ace996 89 };
m5171135 0:7d55d6ace996 90
m5171135 0:7d55d6ace996 91 UNI_TEST_T latH,latL,lonH,lonL;
m5171135 0:7d55d6ace996 92
m5171135 2:95955f38f47a 93
m5171135 0:7d55d6ace996 94 /////////////////////////////////////////
m5171135 0:7d55d6ace996 95 //
m5171135 0:7d55d6ace996 96 //Interruption processing 1: time -> 0.1s
m5171135 0:7d55d6ace996 97 //
m5171135 0:7d55d6ace996 98 /////////////////////////////////////////
m5171135 0:7d55d6ace996 99 void axisRenovation(){
m5171135 0:7d55d6ace996 100
m5171135 0:7d55d6ace996 101 if(manual_count > 100){
m5171135 1:490b793b2e61 102 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 103 manual_count = 0;
m5171135 0:7d55d6ace996 104 }
m5171135 2:95955f38f47a 105 if(my_mode == 1) manual_count++;
m5171135 0:7d55d6ace996 106 }
m5171135 0:7d55d6ace996 107
m5171135 0:7d55d6ace996 108 /////////////////////////////////////////
m5171135 0:7d55d6ace996 109 //
m5171135 0:7d55d6ace996 110 //Interruption processing: time -> 1.0s
m5171135 0:7d55d6ace996 111 //
m5171135 0:7d55d6ace996 112 /////////////////////////////////////////
m5171135 0:7d55d6ace996 113
m5171135 0:7d55d6ace996 114 void randomRenovation(){
m5171135 0:7d55d6ace996 115
m5171135 2:95955f38f47a 116 if(count < 20){
m5171135 1:490b793b2e61 117 motorShield.changeSpeed(1,127,1,127);
m5171135 0:7d55d6ace996 118 }
m5171135 0:7d55d6ace996 119
m5171135 0:7d55d6ace996 120 else{
m5171135 2:95955f38f47a 121 motorShield.changeSpeed(1,64,2,64);
m5171135 2:95955f38f47a 122 if(count > 21) {
m5171135 2:95955f38f47a 123
m5171135 2:95955f38f47a 124 count = 0;
m5171135 2:95955f38f47a 125 motorShield.changeSpeed(1,127,1,127);
m5171135 2:95955f38f47a 126
m5171135 2:95955f38f47a 127 }
m5171135 0:7d55d6ace996 128 }
m5171135 0:7d55d6ace996 129 count++;
m5171135 0:7d55d6ace996 130 }
m5171135 0:7d55d6ace996 131
m5171135 0:7d55d6ace996 132
m5171135 0:7d55d6ace996 133
m5171135 0:7d55d6ace996 134 /////////////////////////////////////////
m5171135 0:7d55d6ace996 135 //
m5171135 0:7d55d6ace996 136 //Main Processing
m5171135 0:7d55d6ace996 137 //
m5171135 0:7d55d6ace996 138 /////////////////////////////////////////
m5171135 0:7d55d6ace996 139 int main() {
m5171135 0:7d55d6ace996 140 //start up time
m5171135 0:7d55d6ace996 141 wait(3);
m5171135 0:7d55d6ace996 142 //set pc frequency to 57600bps
m5171135 2:95955f38f47a 143 pc.baud(PC_BAUD_RATE);
m5171135 0:7d55d6ace996 144 //set xbee frequency to 57600bps
m5171135 2:95955f38f47a 145 xbee.begin(XBEE_BAUD_RATE);
m5171135 0:7d55d6ace996 146
m5171135 0:7d55d6ace996 147 //GPS setting
m5171135 2:95955f38f47a 148 gps_Serial = new Serial(p28,p27);
m5171135 2:95955f38f47a 149 Adafruit_GPS myGPS(gps_Serial);
m5171135 2:95955f38f47a 150 Timer refresh_Timer;
m5171135 0:7d55d6ace996 151 const int refresh_Time = 2000; //refresh time in ms
m5171135 2:95955f38f47a 152 myGPS.begin(GPS_BAUD_RATE);
m5171135 0:7d55d6ace996 153
m5171135 2:95955f38f47a 154 //GPS Send Command
m5171135 2:95955f38f47a 155 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
m5171135 0:7d55d6ace996 156 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
m5171135 0:7d55d6ace996 157 myGPS.sendCommand(PGCMD_ANTENNA);
m5171135 0:7d55d6ace996 158
m5171135 0:7d55d6ace996 159 //gyro_registor Setting
m5171135 0:7d55d6ace996 160 wait(2);
m5171135 0:7d55d6ace996 161
m5171135 0:7d55d6ace996 162 //interrupt start
m5171135 0:7d55d6ace996 163 axis.attach(&axisRenovation, 0.1);
m5171135 0:7d55d6ace996 164 refresh_Timer.start();
m5171135 0:7d55d6ace996 165
m5171135 2:95955f38f47a 166 /// @todo change to true
m5171135 2:95955f38f47a 167 while (true) {
m5171135 0:7d55d6ace996 168
m5171135 0:7d55d6ace996 169 //recive xbee module
m5171135 0:7d55d6ace996 170 xbee.readPacket();
m5171135 0:7d55d6ace996 171 if (xbee.getResponse().isAvailable()) {
m5171135 0:7d55d6ace996 172 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
m5171135 0:7d55d6ace996 173 xbee.getResponse().getZBRxResponse(zbRx);
m5171135 0:7d55d6ace996 174 unsigned char *buf = zbRx.getFrameData();
m5171135 0:7d55d6ace996 175
m5171135 2:95955f38f47a 176 /// @todo use preporcessor (#define) or const.
m5171135 2:95955f38f47a 177 /// @note It's magic number.
m5171135 2:95955f38f47a 178 // COMMAND_TYPE commandType = buf[14];
m5171135 2:95955f38f47a 179 /// switch(commandType)
m5171135 2:95955f38f47a 180 /// {case MANUAL:}
m5171135 2:95955f38f47a 181
m5171135 2:95955f38f47a 182
m5171135 2:95955f38f47a 183 //switch(aigamoCTR.checkCommandType(buf)){
m5171135 0:7d55d6ace996 184 switch((unsigned char)buf[14]){
m5171135 0:7d55d6ace996 185 //ChanegeMode
m5171135 0:7d55d6ace996 186 case 'C':{
m5171135 0:7d55d6ace996 187 my_mode = buf[19];
m5171135 1:490b793b2e61 188 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 189 if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0);
m5171135 0:7d55d6ace996 190 else auth_axis.detach();
m5171135 0:7d55d6ace996 191 break;
m5171135 0:7d55d6ace996 192 }
m5171135 0:7d55d6ace996 193
m5171135 2:95955f38f47a 194 /*
m5171135 2:95955f38f47a 195 case 'C':
m5171135 2:95955f38f47a 196 agz.change_Mode();
m5171135 2:95955f38f47a 197 break;
m5171135 2:95955f38f47a 198 */
m5171135 2:95955f38f47a 199
m5171135 0:7d55d6ace996 200 //MunualCommand
m5171135 0:7d55d6ace996 201 case 'M':{
m5171135 2:95955f38f47a 202 /// @todo Magic.
m5171135 0:7d55d6ace996 203 if(my_mode == 1){
m5171135 0:7d55d6ace996 204 manual_count = 0;
m5171135 1:490b793b2e61 205 motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 0:7d55d6ace996 206 }
m5171135 0:7d55d6ace996 207 break;
m5171135 0:7d55d6ace996 208 }
m5171135 0:7d55d6ace996 209
m5171135 2:95955f38f47a 210 /*
m5171135 2:95955f38f47a 211 case 'M':
m5171135 2:95955f38f47a 212 uint8_t sppeds_buf[4] = agz.parseMunualCommand(buf);
m5171135 2:95955f38f47a 213 motorShield.changeSpeed(sppedbuf[0],sppedbuf[1],sppedbuf[2],sppedbuf[3]);
m5171135 2:95955f38f47a 214 break;
m5171135 2:95955f38f47a 215 */
m5171135 2:95955f38f47a 216
m5171135 0:7d55d6ace996 217 //SendStatus
m5171135 0:7d55d6ace996 218 case 'S':{
m5171135 2:95955f38f47a 219 latH.a = myGPS.latitudeH;;
m5171135 2:95955f38f47a 220 latL.a = myGPS.latitudeL;
m5171135 2:95955f38f47a 221 lonH.a = myGPS.longitudeH;
m5171135 2:95955f38f47a 222 lonL.a = myGPS.longitudeL;
m5171135 0:7d55d6ace996 223 //dummy data
m5171135 2:95955f38f47a 224 //latH.a = sub_latH;latL.a = sub_latL;lonH.a = sub_lonH;lonL.a = sub_lonL;
m5171135 2:95955f38f47a 225
m5171135 2:95955f38f47a 226 // if(validateCommand(commandType));
m5171135 2:95955f38f47a 227 // createReceiveStatusData();
m5171135 2:95955f38f47a 228 uint8_t data[] = {65,71,83,82,70,'2',84,'A',83,my_status,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69};
m5171135 0:7d55d6ace996 229 ZBTxRequest tx64request(remoteAddress,data,sizeof(data));
m5171135 0:7d55d6ace996 230 xbee.send(tx64request);
m5171135 2:95955f38f47a 231 break;
m5171135 2:95955f38f47a 232 /*
m5171135 2:95955f38f47a 233 case 'S':
m5171135 2:95955f38f47a 234 */
m5171135 0:7d55d6ace996 235 }
m5171135 0:7d55d6ace996 236
m5171135 0:7d55d6ace996 237 default:
m5171135 0:7d55d6ace996 238 {
m5171135 0:7d55d6ace996 239 break;
m5171135 0:7d55d6ace996 240 }
m5171135 0:7d55d6ace996 241 }
m5171135 0:7d55d6ace996 242 }
m5171135 0:7d55d6ace996 243 }
m5171135 0:7d55d6ace996 244
m5171135 0:7d55d6ace996 245 myGPS.read();
m5171135 0:7d55d6ace996 246 //recive gps module
m5171135 0:7d55d6ace996 247 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 248 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 249 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 250 continue;
m5171135 0:7d55d6ace996 251 }
m5171135 0:7d55d6ace996 252 }
m5171135 0:7d55d6ace996 253
m5171135 0:7d55d6ace996 254 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 255 refresh_Timer.reset();
m5171135 2:95955f38f47a 256 if (myGPS.fix) my_status = 0;
m5171135 0:7d55d6ace996 257 else my_status = 1;
m5171135 0:7d55d6ace996 258 }
m5171135 0:7d55d6ace996 259 }
m5171135 0:7d55d6ace996 260 }