change kalman
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver4 by
Diff: main.cpp
- Revision:
- 2:886fac7f4399
- Parent:
- 1:b2b950b916ce
- Child:
- 3:1ac506a96fd6
--- a/main.cpp Wed May 13 04:09:16 2015 +0000 +++ b/main.cpp Wed May 13 06:04:47 2015 +0000 @@ -85,10 +85,23 @@ XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)}; XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L); +///////////////////////////////////////// +// +//Plus Speed +// +//MNUAL_MODEの時にスピードを変える +///////////////////////////////////////// +void Plus_Speed(uint8_t *packetdata){ + + if(agz.nowMode == MANUAL_MODE){ + agz.changeSpeed(packetdata); + } + +} ///////////////////////////////////////// // -//Send_Status +//Send Status // //リクエストがきたとき、自分の位置情報などを返信する ///////////////////////////////////////// @@ -158,6 +171,18 @@ else agz.nowStatus = GPS_UNAVAIL; } + +///////////////////////////////////////// +// +//New Mode +// +///////////////////////////////////////// + +void New_Mode(uint8_t *packetdata){ + + agz.changeMode(packetdata); + +} ///////////////////////////////////////// // @@ -192,6 +217,31 @@ } } +void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){ + + //マネージャからデータが来たとき + if(SenderIDc == '0'){ + printf("get manager Status Kalman\n"); + } + //他のロボットからデータが来たとき + if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ + printf("get other robots Status Kalman\n"); + } + //基地局からデータが来たとき + if(SenderIDc >= 'a' && SenderIDc <= 'z'){ + printf("Get Base data Kalman\n"); + int id = SenderIDc - 'a'; + agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]); + + //debug + for(int i = 0;i < 4;i++){ + printf("BASE:%d\n",i); + printf("latitudeK:%f,longitudeK:%f\n", + agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)); + } + } +} + ///////////////////////////////////////// // //Send_Request_to_base @@ -332,14 +382,26 @@ //Check Command Type switch(Command_type){ //Get Request command + case MANUAL:{ + Plus_Speed(buf1); + break; + } case STATUS_REQUEST:{ Send_Status(SenderIDc); break; } + case CHANGE_MODE:{ + New_Mode(buf1); + break; + } case RECEIVE_STATUS:{ Get_Status(SenderIDc,buf1); break; } + case RECEIVE_KALMAN:{ + Get_Status_Kalman(SenderIDc, buf1); + break; + } default:{ break; }