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@43:a05f2571151f, 2017-03-17 (annotated)

Committer:
s1200058
Date:
Fri Mar 17 09:07:45 2017 +0000
Revision:
43:a05f2571151f
Parent:
42:69b4cb31aafc
Child:
44:02873ac29052
????????

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