aigamozu / Mbed 2 deprecated Aigamozu_Robot_templete

展示会用に簡単にしています

Dependencies:   ADXL345 AigamozuControlPackets_展示会 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_March by Mami Yokokawa

main.cpp@41:55539183dbb0, 2017-03-13 (annotated)

Committer:
s1200058
Date:
Mon Mar 13 17:55:45 2017 +0000
Revision:
41:55539183dbb0
Parent:
40:656858844814
Child:
42:69b4cb31aafc
????????; ???????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:daab5accfd83 1 /**********************************************/
kityann 0:daab5accfd83 2 //
kityann 0:daab5accfd83 3 //
kityann 0:daab5accfd83 4 //
kityann 0:daab5accfd83 5 // Program name: Aigamozu ROBOT
kityann 0:daab5accfd83 6 // Author: Mineta Kizuku
s1200058 38:5cd6d4964f65 7 // Yokokawa
s1200058 21:76e7f36df4a9 8 //
kityann 0:daab5accfd83 9 //
kityann 0:daab5accfd83 10 /**********************************************/
kityann 0:daab5accfd83 11
kityann 0:daab5accfd83 12 /**********************************************/
kityann 0:daab5accfd83 13 //更新情報
s1200058 40:656858844814 14 //実験用プログラム
s1200058 40:656858844814 15 //ジャイロ・加速度データをXbeeに送り続けます。
s1200058 40:656858844814 16 //StatusRequestCommandを送るとflagが0から1になります。
kityann 0:daab5accfd83 17 /**********************************************/
kityann 0:daab5accfd83 18
kityann 0:daab5accfd83 19 #include "mbed.h"
kityann 0:daab5accfd83 20 #include "XBee.h"
kityann 0:daab5accfd83 21 #include "MBed_Adafruit_GPS.h"
kityann 0:daab5accfd83 22 #include "AigamozuControlPackets.h"
kityann 0:daab5accfd83 23 #include "agzIDLIST.h"
kityann 0:daab5accfd83 24 #include "aigamozuSetting.h"
s1200058 34:9c3192c00e41 25 #include "math.h"
kityann 0:daab5accfd83 26
s1200058 39:1634312cf621 27 #include "HMC5843.h"
s1200058 39:1634312cf621 28 #include "ADXL345.h"
s1200058 39:1634312cf621 29 #include "ITG3200.h"
s1200058 39:1634312cf621 30 #include "setting.h"
s1200058 39:1634312cf621 31
s1200058 39:1634312cf621 32 //************ID Number*****************
s1200058 39:1634312cf621 33 const char MyID = 'b';
s1200058 39:1634312cf621 34 const char SenderIDc = 'h';
s1200058 39:1634312cf621 35 //************ID Number*****************
s1200058 39:1634312cf621 36
kityann 0:daab5accfd83 37 /////////////////////////////////////////
kityann 0:daab5accfd83 38 //
kityann 0:daab5accfd83 39 //Pin Setting
kityann 0:daab5accfd83 40 //
kityann 0:daab5accfd83 41 /////////////////////////////////////////
kityann 0:daab5accfd83 42 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
s1200058 39:1634312cf621 43 HMC5843 cmp(p9, p10); // sda, scl
s1200058 39:1634312cf621 44 ADXL345 acc(p9, p10); // sda, scl
s1200058 39:1634312cf621 45 ITG3200 gyr(p9, p10); // sda, scl
s1200058 39:1634312cf621 46 setting set;
kityann 0:daab5accfd83 47
kityann 0:daab5accfd83 48
kityann 0:daab5accfd83 49 /////////////////////////////////////////
kityann 0:daab5accfd83 50 //
kityann 0:daab5accfd83 51 //Connection Setting
kityann 0:daab5accfd83 52 //
kityann 0:daab5accfd83 53 /////////////////////////////////////////
kityann 0:daab5accfd83 54
kityann 0:daab5accfd83 55 //Serial Connect Setting: PC <--> mbed
kityann 0:daab5accfd83 56 Serial pc(USBTX, USBRX);
kityann 0:daab5accfd83 57
kityann 0:daab5accfd83 58 //Serial Connect Setting: GPS <--> mbed
kityann 0:daab5accfd83 59 Serial * gps_Serial;
kityann 0:daab5accfd83 60
kityann 0:daab5accfd83 61 //Serial Connect Setting: XBEE <--> mbed
kityann 0:daab5accfd83 62 XBee xbee(p13,p14);
kityann 0:daab5accfd83 63 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:daab5accfd83 64
kityann 0:daab5accfd83 65 //set up GPS module
kityann 0:daab5accfd83 66
kityann 0:daab5accfd83 67 //set up AigamozuControlPackets library
kityann 0:daab5accfd83 68 AigamozuControlPackets agz(agz_motorShield);
kityann 0:daab5accfd83 69
kityann 0:daab5accfd83 70
s1200058 38:5cd6d4964f65 71
s1200058 34:9c3192c00e41 72 int change = 0;
s1200058 39:1634312cf621 73 //values x,y,z
s1200058 39:1634312cf621 74 int readings[3];
s1200058 39:1634312cf621 75 //ID Buffer
s1200058 39:1634312cf621 76 char buffer[3];
s1200058 39:1634312cf621 77
s1200058 39:1634312cf621 78 int flag =0;
s1200058 34:9c3192c00e41 79
s1200058 2:886fac7f4399 80 /////////////////////////////////////////
s1200058 2:886fac7f4399 81 //
s1200058 2:886fac7f4399 82 //Plus Speed
s1200058 2:886fac7f4399 83 //
s1200058 2:886fac7f4399 84 //MNUAL_MODEの時にスピードを変える
s1200058 2:886fac7f4399 85 /////////////////////////////////////////
s1200058 2:886fac7f4399 86 void Plus_Speed(uint8_t *packetdata){
s1200058 2:886fac7f4399 87
s1200058 2:886fac7f4399 88 if(agz.nowMode == MANUAL_MODE){
s1200058 2:886fac7f4399 89 agz.changeSpeed(packetdata);
s1200058 2:886fac7f4399 90 }
s1200058 2:886fac7f4399 91
s1200058 2:886fac7f4399 92 }
kityann 0:daab5accfd83 93
kityann 24:698d4e920d33 94
s1200058 2:886fac7f4399 95
s1200058 2:886fac7f4399 96 /////////////////////////////////////////
s1200058 2:886fac7f4399 97 //
s1200058 2:886fac7f4399 98 //New Mode
s1200058 2:886fac7f4399 99 //
s1200058 2:886fac7f4399 100 /////////////////////////////////////////
s1200058 2:886fac7f4399 101
s1200058 2:886fac7f4399 102 void New_Mode(uint8_t *packetdata){
s1200058 2:886fac7f4399 103
s1200058 19:a806105ba365 104 //bool result;
s1200058 20:eee8ac8d1788 105 agz.changeMode(packetdata);
s1200058 2:886fac7f4399 106
s1200058 2:886fac7f4399 107 }
kityann 1:b2b950b916ce 108
kityann 0:daab5accfd83 109
kityann 0:daab5accfd83 110 /////////////////////////////////////////
kityann 0:daab5accfd83 111 //
s1200058 39:1634312cf621 112 //Send_Status
s1200058 39:1634312cf621 113 //
s1200058 39:1634312cf621 114 //リクエストがきたとき、自分の位置情報などを返信する
s1200058 39:1634312cf621 115 /////////////////////////////////////////
s1200058 39:1634312cf621 116 void Send_Status(void){
s1200058 39:1634312cf621 117
s1200058 39:1634312cf621 118 XBeeAddress64 send_Address;
s1200058 39:1634312cf621 119 if(SenderIDc == '0'){
s1200058 39:1634312cf621 120 send_Address = manager_Address;
s1200058 39:1634312cf621 121 }
s1200058 39:1634312cf621 122 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
s1200058 39:1634312cf621 123 send_Address = robot_Address[SenderIDc - 'A'];
s1200058 39:1634312cf621 124 }
s1200058 39:1634312cf621 125 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
s1200058 39:1634312cf621 126 send_Address = base_Address[SenderIDc - 'a'];
s1200058 39:1634312cf621 127 }
s1200058 39:1634312cf621 128 //send normal data
s1200058 39:1634312cf621 129 //Create GPS Infomation Packet
s1200058 41:55539183dbb0 130 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);
s1200058 39:1634312cf621 131
s1200058 39:1634312cf621 132 /* //debug***************************************************
s1200058 39:1634312cf621 133 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
s1200058 39:1634312cf621 134 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 39:1634312cf621 135 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 39:1634312cf621 136 agz.get_agzCov_lati(),agz.get_agzCov_longi()
s1200058 39:1634312cf621 137 );
s1200058 39:1634312cf621 138 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
s1200058 39:1634312cf621 139 printf("\n");
s1200058 39:1634312cf621 140 //debug end***************************************************
s1200058 39:1634312cf621 141 */
s1200058 39:1634312cf621 142 //debug
s1200058 41:55539183dbb0 143 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);
s1200058 41:55539183dbb0 144 for(int i = 0; i < 24; i++){
s1200058 39:1634312cf621 145 printf("%d ", agz.packetData[i]);
s1200058 39:1634312cf621 146 }
s1200058 39:1634312cf621 147 printf("\n\r");
s1200058 39:1634312cf621 148
s1200058 39:1634312cf621 149 //Select Destination
s1200058 39:1634312cf621 150 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
s1200058 39:1634312cf621 151 //Send -> Base
s1200058 39:1634312cf621 152 xbee.send(tx64request);
s1200058 39:1634312cf621 153 }
s1200058 39:1634312cf621 154
s1200058 39:1634312cf621 155 /////////////////////////////////////////
s1200058 39:1634312cf621 156 //
s1200058 39:1634312cf621 157 //Update Sensor
s1200058 39:1634312cf621 158 //
s1200058 39:1634312cf621 159 /////////////////////////////////////////
s1200058 39:1634312cf621 160
s1200058 39:1634312cf621 161 void update_gyro() {
s1200058 39:1634312cf621 162 set.setGyr((int16_t)gyr.getGyroX(),(int16_t)gyr.getGyroY(),(int16_t)gyr.getGyroZ());
s1200058 39:1634312cf621 163 }
s1200058 39:1634312cf621 164
s1200058 39:1634312cf621 165 void update_accel() {
s1200058 39:1634312cf621 166 acc.getOutput(readings);
s1200058 39:1634312cf621 167 set.setAcc(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
s1200058 39:1634312cf621 168 }
s1200058 39:1634312cf621 169
s1200058 39:1634312cf621 170 void update_cmp(){
s1200058 39:1634312cf621 171 cmp.readData(readings);
s1200058 39:1634312cf621 172 set.setCmp(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
s1200058 39:1634312cf621 173 }
s1200058 39:1634312cf621 174
s1200058 39:1634312cf621 175 /////////////////////////////////////////
s1200058 39:1634312cf621 176 //
s1200058 39:1634312cf621 177 //Set Up Sensor
s1200058 39:1634312cf621 178 //
s1200058 39:1634312cf621 179 /////////////////////////////////////////
s1200058 39:1634312cf621 180 void setup_cmp() {
s1200058 39:1634312cf621 181 // Continuous mode, , 10Hz measurement rate.
s1200058 39:1634312cf621 182 // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
s1200058 39:1634312cf621 183 cmp.setDefault();
s1200058 39:1634312cf621 184 // Wait some time(Need at least 5ms)
s1200058 39:1634312cf621 185 wait(0.1);
s1200058 39:1634312cf621 186 cmp.getAddress(buffer);
s1200058 39:1634312cf621 187 pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
s1200058 39:1634312cf621 188 }
s1200058 39:1634312cf621 189
s1200058 39:1634312cf621 190 bool setup_acc() {
s1200058 39:1634312cf621 191 // These are here to test whether any of the initialization fails. It will print the failure
s1200058 39:1634312cf621 192 if (acc.setPowerControl(0x00)) {
s1200058 39:1634312cf621 193 pc.printf("acc: didn't intitialize power control\n");
s1200058 39:1634312cf621 194 return false;
s1200058 39:1634312cf621 195 }
s1200058 39:1634312cf621 196 wait(.001);
s1200058 39:1634312cf621 197
s1200058 39:1634312cf621 198 //Full resolution, +/-16g, 4mg/LSB.
s1200058 39:1634312cf621 199 //if(acc.setDataFormatControl(0x0B)){
s1200058 39:1634312cf621 200 if(acc.setDataFormatControl(0x09)){ // +/- 4G
s1200058 39:1634312cf621 201 pc.printf("didn't set data format\n");
s1200058 39:1634312cf621 202 return false; }
s1200058 39:1634312cf621 203 wait(.001);
s1200058 39:1634312cf621 204
s1200058 39:1634312cf621 205 //200Hz data rate.
s1200058 39:1634312cf621 206 if(acc.setDataRate(ADXL345_200HZ)){
s1200058 39:1634312cf621 207 pc.printf("didn't set data rate\n");
s1200058 39:1634312cf621 208 return false; }
s1200058 39:1634312cf621 209 wait(.001);
s1200058 39:1634312cf621 210
s1200058 39:1634312cf621 211 //Measurement mode.
s1200058 39:1634312cf621 212 if(acc.setPowerControl(MeasurementMode)) {
s1200058 39:1634312cf621 213 pc.printf("didn't set the power control to measurement\n");
s1200058 39:1634312cf621 214 return false; }
s1200058 39:1634312cf621 215
s1200058 39:1634312cf621 216 pc.printf("Acc Id=%x \n\r", acc.getDeviceID());
s1200058 39:1634312cf621 217 pc.printf("%c" ,13,10);
s1200058 39:1634312cf621 218 return true;
s1200058 39:1634312cf621 219 }
s1200058 39:1634312cf621 220
s1200058 39:1634312cf621 221 void setup_gyr() {
s1200058 39:1634312cf621 222 //Set highest bandwidth.
s1200058 39:1634312cf621 223 gyr.setLpBandwidth(LPFBW_42HZ);
s1200058 39:1634312cf621 224 pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI());
s1200058 39:1634312cf621 225 pc.printf("%c" ,13,10);
s1200058 39:1634312cf621 226 }
s1200058 39:1634312cf621 227
s1200058 39:1634312cf621 228 /////////////////////////////////////////
s1200058 39:1634312cf621 229 //
kityann 0:daab5accfd83 230 //Main Processing
kityann 0:daab5accfd83 231 //
kityann 0:daab5accfd83 232 /////////////////////////////////////////
kityann 0:daab5accfd83 233 int main() {
kityann 0:daab5accfd83 234 //start up time
kityann 0:daab5accfd83 235 wait(3);
kityann 0:daab5accfd83 236 //set pc frequency to 57600bps
kityann 0:daab5accfd83 237 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 238 //set xbee frequency to 57600bps
kityann 0:daab5accfd83 239 xbee.begin(XBEE_BAUD_RATE);
kityann 0:daab5accfd83 240
kityann 0:daab5accfd83 241
kityann 0:daab5accfd83 242 //GPS setting
kityann 0:daab5accfd83 243 gps_Serial = new Serial(p28,p27);
kityann 0:daab5accfd83 244 Adafruit_GPS myGPS(gps_Serial);
s1200058 39:1634312cf621 245 setup_acc();
s1200058 39:1634312cf621 246 setup_gyr();
s1200058 38:5cd6d4964f65 247
s1200058 3:1ac506a96fd6 248 Timer auto_Timer;
s1200058 41:55539183dbb0 249 const int auto_Time = 500; //refresh time in ms
kityann 0:daab5accfd83 250
s1200058 29:524684a1198f 251 wait_ms(2000);
kityann 0:daab5accfd83 252
kityann 0:daab5accfd83 253 //interrupt start
s1200058 3:1ac506a96fd6 254 auto_Timer.start();
s1200058 30:7f6ebe2121d9 255 agz.Move_Timer.start();
s1200058 29:524684a1198f 256 printf("start\n");
kityann 1:b2b950b916ce 257
kityann 0:daab5accfd83 258
kityann 0:daab5accfd83 259 while (true) {
kityann 0:daab5accfd83 260
kityann 0:daab5accfd83 261 //Check Xbee Buffer Available
kityann 0:daab5accfd83 262 xbee.readPacket();
s1200058 39:1634312cf621 263
s1200058 39:1634312cf621 264 update_accel();
s1200058 39:1634312cf621 265 update_gyro();
kityann 0:daab5accfd83 266
kityann 0:daab5accfd83 267 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 268 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 269 uint8_t *buf = zbRx.getFrameData();
kityann 0:daab5accfd83 270
kityann 0:daab5accfd83 271 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 272 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 273 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
s1200058 21:76e7f36df4a9 274 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:daab5accfd83 275
kityann 0:daab5accfd83 276 //Check Command Type
kityann 0:daab5accfd83 277 switch(Command_type){
kityann 0:daab5accfd83 278 //Get Request command
s1200058 2:886fac7f4399 279 case MANUAL:{
s1200058 12:48ef69b4f0e4 280 Plus_Speed(buf);
s1200058 2:886fac7f4399 281 break;
s1200058 2:886fac7f4399 282 }
s1200058 39:1634312cf621 283 case STATUS_REQUEST:{
s1200058 39:1634312cf621 284 flag = 1;
kityann 0:daab5accfd83 285 break;
kityann 0:daab5accfd83 286 }
s1200058 2:886fac7f4399 287 case CHANGE_MODE:{
s1200058 12:48ef69b4f0e4 288 New_Mode(buf);
s1200058 2:886fac7f4399 289 break;
s1200058 2:886fac7f4399 290 }
kityann 1:b2b950b916ce 291 case RECEIVE_STATUS:{
kityann 1:b2b950b916ce 292 break;
kityann 1:b2b950b916ce 293 }
kityann 0:daab5accfd83 294 default:{
kityann 0:daab5accfd83 295 break;
kityann 0:daab5accfd83 296 }
kityann 0:daab5accfd83 297 }//endswitch
kityann 0:daab5accfd83 298 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 299 }//endifisAvailable
s1200058 38:5cd6d4964f65 300
s1200058 41:55539183dbb0 301 if(auto_Timer.read_ms() >= auto_Time){
s1200058 29:524684a1198f 302 auto_Timer.reset();
s1200058 41:55539183dbb0 303 Send_Status();
s1200058 29:524684a1198f 304 }
s1200058 29:524684a1198f 305
s1200058 29:524684a1198f 306 }
s1200058 34:9c3192c00e41 307
kityann 0:daab5accfd83 308 }