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@42:69b4cb31aafc, 2017-03-17 (annotated)

Committer:
s1200058
Date:
Fri Mar 17 09:04:13 2017 +0000
Revision:
42:69b4cb31aafc
Parent:
41:55539183dbb0
Child:
43:a05f2571151f
3/22-23????; ??????????????

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 42:69b4cb31aafc 34 const char SenderIDc = 'e';
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 42:69b4cb31aafc 155 void Get_Status(uint8_t *packetdata){
s1200058 42:69b4cb31aafc 156
s1200058 42:69b4cb31aafc 157 agz.GetData(&packetdata[8],&packetdata[10],&packetdata[12],&packetdata[14],&packetdata[16],&packetdata[18]);
s1200058 42:69b4cb31aafc 158 printf("%d, %d, %d, %d, %d, %d, %d\n\r", agz.r_gx, agz.r_gy, agz.r_gz, agz.r_ax, agz.r_ay, agz.r_az, packetdata[20]);
s1200058 42:69b4cb31aafc 159
s1200058 42:69b4cb31aafc 160 }
s1200058 42:69b4cb31aafc 161
s1200058 42:69b4cb31aafc 162
s1200058 39:1634312cf621 163 /////////////////////////////////////////
s1200058 39:1634312cf621 164 //
s1200058 39:1634312cf621 165 //Update Sensor
s1200058 39:1634312cf621 166 //
s1200058 39:1634312cf621 167 /////////////////////////////////////////
s1200058 39:1634312cf621 168
s1200058 39:1634312cf621 169 void update_gyro() {
s1200058 39:1634312cf621 170 set.setGyr((int16_t)gyr.getGyroX(),(int16_t)gyr.getGyroY(),(int16_t)gyr.getGyroZ());
s1200058 39:1634312cf621 171 }
s1200058 39:1634312cf621 172
s1200058 39:1634312cf621 173 void update_accel() {
s1200058 39:1634312cf621 174 acc.getOutput(readings);
s1200058 39:1634312cf621 175 set.setAcc(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
s1200058 39:1634312cf621 176 }
s1200058 39:1634312cf621 177
s1200058 39:1634312cf621 178 void update_cmp(){
s1200058 39:1634312cf621 179 cmp.readData(readings);
s1200058 39:1634312cf621 180 set.setCmp(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
s1200058 39:1634312cf621 181 }
s1200058 39:1634312cf621 182
s1200058 39:1634312cf621 183 /////////////////////////////////////////
s1200058 39:1634312cf621 184 //
s1200058 39:1634312cf621 185 //Set Up Sensor
s1200058 39:1634312cf621 186 //
s1200058 39:1634312cf621 187 /////////////////////////////////////////
s1200058 39:1634312cf621 188 void setup_cmp() {
s1200058 39:1634312cf621 189 // Continuous mode, , 10Hz measurement rate.
s1200058 39:1634312cf621 190 // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
s1200058 39:1634312cf621 191 cmp.setDefault();
s1200058 39:1634312cf621 192 // Wait some time(Need at least 5ms)
s1200058 39:1634312cf621 193 wait(0.1);
s1200058 39:1634312cf621 194 cmp.getAddress(buffer);
s1200058 39:1634312cf621 195 pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
s1200058 39:1634312cf621 196 }
s1200058 39:1634312cf621 197
s1200058 39:1634312cf621 198 bool setup_acc() {
s1200058 39:1634312cf621 199 // These are here to test whether any of the initialization fails. It will print the failure
s1200058 39:1634312cf621 200 if (acc.setPowerControl(0x00)) {
s1200058 39:1634312cf621 201 pc.printf("acc: didn't intitialize power control\n");
s1200058 39:1634312cf621 202 return false;
s1200058 39:1634312cf621 203 }
s1200058 39:1634312cf621 204 wait(.001);
s1200058 39:1634312cf621 205
s1200058 39:1634312cf621 206 //Full resolution, +/-16g, 4mg/LSB.
s1200058 39:1634312cf621 207 //if(acc.setDataFormatControl(0x0B)){
s1200058 39:1634312cf621 208 if(acc.setDataFormatControl(0x09)){ // +/- 4G
s1200058 39:1634312cf621 209 pc.printf("didn't set data format\n");
s1200058 39:1634312cf621 210 return false; }
s1200058 39:1634312cf621 211 wait(.001);
s1200058 39:1634312cf621 212
s1200058 39:1634312cf621 213 //200Hz data rate.
s1200058 39:1634312cf621 214 if(acc.setDataRate(ADXL345_200HZ)){
s1200058 39:1634312cf621 215 pc.printf("didn't set data rate\n");
s1200058 39:1634312cf621 216 return false; }
s1200058 39:1634312cf621 217 wait(.001);
s1200058 39:1634312cf621 218
s1200058 39:1634312cf621 219 //Measurement mode.
s1200058 39:1634312cf621 220 if(acc.setPowerControl(MeasurementMode)) {
s1200058 39:1634312cf621 221 pc.printf("didn't set the power control to measurement\n");
s1200058 39:1634312cf621 222 return false; }
s1200058 39:1634312cf621 223
s1200058 39:1634312cf621 224 pc.printf("Acc Id=%x \n\r", acc.getDeviceID());
s1200058 39:1634312cf621 225 pc.printf("%c" ,13,10);
s1200058 39:1634312cf621 226 return true;
s1200058 39:1634312cf621 227 }
s1200058 39:1634312cf621 228
s1200058 39:1634312cf621 229 void setup_gyr() {
s1200058 39:1634312cf621 230 //Set highest bandwidth.
s1200058 39:1634312cf621 231 gyr.setLpBandwidth(LPFBW_42HZ);
s1200058 39:1634312cf621 232 pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI());
s1200058 39:1634312cf621 233 pc.printf("%c" ,13,10);
s1200058 39:1634312cf621 234 }
s1200058 39:1634312cf621 235
s1200058 39:1634312cf621 236 /////////////////////////////////////////
s1200058 39:1634312cf621 237 //
kityann 0:daab5accfd83 238 //Main Processing
kityann 0:daab5accfd83 239 //
kityann 0:daab5accfd83 240 /////////////////////////////////////////
kityann 0:daab5accfd83 241 int main() {
kityann 0:daab5accfd83 242 //start up time
kityann 0:daab5accfd83 243 wait(3);
kityann 0:daab5accfd83 244 //set pc frequency to 57600bps
kityann 0:daab5accfd83 245 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 246 //set xbee frequency to 57600bps
kityann 0:daab5accfd83 247 xbee.begin(XBEE_BAUD_RATE);
kityann 0:daab5accfd83 248
kityann 0:daab5accfd83 249
kityann 0:daab5accfd83 250 //GPS setting
kityann 0:daab5accfd83 251 gps_Serial = new Serial(p28,p27);
kityann 0:daab5accfd83 252 Adafruit_GPS myGPS(gps_Serial);
s1200058 39:1634312cf621 253 setup_acc();
s1200058 39:1634312cf621 254 setup_gyr();
s1200058 38:5cd6d4964f65 255
s1200058 3:1ac506a96fd6 256 Timer auto_Timer;
s1200058 42:69b4cb31aafc 257 const int auto_Time = 200; //refresh time in ms
kityann 0:daab5accfd83 258
s1200058 29:524684a1198f 259 wait_ms(2000);
kityann 0:daab5accfd83 260
kityann 0:daab5accfd83 261 //interrupt start
s1200058 3:1ac506a96fd6 262 auto_Timer.start();
s1200058 30:7f6ebe2121d9 263 agz.Move_Timer.start();
s1200058 29:524684a1198f 264 printf("start\n");
kityann 1:b2b950b916ce 265
kityann 0:daab5accfd83 266
kityann 0:daab5accfd83 267 while (true) {
kityann 0:daab5accfd83 268
kityann 0:daab5accfd83 269 //Check Xbee Buffer Available
kityann 0:daab5accfd83 270 xbee.readPacket();
s1200058 39:1634312cf621 271
s1200058 39:1634312cf621 272 update_accel();
s1200058 39:1634312cf621 273 update_gyro();
kityann 0:daab5accfd83 274
kityann 0:daab5accfd83 275 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 276 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 277 uint8_t *buf = zbRx.getFrameData();
kityann 0:daab5accfd83 278
kityann 0:daab5accfd83 279 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 280 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 281 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
s1200058 21:76e7f36df4a9 282 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
s1200058 42:69b4cb31aafc 283 uint8_t *buf1 = &buf[11];
kityann 0:daab5accfd83 284
kityann 0:daab5accfd83 285 //Check Command Type
kityann 0:daab5accfd83 286 switch(Command_type){
kityann 0:daab5accfd83 287 //Get Request command
s1200058 2:886fac7f4399 288 case MANUAL:{
s1200058 12:48ef69b4f0e4 289 Plus_Speed(buf);
s1200058 2:886fac7f4399 290 break;
s1200058 2:886fac7f4399 291 }
s1200058 39:1634312cf621 292 case STATUS_REQUEST:{
kityann 0:daab5accfd83 293 break;
kityann 0:daab5accfd83 294 }
s1200058 2:886fac7f4399 295 case CHANGE_MODE:{
s1200058 12:48ef69b4f0e4 296 New_Mode(buf);
s1200058 42:69b4cb31aafc 297 if(flag == 0){
s1200058 42:69b4cb31aafc 298 flag = 1;
s1200058 42:69b4cb31aafc 299 }
s1200058 42:69b4cb31aafc 300 else{
s1200058 42:69b4cb31aafc 301 flag = 0;
s1200058 42:69b4cb31aafc 302 }
s1200058 2:886fac7f4399 303 break;
s1200058 2:886fac7f4399 304 }
s1200058 42:69b4cb31aafc 305 //status受信後
kityann 1:b2b950b916ce 306 case RECEIVE_STATUS:{
s1200058 42:69b4cb31aafc 307 Get_Status(buf1);
kityann 1:b2b950b916ce 308 break;
kityann 1:b2b950b916ce 309 }
kityann 0:daab5accfd83 310 default:{
kityann 0:daab5accfd83 311 break;
kityann 0:daab5accfd83 312 }
kityann 0:daab5accfd83 313 }//endswitch
kityann 0:daab5accfd83 314 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 315 }//endifisAvailable
s1200058 38:5cd6d4964f65 316
s1200058 42:69b4cb31aafc 317 //200msごとにstatus送信
s1200058 41:55539183dbb0 318 if(auto_Timer.read_ms() >= auto_Time){
s1200058 29:524684a1198f 319 auto_Timer.reset();
s1200058 42:69b4cb31aafc 320 //Send_Status();
s1200058 29:524684a1198f 321 }
s1200058 29:524684a1198f 322
s1200058 29:524684a1198f 323 }
s1200058 34:9c3192c00e41 324
kityann 0:daab5accfd83 325 }