aigamozu
yokokawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of
aigamozu_program_ver2
by 
aigamozu
Revision 0:7d55d6ace996, committed 2014-05-20
- Comitter:
- m5171135
- Date:
- Tue May 20 14:51:25 2014 +0000
- Child:
- 1:490b793b2e61
- Commit message:
- aaa
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MBed_Adafruit-GPS-Library.lib Tue May 20 14:51:25 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mlee350/code/MBed_Adafruit-GPS-Library/#ff72e93bcb0e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XBee.lib Tue May 20 14:51:25 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/okini3939/code/XBee/#b36422ef864f
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue May 20 14:51:25 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/8a40adfe8776 \ No newline at end of file