aigamozu
robot
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common
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;
}
}