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@3:1229ca3df855, 2014-05-30 (annotated)

Committer:
m5171135
Date:
Fri May 30 15:36:33 2014 +0000
Revision:
3:1229ca3df855
Parent:
2:95955f38f47a
Child:
4:39d2aadf3068
2014/5/30 Atsunori Maruyama; add library "AigamozuPacketControl"

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 3:1229ca3df855 7 // ver -> 1.0
m5171135 3:1229ca3df855 8 // day -> 2014/05/21
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 3:1229ca3df855 18 #include "aigamozuSetting.h"
m5171135 3:1229ca3df855 19 #include "AigamozuControlPackets.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 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 2:95955f38f47a 42 //Motor Contorol Pin Setting
m5171135 1:490b793b2e61 43 VNH5019 motorShield(p21,p22,p23,p24,p25,p26);
m5171135 3:1229ca3df855 44 AigamozuControlPackets agz;
m5171135 3:1229ca3df855 45
m5171135 0:7d55d6ace996 46
m5171135 2:95955f38f47a 47 //Interrupt Setting
m5171135 0:7d55d6ace996 48 Ticker axis;
m5171135 0:7d55d6ace996 49 Ticker auth_axis;
m5171135 0:7d55d6ace996 50
m5171135 0:7d55d6ace996 51 /////////////////////////////////////////
m5171135 0:7d55d6ace996 52 //
m5171135 0:7d55d6ace996 53 //Each Value Setting
m5171135 0:7d55d6ace996 54 //
m5171135 0:7d55d6ace996 55 /////////////////////////////////////////
m5171135 2:95955f38f47a 56 int count = 0;
m5171135 2:95955f38f47a 57
m5171135 0:7d55d6ace996 58 //my status
m5171135 0:7d55d6ace996 59 //0: StndbyMode
m5171135 0:7d55d6ace996 60 //1: ManualMode
m5171135 0:7d55d6ace996 61 //2: AuthmaticMode(Random)
m5171135 0:7d55d6ace996 62 unsigned char my_status = 0;
m5171135 0:7d55d6ace996 63
m5171135 0:7d55d6ace996 64 //0 bit: Motor Satatus
m5171135 0:7d55d6ace996 65 //1 bit: GPS Status
m5171135 0:7d55d6ace996 66 //2 bit: Sensor Status
m5171135 0:7d55d6ace996 67 //3 bit: Battery Status
m5171135 0:7d55d6ace996 68 unsigned char my_mode = 0;
m5171135 0:7d55d6ace996 69
m5171135 0:7d55d6ace996 70 //ManualValue
m5171135 0:7d55d6ace996 71 int manual_count = 0;
m5171135 0:7d55d6ace996 72 int manual_flag = 0;
m5171135 0:7d55d6ace996 73
m5171135 0:7d55d6ace996 74 /////////////////////////////////////////
m5171135 0:7d55d6ace996 75 //
m5171135 0:7d55d6ace996 76 //Interruption processing 1: time -> 0.1s
m5171135 0:7d55d6ace996 77 //
m5171135 0:7d55d6ace996 78 /////////////////////////////////////////
m5171135 0:7d55d6ace996 79 void axisRenovation(){
m5171135 0:7d55d6ace996 80
m5171135 0:7d55d6ace996 81 if(manual_count > 100){
m5171135 1:490b793b2e61 82 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 83 manual_count = 0;
m5171135 0:7d55d6ace996 84 }
m5171135 2:95955f38f47a 85 if(my_mode == 1) manual_count++;
m5171135 0:7d55d6ace996 86 }
m5171135 0:7d55d6ace996 87
m5171135 0:7d55d6ace996 88 /////////////////////////////////////////
m5171135 0:7d55d6ace996 89 //
m5171135 0:7d55d6ace996 90 //Interruption processing: time -> 1.0s
m5171135 0:7d55d6ace996 91 //
m5171135 0:7d55d6ace996 92 /////////////////////////////////////////
m5171135 0:7d55d6ace996 93
m5171135 0:7d55d6ace996 94 void randomRenovation(){
m5171135 0:7d55d6ace996 95
m5171135 2:95955f38f47a 96 if(count < 20){
m5171135 1:490b793b2e61 97 motorShield.changeSpeed(1,127,1,127);
m5171135 0:7d55d6ace996 98 }
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
m5171135 2:95955f38f47a 104 count = 0;
m5171135 2:95955f38f47a 105 motorShield.changeSpeed(1,127,1,127);
m5171135 2:95955f38f47a 106
m5171135 2:95955f38f47a 107 }
m5171135 0:7d55d6ace996 108 }
m5171135 0:7d55d6ace996 109 count++;
m5171135 0:7d55d6ace996 110 }
m5171135 0:7d55d6ace996 111
m5171135 0:7d55d6ace996 112 /////////////////////////////////////////
m5171135 0:7d55d6ace996 113 //
m5171135 0:7d55d6ace996 114 //Main Processing
m5171135 0:7d55d6ace996 115 //
m5171135 0:7d55d6ace996 116 /////////////////////////////////////////
m5171135 0:7d55d6ace996 117 int main() {
m5171135 0:7d55d6ace996 118 //start up time
m5171135 0:7d55d6ace996 119 wait(3);
m5171135 0:7d55d6ace996 120 //set pc frequency to 57600bps
m5171135 2:95955f38f47a 121 pc.baud(PC_BAUD_RATE);
m5171135 0:7d55d6ace996 122 //set xbee frequency to 57600bps
m5171135 2:95955f38f47a 123 xbee.begin(XBEE_BAUD_RATE);
m5171135 0:7d55d6ace996 124
m5171135 0:7d55d6ace996 125 //GPS setting
m5171135 2:95955f38f47a 126 gps_Serial = new Serial(p28,p27);
m5171135 2:95955f38f47a 127 Adafruit_GPS myGPS(gps_Serial);
m5171135 2:95955f38f47a 128 Timer refresh_Timer;
m5171135 0:7d55d6ace996 129 const int refresh_Time = 2000; //refresh time in ms
m5171135 2:95955f38f47a 130 myGPS.begin(GPS_BAUD_RATE);
m5171135 0:7d55d6ace996 131
m5171135 2:95955f38f47a 132 //GPS Send Command
m5171135 2:95955f38f47a 133 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
m5171135 0:7d55d6ace996 134 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
m5171135 0:7d55d6ace996 135 myGPS.sendCommand(PGCMD_ANTENNA);
m5171135 0:7d55d6ace996 136
m5171135 0:7d55d6ace996 137 //gyro_registor Setting
m5171135 0:7d55d6ace996 138 wait(2);
m5171135 0:7d55d6ace996 139
m5171135 0:7d55d6ace996 140 //interrupt start
m5171135 0:7d55d6ace996 141 axis.attach(&axisRenovation, 0.1);
m5171135 0:7d55d6ace996 142 refresh_Timer.start();
m5171135 3:1229ca3df855 143
m5171135 2:95955f38f47a 144 while (true) {
m5171135 0:7d55d6ace996 145
m5171135 0:7d55d6ace996 146 //recive xbee module
m5171135 0:7d55d6ace996 147 xbee.readPacket();
m5171135 0:7d55d6ace996 148 if (xbee.getResponse().isAvailable()) {
m5171135 0:7d55d6ace996 149 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
m5171135 0:7d55d6ace996 150 xbee.getResponse().getZBRxResponse(zbRx);
m5171135 0:7d55d6ace996 151 unsigned char *buf = zbRx.getFrameData();
m5171135 0:7d55d6ace996 152
m5171135 2:95955f38f47a 153
m5171135 3:1229ca3df855 154 //Analyze the command type
m5171135 3:1229ca3df855 155 switch(agz.checkCommnadType(buf)){
m5171135 3:1229ca3df855 156
m5171135 3:1229ca3df855 157 //CommnadType -> ChangeModeCommand;;;
m5171135 3:1229ca3df855 158 case CHANGE_MODE:{
m5171135 0:7d55d6ace996 159 my_mode = buf[19];
m5171135 1:490b793b2e61 160 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 161 if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0);
m5171135 0:7d55d6ace996 162 else auth_axis.detach();
m5171135 0:7d55d6ace996 163 break;
m5171135 0:7d55d6ace996 164 }
m5171135 0:7d55d6ace996 165
m5171135 3:1229ca3df855 166 //CommnadType -> ManualCommand;;;
m5171135 3:1229ca3df855 167 case MANUAL:{
m5171135 2:95955f38f47a 168 /// @todo Magic.
m5171135 0:7d55d6ace996 169 if(my_mode == 1){
m5171135 0:7d55d6ace996 170 manual_count = 0;
m5171135 1:490b793b2e61 171 motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 0:7d55d6ace996 172 }
m5171135 0:7d55d6ace996 173 break;
m5171135 0:7d55d6ace996 174 }
m5171135 0:7d55d6ace996 175
m5171135 0:7d55d6ace996 176 //SendStatus
m5171135 3:1229ca3df855 177 case STATUS_REQUEST:{
m5171135 3:1229ca3df855 178 //Create packet
m5171135 3:1229ca3df855 179 uint8_t* data = agz.createStatusPacket(myGPS.latitudeH, myGPS.latitudeL, myGPS.longitudeH, myGPS.longitudeL,1,2);
m5171135 0:7d55d6ace996 180 ZBTxRequest tx64request(remoteAddress,data,sizeof(data));
m5171135 3:1229ca3df855 181 //Send GPS ingomation
m5171135 0:7d55d6ace996 182 xbee.send(tx64request);
m5171135 3:1229ca3df855 183 break;
m5171135 0:7d55d6ace996 184 }
m5171135 3:1229ca3df855 185
m5171135 3:1229ca3df855 186 //Default
m5171135 0:7d55d6ace996 187 default:
m5171135 0:7d55d6ace996 188 {
m5171135 0:7d55d6ace996 189 break;
m5171135 0:7d55d6ace996 190 }
m5171135 0:7d55d6ace996 191 }
m5171135 0:7d55d6ace996 192 }
m5171135 0:7d55d6ace996 193 }
m5171135 3:1229ca3df855 194
m5171135 0:7d55d6ace996 195
m5171135 0:7d55d6ace996 196 myGPS.read();
m5171135 0:7d55d6ace996 197 //recive gps module
m5171135 0:7d55d6ace996 198 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 199 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 200 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 201 continue;
m5171135 0:7d55d6ace996 202 }
m5171135 0:7d55d6ace996 203 }
m5171135 0:7d55d6ace996 204
m5171135 0:7d55d6ace996 205 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 206 refresh_Timer.reset();
m5171135 2:95955f38f47a 207 if (myGPS.fix) my_status = 0;
m5171135 0:7d55d6ace996 208 else my_status = 1;
m5171135 0:7d55d6ace996 209 }
m5171135 0:7d55d6ace996 210 }
m5171135 0:7d55d6ace996 211 }