main.cpp

00001 /**********************************************/
00002 //  
00003 //    
00004 //
00005 //  Program name: Aigamozu ROBOT
00006 //  Author: Mineta Kizuku
00007 //  Yokokawa
00008 // 
00009 //
00010 /**********************************************/
00011 
00012 /**********************************************/
00013 //更新情報
00014 //展示会用プログラム
00015 //中身をすっきりさせました。
00016 //Automodeの際のプログラムはgpsAuto()の中身を変更すれば大丈夫です。
00017 /**********************************************/
00018 
00019 #include "mbed.h"
00020 #include "XBee.h"
00021 #include "MBed_Adafruit_GPS.h"
00022 #include "AigamozuControlPackets.h"
00023 #include "agzIDLIST.h"
00024 #include "aigamozuSetting.h"
00025 #include "math.h"
00026 
00027 /////////////////////////////////////////
00028 //
00029 //Pin Setting
00030 //
00031 /////////////////////////////////////////
00032 //Serial Connect Setting: mbed <--> motor driver
00033 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00034 
00035 
00036 /////////////////////////////////////////
00037 //
00038 //Connection Setting
00039 //
00040 //
00041 /////////////////////////////////////////
00042 
00043 //Serial Connect Setting: PC <--> mbed
00044 Serial pc(USBTX, USBRX);    
00045 
00046 //Serial Connect Setting: GPS <--> mbed
00047 Serial * gps_Serial;
00048 
00049 //Serial Connect Setting: XBEE <--> mbed
00050 XBee xbee(p13,p14);
00051 ZBRxResponse zbRx = ZBRxResponse();
00052 
00053 //set up AigamozuControlPackets library
00054 AigamozuControlPackets agz(agz_motorShield);
00055 
00056 
00057 /////////////////////////////////////////
00058 //
00059 //Plus Speed
00060 //
00061 //ロボットを操縦する
00062 /////////////////////////////////////////
00063 void plusSpeed(uint8_t *packetdata){
00064        
00065     if(agz.nowMode == MANUAL_MODE){ 
00066         agz.changeSpeed(packetdata);
00067     }
00068     
00069 }
00070 
00071 
00072 
00073 /////////////////////////////////////////
00074 //
00075 //New Mode
00076 //
00077 //ロボットのモードを変更する
00078 /////////////////////////////////////////
00079 void newMode(uint8_t *packetdata){
00080   
00081   //bool result;
00082    agz.changeMode(packetdata); 
00083   
00084 }
00085 
00086 
00087 /////////////////////////////////////////
00088 //
00089 //Main Processing
00090 //
00091 /////////////////////////////////////////
00092 int main() {
00093     //start up time
00094     wait(3);
00095     //set pc frequency to 57600bps 
00096     pc.baud(PC_BAUD_RATE); 
00097     //set xbee frequency to 57600bps
00098     xbee.begin(XBEE_BAUD_RATE);    
00099 
00100     //GPS setting
00101     gps_Serial = new Serial(p28,p27);
00102     Adafruit_GPS myGPS(gps_Serial); 
00103   
00104     //2secごとの更新用Timer
00105     Timer autoTimer;
00106     const int autoTime = 1000; //refresh time in ms
00107 
00108     //interrupt start
00109     autoTimer.start();
00110     agz.moveTimer.start();
00111     
00112     wait_ms(2000);
00113     
00114     printf("start\n");    
00115 
00116     while (true) {
00117         
00118         //Check Xbee Buffer Available
00119         xbee.readPacket();
00120         
00121         //Xbee受信があった場合
00122         if (xbee.getResponse().isAvailable()) {
00123             xbee.getResponse().getZBRxResponse(zbRx);
00124             //frame dataの格納
00125             uint8_t *buf = zbRx.getFrameData();
00126             
00127             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00128                 xbee.getResponse().getZBRxResponse(zbRx);
00129                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00130                 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
00131                  
00132                 //Check Command Type 
00133                 switch(Command_type){
00134                     //Get Request command
00135                     case MANUAL:{ //マニュアルモードパケット
00136                         plusSpeed(buf); //ロボットの操縦
00137                         break;
00138                     }
00139                     case STATUS_REQUEST:{ //ステータスリクエストパケット           
00140                         break;          
00141                     }
00142                     case CHANGE_MODE:{ //モード変更パケット
00143                         newMode(buf); //モードの変更
00144                         break;
00145                     }
00146                     case RECEIVE_STATUS:{ //レシーブステータスパケット
00147                         break;
00148                     }
00149                     default:{
00150                         break;
00151                     }
00152                 }//endswitch
00153             }//endifZB_RX_RESPONSE
00154         }//endifisAvailable
00155   
00156   //ロボットがオートモードのとき、シーケンス動作する
00157      if(agz.nowMode == AUTO_GPS_MODE && autoTimer.read_ms() >= autoTime){
00158            autoTimer.reset();
00159            agz.autoMove();
00160         }
00161         
00162     }
00163     
00164 }