Wallbot_CaaS
Dependencies: MPU6050 mbed PID
Fork of BLE_MPU6050_test6_challenge_sb by
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
Generated on Tue Jul 12 2022 18:33:09 by
1.7.2
