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@4:39d2aadf3068, 2014-06-04 (annotated)

Committer:
m5171135
Date:
Wed Jun 04 09:12:29 2014 +0000
Revision:
4:39d2aadf3068
Parent:
3:1229ca3df855
Child:
5:940424ec98a8
aaaa

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 2:95955f38f47a 8 //
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 3:1229ca3df855 46 AigamozuControlPackets agz;
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 0:7d55d6ace996 95 void randomRenovation(){
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
m5171135 0:7d55d6ace996 101 else{
m5171135 2:95955f38f47a 102 motorShield.changeSpeed(1,64,2,64);
m5171135 2:95955f38f47a 103 if(count > 21) {
m5171135 2:95955f38f47a 104
m5171135 2:95955f38f47a 105 count = 0;
m5171135 2:95955f38f47a 106 motorShield.changeSpeed(1,127,1,127);
m5171135 2:95955f38f47a 107
m5171135 2:95955f38f47a 108 }
m5171135 0:7d55d6ace996 109 }
m5171135 0:7d55d6ace996 110 count++;
m5171135 0:7d55d6ace996 111 }
m5171135 0:7d55d6ace996 112
m5171135 4:39d2aadf3068 113
m5171135 4:39d2aadf3068 114
m5171135 0:7d55d6ace996 115 /////////////////////////////////////////
m5171135 0:7d55d6ace996 116 //
m5171135 0:7d55d6ace996 117 //Main Processing
m5171135 0:7d55d6ace996 118 //
m5171135 0:7d55d6ace996 119 /////////////////////////////////////////
m5171135 0:7d55d6ace996 120 int main() {
m5171135 0:7d55d6ace996 121 //start up time
m5171135 0:7d55d6ace996 122 wait(3);
m5171135 0:7d55d6ace996 123 //set pc frequency to 57600bps
m5171135 2:95955f38f47a 124 pc.baud(PC_BAUD_RATE);
m5171135 0:7d55d6ace996 125 //set xbee frequency to 57600bps
m5171135 2:95955f38f47a 126 xbee.begin(XBEE_BAUD_RATE);
m5171135 0:7d55d6ace996 127
m5171135 0:7d55d6ace996 128 //GPS setting
m5171135 2:95955f38f47a 129 gps_Serial = new Serial(p28,p27);
m5171135 2:95955f38f47a 130 Adafruit_GPS myGPS(gps_Serial);
m5171135 2:95955f38f47a 131 Timer refresh_Timer;
m5171135 0:7d55d6ace996 132 const int refresh_Time = 2000; //refresh time in ms
m5171135 2:95955f38f47a 133 myGPS.begin(GPS_BAUD_RATE);
m5171135 0:7d55d6ace996 134
m5171135 2:95955f38f47a 135 //GPS Send Command
m5171135 2:95955f38f47a 136 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
m5171135 0:7d55d6ace996 137 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
m5171135 0:7d55d6ace996 138 myGPS.sendCommand(PGCMD_ANTENNA);
m5171135 0:7d55d6ace996 139
m5171135 0:7d55d6ace996 140 //gyro_registor Setting
m5171135 0:7d55d6ace996 141 wait(2);
m5171135 0:7d55d6ace996 142
m5171135 0:7d55d6ace996 143 //interrupt start
m5171135 0:7d55d6ace996 144 axis.attach(&axisRenovation, 0.1);
m5171135 0:7d55d6ace996 145 refresh_Timer.start();
m5171135 4:39d2aadf3068 146
m5171135 4:39d2aadf3068 147
m5171135 2:95955f38f47a 148 while (true) {
m5171135 0:7d55d6ace996 149
m5171135 4:39d2aadf3068 150 //Check Xbee Buffer Available
m5171135 0:7d55d6ace996 151 xbee.readPacket();
m5171135 0:7d55d6ace996 152 if (xbee.getResponse().isAvailable()) {
m5171135 0:7d55d6ace996 153 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
m5171135 0:7d55d6ace996 154 xbee.getResponse().getZBRxResponse(zbRx);
m5171135 0:7d55d6ace996 155 unsigned char *buf = zbRx.getFrameData();
m5171135 0:7d55d6ace996 156
m5171135 2:95955f38f47a 157
m5171135 4:39d2aadf3068 158 //Check Command Type
m5171135 3:1229ca3df855 159 switch(agz.checkCommnadType(buf)){
m5171135 4:39d2aadf3068 160 //CommandType -> ChanegeMode
m5171135 4:39d2aadf3068 161 case CHANGE_MODE :{
m5171135 0:7d55d6ace996 162 my_mode = buf[19];
m5171135 1:490b793b2e61 163 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 164 if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0);
m5171135 0:7d55d6ace996 165 else auth_axis.detach();
m5171135 0:7d55d6ace996 166 break;
m5171135 0:7d55d6ace996 167 }
m5171135 4:39d2aadf3068 168
m5171135 4:39d2aadf3068 169 //CommandType -> Manual
m5171135 3:1229ca3df855 170 case MANUAL:{
m5171135 4:39d2aadf3068 171 //Check now Mode
m5171135 4:39d2aadf3068 172 if(agz.nowMode == MANUAL_MODE){
m5171135 4:39d2aadf3068 173 //manual_count = 0;
m5171135 4:39d2aadf3068 174 //Change Motor Behavior
m5171135 4:39d2aadf3068 175 motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 0:7d55d6ace996 176 }
m5171135 0:7d55d6ace996 177 break;
m5171135 0:7d55d6ace996 178 }
m5171135 0:7d55d6ace996 179
m5171135 4:39d2aadf3068 180 //CommandType -> Send Status
m5171135 3:1229ca3df855 181 case STATUS_REQUEST:{
m5171135 4:39d2aadf3068 182 //Create GPS Infomation Packet
m5171135 4:39d2aadf3068 183 uint8_t* data = agz.createStatusPacket(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL,'B','a');
m5171135 4:39d2aadf3068 184 //Select Destination
m5171135 4:39d2aadf3068 185 ZBTxRequest tx64request(remoteAddress,data,RECEIVES_GPS_STATUS_LENGTH);
m5171135 4:39d2aadf3068 186 //Send -> Base
m5171135 0:7d55d6ace996 187 xbee.send(tx64request);
m5171135 4:39d2aadf3068 188 //Buffer Release
m5171135 4:39d2aadf3068 189 delete data;
m5171135 4:39d2aadf3068 190 break;
m5171135 4:39d2aadf3068 191
m5171135 0:7d55d6ace996 192 }
m5171135 4:39d2aadf3068 193
m5171135 0:7d55d6ace996 194 default:
m5171135 0:7d55d6ace996 195 {
m5171135 0:7d55d6ace996 196 break;
m5171135 0:7d55d6ace996 197 }
m5171135 0:7d55d6ace996 198 }
m5171135 0:7d55d6ace996 199 }
m5171135 0:7d55d6ace996 200 }
m5171135 0:7d55d6ace996 201
m5171135 0:7d55d6ace996 202 myGPS.read();
m5171135 0:7d55d6ace996 203 //recive gps module
m5171135 0:7d55d6ace996 204 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 205 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 206 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 207 continue;
m5171135 0:7d55d6ace996 208 }
m5171135 0:7d55d6ace996 209 }
m5171135 0:7d55d6ace996 210
m5171135 0:7d55d6ace996 211 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 212 refresh_Timer.reset();
m5171135 4:39d2aadf3068 213 if (myGPS.fix) agz.nowStatus = GPS_AVAIL;
m5171135 4:39d2aadf3068 214 else agz.nowStatus = GPS_UNAVAIL;
m5171135 0:7d55d6ace996 215 }
m5171135 0:7d55d6ace996 216 }
m5171135 0:7d55d6ace996 217 }