Revision 2:95955f38f47a, committed 2014-05-28

Comitter:

m5171135

Date:

Wed May 28 11:54:22 2014 +0000

Parent:

1:490b793b2e61

Child:

3:1229ca3df855

Commit message:

???

Changed in this revision

--- a/VNH5019.lib	Wed May 21 01:23:04 2014 +0000
+++ b/VNH5019.lib	Wed May 28 11:54:22 2014 +0000
@@ -1,1 +1,1 @@
-VNH5019#83e00dc8eb92
+VNH5019#656efbc86da4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agzIDLIST.lib	Wed May 28 11:54:22 2014 +0000
@@ -0,0 +1,1 @@
+agzIDLIST#47077248134f

--- 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;
         }     
     }