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@39:1634312cf621, 2017-03-11 (annotated)

Committer:
s1200058
Date:
Sat Mar 11 14:57:44 2017 +0000
Revision:
39:1634312cf621
Parent:
38:5cd6d4964f65
Child:
40:656858844814
This is for experiment in 22-23 March

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