yokokawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_program_ver2 by
main.cpp
- Committer:
- m5171135
- Date:
- 2014-05-21
- Revision:
- 1:490b793b2e61
- Parent:
- 0:7d55d6ace996
- Child:
- 2:95955f38f47a
File content as of revision 1:490b793b2e61:
#include "mbed.h" #include "XBee.h" #include "MBed_Adafruit_GPS.h" #include "VNH5019.h" ///////////////////////////////////////// // //Connection Setting // ///////////////////////////////////////// DigitalOut led1(LED1); //IMU -> i2c I2C i2c(p9, p10); //set serial Serial pc(USBTX, USBRX); //tx, rx XBee xbee(p13,p14); //tx,rx Serial * gps_Serial; //gps responce ZBRxResponse zbRx = ZBRxResponse(); XBeeAddress64 remoteAddress = XBeeAddress64(0x0013A200,0x40991C67); ///////////////////////////////////////// // //Pin Setting // ///////////////////////////////////////// //Motor & Encorder Setting VNH5019 motorShield(p21,p22,p23,p24,p25,p26); //test data Ticker axis; Ticker auth_axis; ///////////////////////////////////////// // //Each Value Setting // ///////////////////////////////////////// //my status //0: StndbyMode //1: ManualMode //2: AuthmaticMode(Random) unsigned char my_status = 0; //my_status // 0 -> stable // 1 -> error //0 bit: Motor Satatus //1 bit: GPS Status //2 bit: Sensor Status //3 bit: Battery Status //4 bit: //5 bit: //6 bit: //7 bit: unsigned char my_mode = 0; //I2C address 9-axis const int gyro_addr = 0xD0; const int acc_addr = 0xA6; char gyro_head[1]; char read[8]; short int gyro_x=0; short int gyro_y=0; short int gyro_z=0; short int tempr=0; char acc_head[1]; char acc_buf[6]; short int acc_x = 0; short int acc_y = 0; short int acc_z = 0; //GPS value //float longitude = 0.0; //float latitude = 0.0; //motor_speed_feed_back float target_palse = 0.0; float pwm; long encorder_count = 0; int count = 0; //ManualValue int manual_count = 0; int manual_flag = 0; //test value long sub_latH = 12345; long sub_latL = 67890; long sub_lonH = 98765; long sub_lonL = 43211; union UNI_TEST_T { long a; uint8_t b[4]; }; UNI_TEST_T latH,latL,lonH,lonL; ///////////////////////////////////////// // //Interruption processing 1: time -> 0.1s // ///////////////////////////////////////// void axisRenovation(){ //gyro gyro_head[0] = 0x1B; i2c.write(gyro_addr, gyro_head, 1); i2c.read(gyro_addr, read, 8); tempr=(read[0] << 8) + read[1]; gyro_x=(read[2] << 8) + read[3]; gyro_y=(read[4] << 8) + read[5]; gyro_z=(read[6] << 8) + read[7]; //acc acc_head[0] = 0x32; i2c.write(acc_addr,acc_head,1); i2c.read(acc_addr, acc_buf, 6); acc_x = (acc_buf[1] << 8) + acc_buf[0]; acc_y = (acc_buf[3] << 8) + acc_buf[2]; acc_z = (acc_buf[5] << 8) + acc_buf[4]; if(manual_count > 100){ motorShield.changeSpeed(0,0,0,0); manual_count = 0; } manual_count++; //printf("Mode -> %d\n", my_mode); //printf("Status -> %d\n", my_status); //printf("Status -> %d\n", count); } ///////////////////////////////////////// // //Interruption processing: time -> 1.0s // ///////////////////////////////////////// void randomRenovation(){ if(count < 30){ motorShield.changeSpeed(1,127,1,127); } else{ motorShield.changeSpeed(1,127,2,127); if(count > 35) count = 0; } count++; } ///////////////////////////////////////// // //Main Processing // ///////////////////////////////////////// int main() { //start up time wait(3); //set i2c frequency to 400 KHz i2c.frequency(400000); //set pc frequency to 57600bps pc.baud(57600); //set xbee frequency to 57600bps xbee.begin(57600); //GPS setting gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class //char c; //when read via Adafruit_GPS::read(), the class returns single character stored here Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info? const int refresh_Time = 2000; //refresh time in ms myGPS.begin(9600); //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *) //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); myGPS.sendCommand(PGCMD_ANTENNA); //gyro_registor Setting char PWR_M[2]={0x3E,0x80}; i2c.write(gyro_addr, PWR_M, 2); // Send command string char SMPL[2]={0x15,0x00}; i2c.write(gyro_addr, SMPL, 2); // Send command string char DLPF[2]={0x16,0x18}; i2c.write(gyro_addr, DLPF, 2); // Send command string char INT_C[2]={0x17,0x05}; i2c.write(gyro_addr, INT_C, 2); // Send commanad string char PWR_M2[2]={0x3E,0x00}; i2c.write(gyro_addr, PWR_M2, 2); // Send command string wait(2); //interrupt start axis.attach(&axisRenovation, 0.1); refresh_Timer.start(); while (1) { //recive xbee module xbee.readPacket(); if (xbee.getResponse().isAvailable()) { if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { xbee.getResponse().getZBRxResponse(zbRx); unsigned char *buf = zbRx.getFrameData(); switch((unsigned char)buf[14]){ //ChanegeMode case 'C':{ my_mode = buf[19]; motorShield.changeSpeed(0,0,0,0); if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0); else auth_axis.detach(); break; } //MunualCommand case 'M':{ if(my_mode == 1){ manual_count = 0; motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]); } break; } //SendStatus case 'S':{ //latH.a = myGPS.latitudeH;; //latL.a = myGPS.latitudeL; //lonH.a = myGPS.longitudeH; //lonL.a = myGPS.longitudeL; led1 = 1; //dummy data latH.a = sub_latH; latL.a = sub_latL; lonH.a = sub_lonH; lonL.a = sub_lonL; 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}; ZBTxRequest tx64request(remoteAddress,data,sizeof(data)); xbee.send(tx64request); break; } default: { break; } } } } myGPS.read(); //recive gps module //check if we recieved a new message from GPS, if so, attempt to parse it, if ( myGPS.newNMEAreceived() ) { if ( !myGPS.parse(myGPS.lastNMEA()) ) { continue; } } if (refresh_Timer.read_ms() >= refresh_Time) { refresh_Timer.reset(); if (myGPS.fix) { my_status = 0; //longitude = myGPS.longitudeH; //latitude = myGPS.latitudeH; } else my_status = 1; } } }