File content as of revision 41:55539183dbb0:

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

/**********************************************/
//更新情報
//実験用プログラム
//ジャイロ・加速度データをXbeeに送り続けます。
//StatusRequestCommandを送るとflagが0から1になります。
/**********************************************/

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

#include "HMC5843.h"
#include "ADXL345.h"
#include "ITG3200.h"
#include "setting.h"

//************ID Number*****************
const char MyID = 'b';
const char SenderIDc = 'h';
//************ID Number*****************

/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////
VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
HMC5843 cmp(p9, p10);      // sda, scl
ADXL345 acc(p9, p10);      // sda, scl
ITG3200 gyr(p9, p10);      // sda, scl
setting set;


/////////////////////////////////////////
//
//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 GPS module

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



int change = 0;
//values x,y,z
int readings[3];
//ID Buffer
char buffer[3];

int flag =0;

/////////////////////////////////////////
//
//Plus Speed
//
//MNUAL_MODEの時にスピードを変える
/////////////////////////////////////////
void Plus_Speed(uint8_t *packetdata){
       
    if(agz.nowMode == MANUAL_MODE){ 
        agz.changeSpeed(packetdata);
    }
    
}



/////////////////////////////////////////
//
//New Mode
//
/////////////////////////////////////////

void New_Mode(uint8_t *packetdata){
  
  //bool result;
   agz.changeMode(packetdata); 
  
}


/////////////////////////////////////////
//
//Send_Status
//
//リクエストがきたとき、自分の位置情報などを返信する
/////////////////////////////////////////
void Send_Status(void){

    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
    //Create GPS Infomation Packet
    agz.SendDataCommand(MyID,SenderIDc,(int16_t)set.agzGyr.x,(int16_t)set.agzGyr.y, (int16_t)set.agzGyr.z, (int16_t)set.agzAcc.x,(int16_t)set.agzAcc.y,(int16_t)set.agzAcc.z, (uint8_t)flag);
    
 /*   //debug***************************************************
    printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
            agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
            agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
            agz.get_agzCov_lati(),agz.get_agzCov_longi()
            );
    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
        printf("\n");
    //debug end***************************************************
   */
   //debug
   printf("%d %d %d %d %d %d\n\r",(int16_t)set.agzGyr.x,(int16_t)set.agzGyr.y, (int16_t)set.agzGyr.z, (int16_t)set.agzAcc.x, (int16_t)set.agzAcc.y, (int16_t)set.agzAcc.z); 
for(int i = 0; i < 24; i++){
       printf("%d ", agz.packetData[i]);
       }
       printf("\n\r");
   
    //Select Destination
    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
    //Send -> Base
   xbee.send(tx64request);
}

/////////////////////////////////////////
//
//Update Sensor
//
/////////////////////////////////////////

void update_gyro() {
    set.setGyr((int16_t)gyr.getGyroX(),(int16_t)gyr.getGyroY(),(int16_t)gyr.getGyroZ());
}

void update_accel() {
    acc.getOutput(readings);
    set.setAcc(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
}

void update_cmp(){
    cmp.readData(readings);
    set.setCmp(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
}

/////////////////////////////////////////
//
//Set Up Sensor
//
/////////////////////////////////////////
void setup_cmp() {
    // Continuous mode, , 10Hz measurement rate.
    // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
    cmp.setDefault();
    // Wait some time(Need at least 5ms)
    wait(0.1);
    cmp.getAddress(buffer);
    pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
}
        
bool setup_acc() {
    // These are here to test whether any of the initialization fails. It will print the failure
    if (acc.setPowerControl(0x00)) {
        pc.printf("acc: didn't intitialize power control\n"); 
        return false;  
    }
    wait(.001);
    
    //Full resolution, +/-16g, 4mg/LSB.
    //if(acc.setDataFormatControl(0x0B)){
    if(acc.setDataFormatControl(0x09)){  // +/- 4G               
        pc.printf("didn't set data format\n");
        return false;  }
    wait(.001);
     
    //200Hz data rate.
    if(acc.setDataRate(ADXL345_200HZ)){
        pc.printf("didn't set data rate\n");
        return false;    }
    wait(.001);
     
    //Measurement mode.
    if(acc.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\n"); 
        return false;   } 

    pc.printf("Acc Id=%x \n\r", acc.getDeviceID());
    pc.printf("%c" ,13,10);
    return true;
}

void setup_gyr() {
    //Set highest bandwidth.
    gyr.setLpBandwidth(LPFBW_42HZ);
    pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI());
    pc.printf("%c" ,13,10);
}

/////////////////////////////////////////
//
//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); 
    setup_acc();    
    setup_gyr();
    
    Timer auto_Timer;
    const int auto_Time = 500; //refresh time in ms
    
    wait_ms(2000);
       
    //interrupt start
    auto_Timer.start();
    agz.Move_Timer.start();
    printf("start\n");
    
    
    while (true) {
        
        //Check Xbee Buffer Available
        xbee.readPacket();

        update_accel();
        update_gyro();
            
        if (xbee.getResponse().isAvailable()) {
            xbee.getResponse().getZBRxResponse(zbRx);
            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:{
                        Plus_Speed(buf);
                        break;
                    }
                    case STATUS_REQUEST:{
                        flag = 1;
                        break;          
                    }
                    case CHANGE_MODE:{
                        New_Mode(buf);
                        break;
                    }
                    case RECEIVE_STATUS:{
                        break;
                    }
                    default:{
                        break;
                    }
                }//endswitch
            }//endifZB_RX_RESPONSE
        }//endifisAvailable
  
     if(auto_Timer.read_ms() >= auto_Time){
           auto_Timer.reset();
           Send_Status();
        }
        
    }
    
}