yokokawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_program_ver2 by
Diff: main.cpp
- Revision:
- 2:95955f38f47a
- Parent:
- 1:490b793b2e61
- Child:
- 3:1229ca3df855
--- a/main.cpp Wed May 21 01:23:04 2014 +0000 +++ b/main.cpp Wed May 28 11:54:22 2014 +0000 @@ -1,24 +1,43 @@ +/**********************************************/ +// +// +// Program name: Aigamozu Robot Control +// author: Atsunori Maruyama +// +// +// ver -> 1.0 +// +// +/**********************************************/ + #include "mbed.h" #include "XBee.h" #include "MBed_Adafruit_GPS.h" #include "VNH5019.h" +#include "agzIDLIST.h" + + +#define GPS_BAUD_RATE 9600 +#define XBEE_BAUD_RATE 57600 +#define PC_BAUD_RATE 57600 +#define IMU_BAUD_RATE 400000 ///////////////////////////////////////// // //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 Connect Setting: PC <--> mbed +Serial pc(USBTX, USBRX); + +//Serial Connect Setting: GPS <--> mbed Serial * gps_Serial; -//gps responce + +//Serial Connect Setting: XBEE <--> mbed +XBee xbee(p13,p14); ZBRxResponse zbRx = ZBRxResponse(); -XBeeAddress64 remoteAddress = XBeeAddress64(0x0013A200,0x40991C67); - +XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L); ///////////////////////////////////////// // @@ -26,10 +45,10 @@ // ///////////////////////////////////////// -//Motor & Encorder Setting +//Motor Contorol Pin Setting VNH5019 motorShield(p21,p22,p23,p24,p25,p26); -//test data +//Interrupt Setting Ticker axis; Ticker auth_axis; @@ -38,63 +57,27 @@ //Each Value Setting // ///////////////////////////////////////// +int count = 0; + //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; @@ -107,43 +90,19 @@ 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); - + if(my_mode == 1) manual_count++; } ///////////////////////////////////////// @@ -154,13 +113,18 @@ void randomRenovation(){ - if(count < 30){ + if(count < 20){ motorShield.changeSpeed(1,127,1,127); } else{ - motorShield.changeSpeed(1,127,2,127); - if(count > 35) count = 0; + motorShield.changeSpeed(1,64,2,64); + if(count > 21) { + + count = 0; + motorShield.changeSpeed(1,127,1,127); + + } } count++; } @@ -175,47 +139,32 @@ int main() { //start up time wait(3); - //set i2c frequency to 400 KHz - i2c.frequency(400000); //set pc frequency to 57600bps - pc.baud(57600); + pc.baud(PC_BAUD_RATE); //set xbee frequency to 57600bps - xbee.begin(57600); + xbee.begin(XBEE_BAUD_RATE); //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? + gps_Serial = new Serial(p28,p27); + Adafruit_GPS myGPS(gps_Serial); + Timer refresh_Timer; const int refresh_Time = 2000; //refresh time in ms + myGPS.begin(GPS_BAUD_RATE); - 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 + //GPS Send Command + myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 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) { + /// @todo change to true + while (true) { //recive xbee module xbee.readPacket(); @@ -224,45 +173,65 @@ xbee.getResponse().getZBRxResponse(zbRx); unsigned char *buf = zbRx.getFrameData(); + /// @todo use preporcessor (#define) or const. + /// @note It's magic number. + // COMMAND_TYPE commandType = buf[14]; + /// switch(commandType) + /// {case MANUAL:} + + + //switch(aigamoCTR.checkCommandType(buf)){ 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; } + /* + case 'C': + agz.change_Mode(); + break; + */ + //MunualCommand case 'M':{ + /// @todo Magic. if(my_mode == 1){ manual_count = 0; motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]); - } break; } + /* + case 'M': + uint8_t sppeds_buf[4] = agz.parseMunualCommand(buf); + motorShield.changeSpeed(sppedbuf[0],sppedbuf[1],sppedbuf[2],sppedbuf[3]); + break; + */ + //SendStatus case 'S':{ - //latH.a = myGPS.latitudeH;; - //latL.a = myGPS.latitudeL; - //lonH.a = myGPS.longitudeH; - //lonL.a = myGPS.longitudeL; - led1 = 1; + latH.a = myGPS.latitudeH;; + latL.a = myGPS.latitudeL; + lonH.a = myGPS.longitudeH; + lonL.a = myGPS.longitudeL; //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}; + //latH.a = sub_latH;latL.a = sub_latL;lonH.a = sub_lonH;lonL.a = sub_lonL; + + // if(validateCommand(commandType)); + // createReceiveStatusData(); + 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}; ZBTxRequest tx64request(remoteAddress,data,sizeof(data)); xbee.send(tx64request); - break; + break; + /* + case 'S': + */ } default: @@ -284,12 +253,7 @@ if (refresh_Timer.read_ms() >= refresh_Time) { refresh_Timer.reset(); - if (myGPS.fix) { - my_status = 0; - //longitude = myGPS.longitudeH; - //latitude = myGPS.latitudeH; - } - + if (myGPS.fix) my_status = 0; else my_status = 1; } }