Mami Yokokawa / Mbed 2 deprecated Aigamozu_Robot_March

3/22-23の実験用 Send_Status(): 加速度・ジャイロをパケットで相手に送信 Get_Status():受信したパケットを表示

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

Fork of Aigamozu_Robot_展示会 by aigamozu

main.cpp@38:5cd6d4964f65, 2016-10-24 (annotated)

Committer:
s1200058
Date:
Mon Oct 24 08:15:16 2016 +0000
Revision:
38:5cd6d4964f65
Parent:
37:19a9a37a5658
Child:
39:1634312cf621
??????

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
kityann 0:daab5accfd83 27 /////////////////////////////////////////
kityann 0:daab5accfd83 28 //
kityann 0:daab5accfd83 29 //Pin Setting
kityann 0:daab5accfd83 30 //
kityann 0:daab5accfd83 31 /////////////////////////////////////////
kityann 0:daab5accfd83 32 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:daab5accfd83 33
kityann 0:daab5accfd83 34
kityann 0:daab5accfd83 35 /////////////////////////////////////////
kityann 0:daab5accfd83 36 //
kityann 0:daab5accfd83 37 //Connection Setting
kityann 0:daab5accfd83 38 //
kityann 0:daab5accfd83 39 /////////////////////////////////////////
kityann 0:daab5accfd83 40
kityann 0:daab5accfd83 41 //Serial Connect Setting: PC <--> mbed
kityann 0:daab5accfd83 42 Serial pc(USBTX, USBRX);
kityann 0:daab5accfd83 43
kityann 0:daab5accfd83 44 //Serial Connect Setting: GPS <--> mbed
kityann 0:daab5accfd83 45 Serial * gps_Serial;
kityann 0:daab5accfd83 46
kityann 0:daab5accfd83 47 //Serial Connect Setting: XBEE <--> mbed
kityann 0:daab5accfd83 48 XBee xbee(p13,p14);
kityann 0:daab5accfd83 49 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:daab5accfd83 50
kityann 0:daab5accfd83 51 //set up GPS module
kityann 0:daab5accfd83 52
kityann 0:daab5accfd83 53 //set up AigamozuControlPackets library
kityann 0:daab5accfd83 54 AigamozuControlPackets agz(agz_motorShield);
kityann 0:daab5accfd83 55
kityann 0:daab5accfd83 56
s1200058 38:5cd6d4964f65 57
s1200058 34:9c3192c00e41 58 int change = 0;
s1200058 34:9c3192c00e41 59
s1200058 2:886fac7f4399 60 /////////////////////////////////////////
s1200058 2:886fac7f4399 61 //
s1200058 2:886fac7f4399 62 //Plus Speed
s1200058 2:886fac7f4399 63 //
s1200058 2:886fac7f4399 64 //MNUAL_MODEの時にスピードを変える
s1200058 2:886fac7f4399 65 /////////////////////////////////////////
s1200058 2:886fac7f4399 66 void Plus_Speed(uint8_t *packetdata){
s1200058 2:886fac7f4399 67
s1200058 2:886fac7f4399 68 if(agz.nowMode == MANUAL_MODE){
s1200058 2:886fac7f4399 69 agz.changeSpeed(packetdata);
s1200058 2:886fac7f4399 70 }
s1200058 2:886fac7f4399 71
s1200058 2:886fac7f4399 72 }
kityann 0:daab5accfd83 73
kityann 24:698d4e920d33 74
s1200058 2:886fac7f4399 75
s1200058 2:886fac7f4399 76 /////////////////////////////////////////
s1200058 2:886fac7f4399 77 //
s1200058 2:886fac7f4399 78 //New Mode
s1200058 2:886fac7f4399 79 //
s1200058 2:886fac7f4399 80 /////////////////////////////////////////
s1200058 2:886fac7f4399 81
s1200058 2:886fac7f4399 82 void New_Mode(uint8_t *packetdata){
s1200058 2:886fac7f4399 83
s1200058 19:a806105ba365 84 //bool result;
s1200058 20:eee8ac8d1788 85 agz.changeMode(packetdata);
s1200058 2:886fac7f4399 86
s1200058 2:886fac7f4399 87 }
kityann 1:b2b950b916ce 88
kityann 0:daab5accfd83 89
kityann 0:daab5accfd83 90 /////////////////////////////////////////
kityann 0:daab5accfd83 91 //
kityann 0:daab5accfd83 92 //Main Processing
kityann 0:daab5accfd83 93 //
kityann 0:daab5accfd83 94 /////////////////////////////////////////
kityann 0:daab5accfd83 95 int main() {
kityann 0:daab5accfd83 96 //start up time
kityann 0:daab5accfd83 97 wait(3);
kityann 0:daab5accfd83 98 //set pc frequency to 57600bps
kityann 0:daab5accfd83 99 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 100 //set xbee frequency to 57600bps
kityann 0:daab5accfd83 101 xbee.begin(XBEE_BAUD_RATE);
kityann 0:daab5accfd83 102
kityann 0:daab5accfd83 103
kityann 0:daab5accfd83 104 //GPS setting
kityann 0:daab5accfd83 105 gps_Serial = new Serial(p28,p27);
kityann 0:daab5accfd83 106 Adafruit_GPS myGPS(gps_Serial);
s1200058 38:5cd6d4964f65 107
s1200058 3:1ac506a96fd6 108 Timer auto_Timer;
s1200058 3:1ac506a96fd6 109 const int auto_Time = 2000; //refresh time in ms
kityann 0:daab5accfd83 110
s1200058 29:524684a1198f 111 wait_ms(2000);
kityann 0:daab5accfd83 112
kityann 0:daab5accfd83 113 //interrupt start
s1200058 3:1ac506a96fd6 114 auto_Timer.start();
s1200058 30:7f6ebe2121d9 115 agz.Move_Timer.start();
s1200058 29:524684a1198f 116 printf("start\n");
kityann 1:b2b950b916ce 117
kityann 0:daab5accfd83 118
kityann 0:daab5accfd83 119 while (true) {
kityann 0:daab5accfd83 120
kityann 0:daab5accfd83 121 //Check Xbee Buffer Available
kityann 0:daab5accfd83 122 xbee.readPacket();
kityann 0:daab5accfd83 123
kityann 0:daab5accfd83 124 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 125 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 126 uint8_t *buf = zbRx.getFrameData();
kityann 0:daab5accfd83 127
kityann 0:daab5accfd83 128 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 129 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 130 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
s1200058 21:76e7f36df4a9 131 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:daab5accfd83 132
kityann 0:daab5accfd83 133 //Check Command Type
kityann 0:daab5accfd83 134 switch(Command_type){
kityann 0:daab5accfd83 135 //Get Request command
s1200058 2:886fac7f4399 136 case MANUAL:{
s1200058 12:48ef69b4f0e4 137 Plus_Speed(buf);
s1200058 2:886fac7f4399 138 break;
s1200058 2:886fac7f4399 139 }
s1200058 38:5cd6d4964f65 140 case STATUS_REQUEST:{
kityann 0:daab5accfd83 141 break;
kityann 0:daab5accfd83 142 }
s1200058 2:886fac7f4399 143 case CHANGE_MODE:{
s1200058 12:48ef69b4f0e4 144 New_Mode(buf);
s1200058 2:886fac7f4399 145 break;
s1200058 2:886fac7f4399 146 }
kityann 1:b2b950b916ce 147 case RECEIVE_STATUS:{
kityann 1:b2b950b916ce 148 break;
kityann 1:b2b950b916ce 149 }
kityann 0:daab5accfd83 150 default:{
kityann 0:daab5accfd83 151 break;
kityann 0:daab5accfd83 152 }
kityann 0:daab5accfd83 153 }//endswitch
kityann 0:daab5accfd83 154 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 155 }//endifisAvailable
s1200058 38:5cd6d4964f65 156
s1200058 38:5cd6d4964f65 157 if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
s1200058 29:524684a1198f 158 auto_Timer.reset();
s1200058 38:5cd6d4964f65 159 agz.gpsAuto();
s1200058 29:524684a1198f 160 }
s1200058 29:524684a1198f 161
s1200058 29:524684a1198f 162 }
s1200058 34:9c3192c00e41 163
kityann 0:daab5accfd83 164 }