File content as of revision 47:626572030a78:

/**********************************************/
//  
//    
//
//  Program name: Aigamozu ROBOT
//  Author: Mineta Kizuku
//  Yokokawa
// 
//
/**********************************************/

/**********************************************/
//更新情報
//展示会用プログラム
//中身をすっきりさせました。
//Automodeの際のプログラムはautoMode()の中身を変更すれば大丈夫です。
/**********************************************/

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
#include "AigamozuControlPackets.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.h"
#include "math.h"

//************ID Number*****************
const char MyID = 'e';
const char SenderIDc = 'c';
//************ID Number*****************

/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////
//Serial Connect Setting: mbed <--> motor driver
VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);


/////////////////////////////////////////
//
//Connection Setting
//
/////////////////////////////////////////

//Serial Connect Setting: PC <--> mbed
Serial pc(USBTX, USBRX);    

//Serial Connect Setting: GPS <--> mbed
Serial * gps_Serial;

//Serial Connect Setting: XBEE <--> mbed
XBee xbee(p13,p14);
ZBRxResponse zbRx = ZBRxResponse();

//set up AigamozuControlPackets library
AigamozuControlPackets agz(agz_motorShield);


/////////////////////////////////////////
//
//Plus Speed
//
//ロボットを操縦する
/////////////////////////////////////////
void plusSpeed(uint8_t *packetdata){
       
    if(agz.nowMode == MANUAL_MODE){ 
        agz.changeSpeed(packetdata);
    }
    
}



/////////////////////////////////////////
//
//New Mode
//
//ロボットのモードを変更する
/////////////////////////////////////////
void newMode(uint8_t *packetdata){
  
  //bool result;
   agz.changeMode(packetdata); 
  
}


/////////////////////////////////////////
//
//Send_Change_Mode
//
//マニュアルモードのコマンドを送る
/////////////////////////////////////////
void Send_Change_Mode(char c){

    XBeeAddress64 send_Address;
    if(SenderIDc == '0'){
        send_Address = manager_Address;
    }
    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
        send_Address = robot_Address[SenderIDc - 'A'];
    }
    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
        send_Address = base_Address[SenderIDc - 'a'];
    }
    
    //send normal data
    int cmode=0;
    if(c == 'n'){
        cmode = 0;
        pc.printf("stand by mode\n\r");
    }
    else if(c == 'm'){
        cmode = 1;
        pc.printf("manual mode\n\r");
    }
    else if(c == 'b'){
        cmode = 2;
        pc.printf("auto mode\n\r");
    }

    //Create GPS Infomation Packet
    agz.createChangeModeCommand(MyID,SenderIDc,cmode);
    
   //debug
   printf("RF data:");
    for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; i++){
       printf("%x ", agz.packetData[i]);
    }
    printf("\n\r");
   
    //Select Destination
    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
    //Send -> Base
   xbee.send(tx64request);
}


/////////////////////////////////////////
//
//Main Processing
//
/////////////////////////////////////////
int main() {
    //start up time
    wait(3);
    //set pc frequency to 57600bps 
    pc.baud(PC_BAUD_RATE); 
    //set xbee frequency to 57600bps
    xbee.begin(XBEE_BAUD_RATE);    

    //GPS setting
    gps_Serial = new Serial(p28,p27);
    Adafruit_GPS myGPS(gps_Serial); 
  
    //2secごとの更新用Timer
    Timer autoTimer;
    const int autoTime = 200; //refresh time in ms

    //interrupt start
    autoTimer.start();
    agz.moveTimer.start();
    
    wait_ms(2000);
    
    printf("start\n");    

    while (true) {
        
        //Check Xbee Buffer Available
        xbee.readPacket();
        
        //Xbee受信があった場合
        if (xbee.getResponse().isAvailable()) {
            xbee.getResponse().getZBRxResponse(zbRx);
            //frame dataの格納
            uint8_t *buf = zbRx.getFrameData();
            
            if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                xbee.getResponse().getZBRxResponse(zbRx);
                uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
                char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
                 
                //Check Command Type 
                switch(Command_type){
                    //Get Request command
                    case MANUAL:{ //マニュアルモードパケット
                        plusSpeed(buf); //ロボットの操縦
                        break;
                    }
                    case STATUS_REQUEST:{ //ステータスリクエストパケット           
                        break;          
                    }
                    case CHANGE_MODE:{ //モード変更パケット
                        newMode(buf); //モードの変更
                        break;
                    }
                    case RECEIVE_STATUS:{ //レシーブステータスパケット
                        break;
                    }
                    default:{
                        break;
                    }
                }//endswitch
            }//endifZB_RX_RESPONSE
        }//endifisAvailable
  
    //ロボットがオートモードのとき、シーケンス動作する
    // if(agz.nowMode == AUTO_GPS_MODE && autoTimer.read_ms() >= autoTime){
    //       autoTimer.reset();
    //       agz.autoMove();
    //    }
    //
    
    if(autoTimer.read_ms() >= autoTime && pc.readable()){
            autoTimer.reset();
            char c = pc.getc();
            
            if(c == 'm' || c == 'n' || c == 'b'){
                //m:manual-mode,n:stand-by-mode,b:auto-gps-mode
                Send_Change_Mode(c);
                //pc.printf("%c\n\r", c);
            }
        }
    }
    
}