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:
- 0:7d55d6ace996
- Child:
- 1:490b793b2e61
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue May 20 14:51:25 2014 +0000 @@ -0,0 +1,368 @@ +#include "mbed.h" +#include "XBee.h" +#include "MBed_Adafruit_GPS.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 +//Left +DigitalOut motorL_A(p21); +DigitalOut motorL_B(p22); +PwmOut motorL_pwm(p23); +InterruptIn encoderR_A(p15); +InterruptIn encoderR_B(p16); + +//Rignt +DigitalOut motorR_A(p24); +DigitalOut motorR_B(p25); +PwmOut motorR_pwm(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; + + + + + +///////////////////////////////////////// +// +//Change Motor State Processing +// +///////////////////////////////////////// +void change_speed(unsigned char L_state,unsigned char L_pwm,unsigned char R_state,unsigned char R_pwm){ + switch(L_state){ + case 0: + motorL_A = 0; + motorL_B = 0; + break; + + case 1: + motorL_A = 1; + motorL_B = 0; + break; + + case 2: + motorL_A = 0; + motorL_B = 1; + break; + + default: + motorL_A = 0; + motorL_B = 0; + break; + } + + motorL_pwm = (float)L_pwm/256.0; + + switch(R_state){ + case 0: + motorR_A = 0; + motorR_B = 0; + break; + + case 1: + motorR_A = 1; + motorR_B = 0; + break; + + case 2: + motorR_A = 0; + motorR_B = 1; + break; + + default: + motorR_A = 0; + motorR_B = 0; + break; + } + motorR_pwm = (float)R_pwm/256.0; + + } + +///////////////////////////////////////// +// +//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){ + change_speed(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){ + change_speed(1,127,1,127); + } + + else{ + change_speed(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); + motorL_pwm = 0.0; + motorR_pwm = 0.0; + + 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]; + change_speed(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; + change_speed(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; + } + } +} \ No newline at end of file