Wallbot_CaaS

Dependencies:   MPU6050 mbed PID

Fork of BLE_MPU6050_test6_challenge_sb by Junichi Katsu

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #include "BLEDevice.h"
00004 #include "wallbotble.h"
00005 //#include "RCBController.h"
00006 //#include "Adafruit_LEDBackpack.h"
00007 //#include "Adafruit_GFX.h"
00008 //#include "pictLIB.h"
00009 
00010 //#define DEBUG
00011 
00012 Serial pc(USBTX, USBRX);
00013 BLEDevice  ble;
00014 MPU6050 mpu(I2C_SDA,I2C_SCL);
00015 wallbotble wb;
00016 
00017 //------------------------------------------------------------
00018 //Service & Characteristic Setting
00019 //------------------------------------------------------------ 
00020 //Service UUID
00021 static const uint16_t base_uuid = 0xFFF0;
00022 
00023 //Characteristic UUID
00024 static const uint16_t cmd_uuid = 0xFFF1;
00025 static const uint16_t sen_uuid = 0xFFF2;
00026 static const uint16_t mpu_uuid = 0xFFF3;
00027  
00028 //Characteristic buffer ?
00029 uint8_t cmdPayload[8] = {0,};
00030 uint8_t senPayload[10] = {0,};
00031 uint8_t mpuPayload[12] = {0,};
00032  
00033 
00034 //Characteristic Property Setting etc
00035 GattCharacteristic  cmdChar (cmd_uuid, cmdPayload, sizeof(cmdPayload), sizeof(cmdPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE);
00036 GattCharacteristic  senChar (sen_uuid, senPayload, sizeof(senPayload), sizeof(senPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00037 GattCharacteristic  mpuChar (mpu_uuid, mpuPayload, sizeof(mpuPayload), sizeof(mpuPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00038 GattCharacteristic *myChars[] = {&cmdChar, &senChar, &mpuChar};
00039  
00040 //Service Setting
00041 GattService         myService(base_uuid, myChars, sizeof(myChars) / sizeof(GattCharacteristic *));
00042 
00043 //======================================================================
00044 //onDisconnection
00045 //======================================================================
00046 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00047 {   
00048     wb.control_enable(false);
00049     wb.stop();
00050 
00051     ble.startAdvertising();
00052     wb.set_led2(0);
00053 #ifdef DEBUG
00054     //割り込みハンドラの中でシリアル通信しないこと。
00055     //pc.printf("Disconnected\n\r");
00056 #endif
00057 }
00058 //======================================================================
00059 //onConnection
00060 //======================================================================
00061 void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params)
00062 {
00063     wb.set_led2(1);
00064     
00065     wb.control_enable(true);
00066     wb.SetRPM(0,0);
00067 #ifdef DEBUG
00068     //割り込みハンドラの中でシリアル通信しないこと。
00069     //pc.printf("Connected\n\r");
00070 #endif
00071 }
00072 
00073 //======================================================================
00074 //onDataWritten
00075 //======================================================================
00076 
00077 void onDataWritten(const GattCharacteristicWriteCBParams *params)
00078 {   
00079     float right_cmd = 0;
00080     float left_cmd = 0;
00081     memcpy( &right_cmd, params->data , sizeof(right_cmd));
00082     memcpy( &left_cmd, params->data + sizeof(right_cmd), sizeof(left_cmd));
00083     wb.SetRPM(left_cmd,right_cmd);
00084 #ifdef DEBUG
00085     //割り込みハンドラの中でシリアル通信しないこと。
00086     //printf("SetRPM:%f,%f",left_cmd,right_cmd)
00087 #endif
00088 }
00089 /**************************************************************************/
00090 /*!
00091     @brief  Program entry point
00092 */
00093 /**************************************************************************/
00094 int main(void)
00095 {
00096 
00097     int16_t mpu_a[3];
00098     int16_t mpu_g[3];
00099     short line;
00100     float rrpm,lrpm;
00101 
00102     uint8_t sen_buf[10];
00103     uint8_t mpu_buf[12];
00104 
00105     pc.baud(9600);
00106     
00107     //MPU 成功するまでリトライ
00108     wb.set_led1(1);
00109     wait(1);    
00110     
00111     if(!mpu.testConnection()){
00112         pc.printf("MPU connection fail, -> soft reset \n\r");
00113         NVIC_SystemReset();
00114     }
00115 
00116     wb.set_led1(0);
00117     pc.printf("MPU6050 initialize passed \n\r");
00118 
00119     
00120     const uint8_t address1[] = {0xe7,0xAA,0xAA,0xAA,0xAA,0xAA};
00121     ble.setAddress(Gap::ADDR_TYPE_PUBLIC, address1);
00122     
00123     ble.init();
00124     //イベント時のコールバック関数
00125     ble.onConnection(onConnected);
00126     ble.onDisconnection(onDisconnected);
00127     ble.onDataWritten(onDataWritten);
00128 
00129     /* setup advertising */
00130     //クラシックBTはサポートせず、BLEデバイスとして認識してもらう
00131     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00132     //デバイスがセントラルに接続可能であることを設定
00133     ble.setAdvertisingType (GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00134     //LOCAL NAMEの設定、BLEでは終端記号は不要なので-1する
00135     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
00136 
00137     //16Bit短縮UUIDの設定
00138     //HACK:リバースが必要?「mbed BLE APIで128bit UUIDを含むGATT Serviceを2つ以上登録すると Service UUIDがひっくり返る?」
00139     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,(const uint8_t *)base_uuid, sizeof(base_uuid));
00140 
00141     //Advertiseパケットの送信周期
00142     ble.setAdvertisingInterval (160); /* 100ms; in multiples of 0.625ms. */
00143 
00144     //サービスの登録
00145     ble.addService(myService);
00146 
00147     //Advertizeの開始
00148     ble.startAdvertising();
00149 
00150     //ラインセンサキャリブレーション
00151     //pc.printf("auto calibrate start\n\r");
00152 
00153     //適当キャリブレーション
00154     /*
00155     for(int i = 0 ; i < 4 ; i++)
00156     {
00157         wb._calibratedMinimum[i] = 300;
00158         wb._calibratedMaximum[i] = 900;
00159     }
00160     */
00161 #ifndef DEBUG
00162     wb.auto_calibrate(); //自動キャリブレーション
00163 #endif
00164 #ifdef DEBUG
00165     for(int i = 0 ; i < 4 ; i++)
00166     {
00167         pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]);
00168     }
00169     pc.printf("\n\rauto calibrate end\n\r");
00170 #endif
00171     
00172 #if 0 //エンコーダキャリブレーション用コード
00173     BusIn enc(P0_8,P0_10,P0_6,P0_7);
00174     enc.mode(PullNone);
00175     while(1){
00176         char c = enc.read();
00177         pc.putc(c);
00178         wait_ms(10);
00179     }
00180 #endif
00181 
00182     //速度制御
00183     //wb.control_enable(true);
00184     //Timer time;
00185     //int now;
00186     //time.start();
00187     while (true) {
00188         ///////////////////PID調整
00189         //now = time.read();
00190         //if((now % 10) == 0){
00191         //  pc.printf("set RPM %d ",now % 50 +10);
00192         //  wb.SetRPM(now % 50 +10,now % 50 +10);
00193         //}
00194         ///////////////////////
00195         
00196         //センサ値の取得
00197         line = wb.GetLinePosition();
00198         rrpm = wb.get_right_rpm();
00199         lrpm = wb.get_left_rpm();
00200         mpu.getAcceleroRaw(mpu_a);
00201         mpu.getGyroRaw(mpu_g);
00202         
00203         //送信の準備(リトルエンディアン)
00204         memcpy(sen_buf                          ,&line,sizeof(line));
00205         memcpy(sen_buf+sizeof(line)             ,&rrpm,sizeof(rrpm));
00206         memcpy(sen_buf+sizeof(line)+sizeof(rrpm),&lrpm,sizeof(lrpm));
00207         
00208         memcpy(mpu_buf              ,mpu_a,sizeof(mpu_a));
00209         memcpy(mpu_buf+sizeof(mpu_a),mpu_g,sizeof(mpu_g));
00210         
00211         //送信
00212         ble.updateCharacteristicValue (senChar.getValueAttribute().getHandle(), (uint8_t *)sen_buf, sizeof(sen_buf));
00213         ble.updateCharacteristicValue (mpuChar.getValueAttribute().getHandle(), (uint8_t *)mpu_buf, sizeof(mpu_buf));
00214         
00215         //
00216         #ifdef DEBUG
00217             pc.printf("Pulse(%d,%d)",wb._left_pulses,wb._right_pulses);
00218             pc.printf("RPM(%.2f,%.2f)",lrpm, rrpm);
00219             pc.printf("LinePos:%d ",line);
00220             pc.printf("Sens(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]);
00221             pc.printf("MPU(%d,%d,%d,  %d,%d,%d)",mpu_a[0],mpu_a[1],mpu_a[2],mpu_g[0],mpu_g[1],mpu_g[2]);
00222         
00223             pc.printf("\n\r");
00224         #endif
00225         wait_ms(10);
00226   
00227     }
00228 }
00229