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