aigamozu / Mbed 2 deprecated aigamozu_program_ver2

robot

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

main.cpp@1:490b793b2e61, 2014-05-21 (annotated)

Committer:
m5171135
Date:
Wed May 21 01:23:04 2014 +0000
Revision:
1:490b793b2e61
Parent:
0:7d55d6ace996
Child:
2:95955f38f47a
add VNH5019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m5171135 0:7d55d6ace996 1 #include "mbed.h"
m5171135 0:7d55d6ace996 2 #include "XBee.h"
m5171135 0:7d55d6ace996 3 #include "MBed_Adafruit_GPS.h"
m5171135 1:490b793b2e61 4 #include "VNH5019.h"
m5171135 1:490b793b2e61 5
m5171135 0:7d55d6ace996 6 /////////////////////////////////////////
m5171135 0:7d55d6ace996 7 //
m5171135 0:7d55d6ace996 8 //Connection Setting
m5171135 0:7d55d6ace996 9 //
m5171135 0:7d55d6ace996 10 /////////////////////////////////////////
m5171135 0:7d55d6ace996 11 DigitalOut led1(LED1);
m5171135 0:7d55d6ace996 12 //IMU -> i2c
m5171135 0:7d55d6ace996 13 I2C i2c(p9, p10);
m5171135 0:7d55d6ace996 14 //set serial
m5171135 0:7d55d6ace996 15 Serial pc(USBTX, USBRX); //tx, rx
m5171135 0:7d55d6ace996 16 XBee xbee(p13,p14); //tx,rx
m5171135 0:7d55d6ace996 17 Serial * gps_Serial;
m5171135 0:7d55d6ace996 18 //gps responce
m5171135 0:7d55d6ace996 19 ZBRxResponse zbRx = ZBRxResponse();
m5171135 0:7d55d6ace996 20 XBeeAddress64 remoteAddress = XBeeAddress64(0x0013A200,0x40991C67);
m5171135 0:7d55d6ace996 21
m5171135 0:7d55d6ace996 22
m5171135 0:7d55d6ace996 23 /////////////////////////////////////////
m5171135 0:7d55d6ace996 24 //
m5171135 0:7d55d6ace996 25 //Pin Setting
m5171135 0:7d55d6ace996 26 //
m5171135 0:7d55d6ace996 27 /////////////////////////////////////////
m5171135 0:7d55d6ace996 28
m5171135 0:7d55d6ace996 29 //Motor & Encorder Setting
m5171135 1:490b793b2e61 30 VNH5019 motorShield(p21,p22,p23,p24,p25,p26);
m5171135 0:7d55d6ace996 31
m5171135 0:7d55d6ace996 32 //test data
m5171135 0:7d55d6ace996 33 Ticker axis;
m5171135 0:7d55d6ace996 34 Ticker auth_axis;
m5171135 0:7d55d6ace996 35
m5171135 0:7d55d6ace996 36 /////////////////////////////////////////
m5171135 0:7d55d6ace996 37 //
m5171135 0:7d55d6ace996 38 //Each Value Setting
m5171135 0:7d55d6ace996 39 //
m5171135 0:7d55d6ace996 40 /////////////////////////////////////////
m5171135 0:7d55d6ace996 41 //my status
m5171135 0:7d55d6ace996 42 //0: StndbyMode
m5171135 0:7d55d6ace996 43 //1: ManualMode
m5171135 0:7d55d6ace996 44 //2: AuthmaticMode(Random)
m5171135 0:7d55d6ace996 45 unsigned char my_status = 0;
m5171135 0:7d55d6ace996 46
m5171135 0:7d55d6ace996 47 //my_status
m5171135 0:7d55d6ace996 48 // 0 -> stable
m5171135 0:7d55d6ace996 49 // 1 -> error
m5171135 0:7d55d6ace996 50
m5171135 0:7d55d6ace996 51 //0 bit: Motor Satatus
m5171135 0:7d55d6ace996 52 //1 bit: GPS Status
m5171135 0:7d55d6ace996 53 //2 bit: Sensor Status
m5171135 0:7d55d6ace996 54 //3 bit: Battery Status
m5171135 0:7d55d6ace996 55 //4 bit:
m5171135 0:7d55d6ace996 56 //5 bit:
m5171135 0:7d55d6ace996 57 //6 bit:
m5171135 0:7d55d6ace996 58 //7 bit:
m5171135 0:7d55d6ace996 59
m5171135 0:7d55d6ace996 60 unsigned char my_mode = 0;
m5171135 0:7d55d6ace996 61
m5171135 0:7d55d6ace996 62 //I2C address 9-axis
m5171135 0:7d55d6ace996 63 const int gyro_addr = 0xD0;
m5171135 0:7d55d6ace996 64 const int acc_addr = 0xA6;
m5171135 0:7d55d6ace996 65
m5171135 0:7d55d6ace996 66 char gyro_head[1];
m5171135 0:7d55d6ace996 67 char read[8];
m5171135 0:7d55d6ace996 68 short int gyro_x=0;
m5171135 0:7d55d6ace996 69 short int gyro_y=0;
m5171135 0:7d55d6ace996 70 short int gyro_z=0;
m5171135 0:7d55d6ace996 71 short int tempr=0;
m5171135 0:7d55d6ace996 72
m5171135 0:7d55d6ace996 73 char acc_head[1];
m5171135 0:7d55d6ace996 74 char acc_buf[6];
m5171135 0:7d55d6ace996 75 short int acc_x = 0;
m5171135 0:7d55d6ace996 76 short int acc_y = 0;
m5171135 0:7d55d6ace996 77 short int acc_z = 0;
m5171135 0:7d55d6ace996 78
m5171135 0:7d55d6ace996 79 //GPS value
m5171135 0:7d55d6ace996 80 //float longitude = 0.0;
m5171135 0:7d55d6ace996 81 //float latitude = 0.0;
m5171135 0:7d55d6ace996 82
m5171135 0:7d55d6ace996 83 //motor_speed_feed_back
m5171135 0:7d55d6ace996 84 float target_palse = 0.0;
m5171135 0:7d55d6ace996 85 float pwm;
m5171135 0:7d55d6ace996 86 long encorder_count = 0;
m5171135 0:7d55d6ace996 87 int count = 0;
m5171135 0:7d55d6ace996 88
m5171135 0:7d55d6ace996 89 //ManualValue
m5171135 0:7d55d6ace996 90 int manual_count = 0;
m5171135 0:7d55d6ace996 91 int manual_flag = 0;
m5171135 0:7d55d6ace996 92
m5171135 0:7d55d6ace996 93
m5171135 0:7d55d6ace996 94 //test value
m5171135 0:7d55d6ace996 95 long sub_latH = 12345;
m5171135 0:7d55d6ace996 96 long sub_latL = 67890;
m5171135 0:7d55d6ace996 97
m5171135 0:7d55d6ace996 98 long sub_lonH = 98765;
m5171135 0:7d55d6ace996 99 long sub_lonL = 43211;
m5171135 0:7d55d6ace996 100
m5171135 0:7d55d6ace996 101
m5171135 0:7d55d6ace996 102 union UNI_TEST_T
m5171135 0:7d55d6ace996 103 {
m5171135 0:7d55d6ace996 104 long a;
m5171135 0:7d55d6ace996 105 uint8_t b[4];
m5171135 0:7d55d6ace996 106 };
m5171135 0:7d55d6ace996 107
m5171135 0:7d55d6ace996 108 UNI_TEST_T latH,latL,lonH,lonL;
m5171135 0:7d55d6ace996 109
m5171135 0:7d55d6ace996 110 /////////////////////////////////////////
m5171135 0:7d55d6ace996 111 //
m5171135 0:7d55d6ace996 112 //Interruption processing 1: time -> 0.1s
m5171135 0:7d55d6ace996 113 //
m5171135 0:7d55d6ace996 114 /////////////////////////////////////////
m5171135 0:7d55d6ace996 115 void axisRenovation(){
m5171135 0:7d55d6ace996 116 //gyro
m5171135 0:7d55d6ace996 117 gyro_head[0] = 0x1B;
m5171135 0:7d55d6ace996 118 i2c.write(gyro_addr, gyro_head, 1);
m5171135 0:7d55d6ace996 119 i2c.read(gyro_addr, read, 8);
m5171135 0:7d55d6ace996 120
m5171135 0:7d55d6ace996 121 tempr=(read[0] << 8) + read[1];
m5171135 0:7d55d6ace996 122 gyro_x=(read[2] << 8) + read[3];
m5171135 0:7d55d6ace996 123 gyro_y=(read[4] << 8) + read[5];
m5171135 0:7d55d6ace996 124 gyro_z=(read[6] << 8) + read[7];
m5171135 0:7d55d6ace996 125
m5171135 0:7d55d6ace996 126 //acc
m5171135 0:7d55d6ace996 127 acc_head[0] = 0x32;
m5171135 0:7d55d6ace996 128 i2c.write(acc_addr,acc_head,1);
m5171135 0:7d55d6ace996 129 i2c.read(acc_addr, acc_buf, 6);
m5171135 0:7d55d6ace996 130
m5171135 0:7d55d6ace996 131 acc_x = (acc_buf[1] << 8) + acc_buf[0];
m5171135 0:7d55d6ace996 132 acc_y = (acc_buf[3] << 8) + acc_buf[2];
m5171135 0:7d55d6ace996 133 acc_z = (acc_buf[5] << 8) + acc_buf[4];
m5171135 0:7d55d6ace996 134
m5171135 0:7d55d6ace996 135
m5171135 0:7d55d6ace996 136 if(manual_count > 100){
m5171135 1:490b793b2e61 137 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 138 manual_count = 0;
m5171135 0:7d55d6ace996 139 }
m5171135 0:7d55d6ace996 140
m5171135 0:7d55d6ace996 141 manual_count++;
m5171135 0:7d55d6ace996 142
m5171135 0:7d55d6ace996 143 //printf("Mode -> %d\n", my_mode);
m5171135 0:7d55d6ace996 144 //printf("Status -> %d\n", my_status);
m5171135 0:7d55d6ace996 145 //printf("Status -> %d\n", count);
m5171135 0:7d55d6ace996 146
m5171135 0:7d55d6ace996 147 }
m5171135 0:7d55d6ace996 148
m5171135 0:7d55d6ace996 149 /////////////////////////////////////////
m5171135 0:7d55d6ace996 150 //
m5171135 0:7d55d6ace996 151 //Interruption processing: time -> 1.0s
m5171135 0:7d55d6ace996 152 //
m5171135 0:7d55d6ace996 153 /////////////////////////////////////////
m5171135 0:7d55d6ace996 154
m5171135 0:7d55d6ace996 155 void randomRenovation(){
m5171135 0:7d55d6ace996 156
m5171135 0:7d55d6ace996 157 if(count < 30){
m5171135 1:490b793b2e61 158 motorShield.changeSpeed(1,127,1,127);
m5171135 0:7d55d6ace996 159 }
m5171135 0:7d55d6ace996 160
m5171135 0:7d55d6ace996 161 else{
m5171135 1:490b793b2e61 162 motorShield.changeSpeed(1,127,2,127);
m5171135 0:7d55d6ace996 163 if(count > 35) count = 0;
m5171135 0:7d55d6ace996 164 }
m5171135 0:7d55d6ace996 165 count++;
m5171135 0:7d55d6ace996 166 }
m5171135 0:7d55d6ace996 167
m5171135 0:7d55d6ace996 168
m5171135 0:7d55d6ace996 169
m5171135 0:7d55d6ace996 170 /////////////////////////////////////////
m5171135 0:7d55d6ace996 171 //
m5171135 0:7d55d6ace996 172 //Main Processing
m5171135 0:7d55d6ace996 173 //
m5171135 0:7d55d6ace996 174 /////////////////////////////////////////
m5171135 0:7d55d6ace996 175 int main() {
m5171135 0:7d55d6ace996 176 //start up time
m5171135 0:7d55d6ace996 177 wait(3);
m5171135 0:7d55d6ace996 178 //set i2c frequency to 400 KHz
m5171135 0:7d55d6ace996 179 i2c.frequency(400000);
m5171135 0:7d55d6ace996 180 //set pc frequency to 57600bps
m5171135 0:7d55d6ace996 181 pc.baud(57600);
m5171135 0:7d55d6ace996 182 //set xbee frequency to 57600bps
m5171135 0:7d55d6ace996 183 xbee.begin(57600);
m5171135 0:7d55d6ace996 184
m5171135 0:7d55d6ace996 185 //GPS setting
m5171135 0:7d55d6ace996 186 gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS
m5171135 0:7d55d6ace996 187 Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
m5171135 0:7d55d6ace996 188 //char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
m5171135 0:7d55d6ace996 189 Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
m5171135 0:7d55d6ace996 190 const int refresh_Time = 2000; //refresh time in ms
m5171135 0:7d55d6ace996 191
m5171135 0:7d55d6ace996 192 myGPS.begin(9600); //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
m5171135 0:7d55d6ace996 193 //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
m5171135 0:7d55d6ace996 194
m5171135 0:7d55d6ace996 195 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
m5171135 0:7d55d6ace996 196 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
m5171135 0:7d55d6ace996 197 myGPS.sendCommand(PGCMD_ANTENNA);
m5171135 0:7d55d6ace996 198
m5171135 0:7d55d6ace996 199 //gyro_registor Setting
m5171135 0:7d55d6ace996 200 char PWR_M[2]={0x3E,0x80};
m5171135 0:7d55d6ace996 201 i2c.write(gyro_addr, PWR_M, 2); // Send command string
m5171135 0:7d55d6ace996 202 char SMPL[2]={0x15,0x00};
m5171135 0:7d55d6ace996 203 i2c.write(gyro_addr, SMPL, 2); // Send command string
m5171135 0:7d55d6ace996 204 char DLPF[2]={0x16,0x18};
m5171135 0:7d55d6ace996 205 i2c.write(gyro_addr, DLPF, 2); // Send command string
m5171135 0:7d55d6ace996 206 char INT_C[2]={0x17,0x05};
m5171135 0:7d55d6ace996 207 i2c.write(gyro_addr, INT_C, 2); // Send commanad string
m5171135 0:7d55d6ace996 208 char PWR_M2[2]={0x3E,0x00};
m5171135 0:7d55d6ace996 209 i2c.write(gyro_addr, PWR_M2, 2); // Send command string
m5171135 0:7d55d6ace996 210
m5171135 0:7d55d6ace996 211 wait(2);
m5171135 0:7d55d6ace996 212
m5171135 0:7d55d6ace996 213 //interrupt start
m5171135 0:7d55d6ace996 214 axis.attach(&axisRenovation, 0.1);
m5171135 0:7d55d6ace996 215
m5171135 0:7d55d6ace996 216 refresh_Timer.start();
m5171135 0:7d55d6ace996 217
m5171135 0:7d55d6ace996 218 while (1) {
m5171135 0:7d55d6ace996 219
m5171135 0:7d55d6ace996 220 //recive xbee module
m5171135 0:7d55d6ace996 221 xbee.readPacket();
m5171135 0:7d55d6ace996 222 if (xbee.getResponse().isAvailable()) {
m5171135 0:7d55d6ace996 223 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
m5171135 0:7d55d6ace996 224 xbee.getResponse().getZBRxResponse(zbRx);
m5171135 0:7d55d6ace996 225 unsigned char *buf = zbRx.getFrameData();
m5171135 0:7d55d6ace996 226
m5171135 0:7d55d6ace996 227 switch((unsigned char)buf[14]){
m5171135 0:7d55d6ace996 228
m5171135 0:7d55d6ace996 229 //ChanegeMode
m5171135 0:7d55d6ace996 230 case 'C':{
m5171135 0:7d55d6ace996 231 my_mode = buf[19];
m5171135 1:490b793b2e61 232 motorShield.changeSpeed(0,0,0,0);
m5171135 0:7d55d6ace996 233
m5171135 0:7d55d6ace996 234 if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0);
m5171135 0:7d55d6ace996 235 else auth_axis.detach();
m5171135 0:7d55d6ace996 236 break;
m5171135 0:7d55d6ace996 237 }
m5171135 0:7d55d6ace996 238
m5171135 0:7d55d6ace996 239 //MunualCommand
m5171135 0:7d55d6ace996 240 case 'M':{
m5171135 0:7d55d6ace996 241 if(my_mode == 1){
m5171135 0:7d55d6ace996 242 manual_count = 0;
m5171135 1:490b793b2e61 243 motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
m5171135 0:7d55d6ace996 244
m5171135 0:7d55d6ace996 245 }
m5171135 0:7d55d6ace996 246 break;
m5171135 0:7d55d6ace996 247 }
m5171135 0:7d55d6ace996 248
m5171135 0:7d55d6ace996 249 //SendStatus
m5171135 0:7d55d6ace996 250 case 'S':{
m5171135 0:7d55d6ace996 251 //latH.a = myGPS.latitudeH;;
m5171135 0:7d55d6ace996 252 //latL.a = myGPS.latitudeL;
m5171135 0:7d55d6ace996 253 //lonH.a = myGPS.longitudeH;
m5171135 0:7d55d6ace996 254 //lonL.a = myGPS.longitudeL;
m5171135 0:7d55d6ace996 255 led1 = 1;
m5171135 0:7d55d6ace996 256 //dummy data
m5171135 0:7d55d6ace996 257 latH.a = sub_latH;
m5171135 0:7d55d6ace996 258 latL.a = sub_latL;
m5171135 0:7d55d6ace996 259 lonH.a = sub_lonH;
m5171135 0:7d55d6ace996 260 lonL.a = sub_lonL;
m5171135 0:7d55d6ace996 261
m5171135 0:7d55d6ace996 262 uint8_t data[] = {65,71,83,82,70,65,84,66,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 263 ZBTxRequest tx64request(remoteAddress,data,sizeof(data));
m5171135 0:7d55d6ace996 264 xbee.send(tx64request);
m5171135 0:7d55d6ace996 265 break;
m5171135 0:7d55d6ace996 266 }
m5171135 0:7d55d6ace996 267
m5171135 0:7d55d6ace996 268 default:
m5171135 0:7d55d6ace996 269 {
m5171135 0:7d55d6ace996 270 break;
m5171135 0:7d55d6ace996 271 }
m5171135 0:7d55d6ace996 272 }
m5171135 0:7d55d6ace996 273 }
m5171135 0:7d55d6ace996 274 }
m5171135 0:7d55d6ace996 275
m5171135 0:7d55d6ace996 276 myGPS.read();
m5171135 0:7d55d6ace996 277 //recive gps module
m5171135 0:7d55d6ace996 278 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 279 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 280 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 281 continue;
m5171135 0:7d55d6ace996 282 }
m5171135 0:7d55d6ace996 283 }
m5171135 0:7d55d6ace996 284
m5171135 0:7d55d6ace996 285 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 286 refresh_Timer.reset();
m5171135 0:7d55d6ace996 287 if (myGPS.fix) {
m5171135 0:7d55d6ace996 288 my_status = 0;
m5171135 0:7d55d6ace996 289 //longitude = myGPS.longitudeH;
m5171135 0:7d55d6ace996 290 //latitude = myGPS.latitudeH;
m5171135 0:7d55d6ace996 291 }
m5171135 0:7d55d6ace996 292
m5171135 0:7d55d6ace996 293 else my_status = 1;
m5171135 0:7d55d6ace996 294 }
m5171135 0:7d55d6ace996 295 }
m5171135 0:7d55d6ace996 296 }