Wallbot_CaaS
Dependencies: MPU6050 mbed PID
Fork of BLE_MPU6050_test6_challenge_sb by
wallbotble.cpp
00001 /* JKsoft Wallbot BLE Library 00002 * 00003 * wallbotble.cpp 00004 * 00005 * Copyright (c) 2010-2014 jksoft 00006 * 00007 * Permission is hereby granted, free of charge, to any person obtaining a copy 00008 * of this software and associated documentation files (the "Software"), to deal 00009 * in the Software without restriction, including without limitation the rights 00010 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00011 * copies of the Software, and to permit persons to whom the Software is 00012 * furnished to do so, subject to the following conditions: 00013 * 00014 * The above copyright notice and this permission notice shall be included in 00015 * all copies or substantial portions of the Software. 00016 * 00017 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00018 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00019 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00020 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00021 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00022 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00023 * THE SOFTWARE. 00024 */ 00025 00026 #include "mbed.h" 00027 #include "wallbotble.h" 00028 00029 00030 00031 wallbotble::wallbotble() : 00032 _right_motor(P0_28,P0_30,P0_0) , 00033 _left_motor(P0_29,P0_23,P0_24) , 00034 _sw(P0_16,P0_17), 00035 _outlow(P0_20), 00036 _statusled(P0_18,P0_19), 00037 //_f_sensor1(P0_2),_f_sensor2(P0_3),_f_sensor3(P0_4),_f_sensor4(P0_5), 00038 _f_sensor1(P0_5),_f_sensor2(P0_4),_f_sensor3(P0_3),_f_sensor4(P0_2), 00039 _right_enc(P0_8), 00040 _left_enc(P0_6), 00041 _ctrl_r(Kp, Ki, Kd, RATE), 00042 _ctrl_l(Kp, Ki, Kd, RATE) 00043 { 00044 _right_motor = 0.0; 00045 _left_motor = 0.0; 00046 _outlow = 0; 00047 _statusled = 0x3; 00048 _sw.mode(PullUp); 00049 //エンコーダのセットアップ(位相差がないのでA相のみを使う簡易エンコーダ) 00050 _right_enc.mode(PullNone); 00051 _left_enc.mode(PullNone); 00052 00053 _right_pulses = 0; 00054 _left_pulses =0; 00055 00056 _right_timer.start(); 00057 _left_timer.start(); 00058 00059 _right_enc.rise(this, &wallbotble::right_count); 00060 _right_enc.fall(this, &wallbotble::right_count); 00061 00062 _left_enc.rise(this, &wallbotble::left_count); 00063 _left_enc.fall(this, &wallbotble::left_count); 00064 00065 //ラインセンサキャリブレーション値のリセット 00066 for(int i = 0 ; i < 4 ; i++) 00067 { 00068 _calibratedMinimum[i] = 65535; 00069 _calibratedMaximum[i] = 0; 00070 } 00071 //PIDの初期化 00072 _ctrl_r.setInputLimits(0,100); 00073 _ctrl_l.setInputLimits(0,100); 00074 _ctrl_r.setOutputLimits(0, 1.0); 00075 _ctrl_l.setOutputLimits(0, 1.0); 00076 00077 _right_back = false; 00078 _left_back = false; 00079 00080 } 00081 void wallbotble::control_enable(bool enable){ 00082 if(enable) 00083 _control_tic.attach(this, &wallbotble::tick_callback,RATE); 00084 else 00085 _control_tic.detach(); 00086 00087 _ctrl_r.reset(); 00088 _ctrl_l.reset(); 00089 00090 } 00091 void wallbotble::tick_callback(){ 00092 //現状態を与える 00093 _ctrl_r.setProcessValue(get_right_rpm()); 00094 _ctrl_l.setProcessValue(get_left_rpm()); 00095 00096 //制御指令 00097 if(_right_back) 00098 _right_motor = -1*_ctrl_r.compute() + ff_r; 00099 else 00100 _right_motor = _ctrl_r.compute() + ff_r; 00101 00102 if(_left_back) 00103 _left_motor = -1*_ctrl_l.compute() + ff_l; 00104 else 00105 _left_motor = _ctrl_l.compute() + ff_l; 00106 00107 } 00108 00109 void wallbotble::SetRPM(float leftRPM, float rightRPM){ 00110 _right_back = false; 00111 _left_back = false; 00112 00113 ff_r = 0.022*rightRPM; 00114 ff_l = 0.022*leftRPM; 00115 00116 if(rightRPM < 0){ 00117 _right_back = true; 00118 rightRPM = -1 *rightRPM; 00119 } 00120 if(leftRPM < 0){ 00121 _left_back = true; 00122 leftRPM = -1 *leftRPM; 00123 } 00124 00125 _ctrl_r.setSetPoint(rightRPM); 00126 _ctrl_l.setSetPoint(leftRPM); 00127 } 00128 00129 float wallbotble::get_right_rpm(){ 00130 //500ms以上パルスが来ていなければ0 00131 if(_right_timer.read_ms() > 500) 00132 _right_rpm = 0; 00133 00134 return _right_rpm; 00135 } 00136 00137 float wallbotble::get_left_rpm(){ 00138 //500ms以上パルスが来ていなければ0 00139 if(_left_timer.read_ms() > 500) 00140 _left_rpm = 0; 00141 00142 return _left_rpm; 00143 } 00144 00145 void wallbotble::right_count(){ 00146 _right_pulses++; 00147 00148 static int _right_times[4]; 00149 00150 _right_times[_right_pulses%4] = _right_timer.read_ms(); 00151 00152 _right_timer.reset(); 00153 _right_timer.start(); 00154 00155 _right_rpm = (60.0/((float)(_right_times[0]+_right_times[1]+_right_times[2]+_right_times[3])/1000.0)) * 4.0/PulsesPerRev ; 00156 00157 } 00158 void wallbotble::left_count(){ 00159 _left_pulses++; 00160 00161 static int _left_times[4]; 00162 00163 _left_times[_left_pulses%4] = _left_timer.read_ms(); 00164 00165 _left_timer.reset(); 00166 _left_timer.start(); 00167 00168 _left_rpm = (60/((float)(_left_times[0]+_left_times[1]+_left_times[2]+_left_times[3])/1000))* 4/PulsesPerRev; 00169 } 00170 // 自動キャリブレ 00171 void wallbotble::auto_calibrate() 00172 { 00173 control_enable(false); 00174 Timer time; 00175 time.start(); 00176 left_turn(0.7); 00177 while(time.read_ms() < 1000) 00178 f_sensor_calibrate(); 00179 right_turn(0.7); 00180 while(time.read_ms() < 3000) 00181 f_sensor_calibrate(); 00182 left_turn(0.7); 00183 while(time.read_ms() < 4000) 00184 f_sensor_calibrate(); 00185 stop(); 00186 time.stop(); 00187 } 00188 00189 // キャリブレーションのリセット 00190 void wallbotble::resetCalibration(){ 00191 for(int i = 0 ; i < 4 ; i++) 00192 { 00193 _calibratedMinimum[i] = 65535; 00194 _calibratedMaximum[i] = 0; 00195 } 00196 } 00197 00198 void wallbotble::left_motor (float duty) { 00199 _left_motor = duty; 00200 } 00201 00202 void wallbotble::right_motor (float duty) { 00203 _right_motor = duty; 00204 } 00205 00206 void wallbotble::forward (float duty) { 00207 _left_motor = duty; 00208 _right_motor = duty; 00209 } 00210 00211 void wallbotble::backward (float duty) { 00212 _left_motor = -duty; 00213 _right_motor = -duty; 00214 } 00215 00216 void wallbotble::left_turn (float duty) { 00217 _left_motor = -duty; 00218 _right_motor = duty; 00219 } 00220 00221 void wallbotble::right_turn (float duty) { 00222 _left_motor = duty; 00223 _right_motor = -duty; 00224 } 00225 00226 void wallbotble::stop (void) { 00227 _right_motor = 0.0; 00228 _left_motor = 0.0; 00229 } 00230 00231 void wallbotble::set_led(char bit) { 00232 bit |= bit >> 2; 00233 _statusled = (bit ^ 0x03) & 0x03; 00234 } 00235 00236 void wallbotble::set_led1(char bit) { 00237 if( bit ) 00238 { 00239 bit = _statusled ^ 0x03; 00240 bit = bit | 0x1; 00241 } 00242 else 00243 { 00244 bit = _statusled ^ 0x03; 00245 bit = bit & 0x2; 00246 } 00247 set_led(bit); 00248 } 00249 00250 void wallbotble::set_led2(char bit) { 00251 if( bit ) 00252 { 00253 bit = _statusled ^ 0x03; 00254 bit = bit | 0x2; 00255 } 00256 else 00257 { 00258 bit = _statusled ^ 0x03; 00259 bit = bit & 0x1; 00260 } 00261 set_led(bit); 00262 } 00263 00264 /**センサ値を10回読み込み、最大値と最小値を取得する 00265 * センサ毎にライン→ライン外で2回この関数を呼ぶイメージ 00266 * 00267 */ 00268 00269 void wallbotble::f_sensor_calibrate(void) { 00270 00271 unsigned short max_sensor_values[4]; 00272 unsigned short min_sensor_values[4]; 00273 00274 for(char j=0;j<10;j++) 00275 { 00276 unsigned short sensor_values[4] = { _f_sensor1.read_u16(), 00277 _f_sensor2.read_u16(), 00278 _f_sensor3.read_u16(), 00279 _f_sensor4.read_u16() 00280 }; 00281 00282 for(int i=0;i<4;i++) 00283 { 00284 // set the max we found THIS time 00285 if(j == 0 || max_sensor_values[i] < sensor_values[i]) 00286 max_sensor_values[i] = sensor_values[i]; 00287 00288 // set the min we found THIS time 00289 if(j == 0 || min_sensor_values[i] > sensor_values[i]) 00290 min_sensor_values[i] = sensor_values[i]; 00291 } 00292 } 00293 00294 // record the min and max calibration values 00295 for(char i=0;i<4;i++) 00296 { 00297 if(min_sensor_values[i] > _calibratedMaximum[i]) //より大きな最小値へ更新 00298 _calibratedMaximum[i] = min_sensor_values[i]; 00299 if(max_sensor_values[i] < _calibratedMinimum[i]) //より小さな最大値へ更新 00300 _calibratedMinimum[i] = max_sensor_values[i]; 00301 } 00302 00303 } 00304 00305 // ラインの推定位置を返す。-1500~0(中央)~1500 00306 // ラインセンサの出力が低すぎる場合(ロスト)は、前回値から判定し、0か3000を返す 00307 short wallbotble::GetLinePosition(void) { 00308 unsigned char i, on_line = 0; 00309 unsigned long avg = 0; // this is for the weighted total, which is long before division 00310 unsigned int sum = 0; // this is for the denominator which is <= 64000 00311 static int last_value=0; // assume initially that the line is left. 00312 00313 readCalibrated(); 00314 00315 for(i=0;i<4;i++) { 00316 int value = sensor_values[i]; 00317 00318 // keep track of whether we see the line at all 00319 if(value > 450) { 00320 on_line = 1; 00321 } 00322 00323 // only average in values that are above a noise threshold 00324 if(value > 50) { 00325 avg += (long)(value) * (i * 1000); 00326 sum += value; 00327 } 00328 } 00329 00330 if(!on_line)//ラインをロストしていたら、前回値から右端、左端として出力する 00331 { 00332 if(last_value < 0) 00333 return -1500; 00334 else 00335 return 1500; 00336 00337 } 00338 00339 last_value = avg/sum -1500; 00340 00341 return last_value; 00342 } 00343 00344 // ラインセンサ値を補正し0から1000にマッピングしてsensor_valuesにロードする。 00345 // 0 はキャリブレートmin以下を、1000はキャリブレートmax以上を意味します。 00346 // キャリブレートmin,maxはセンサ毎にストアされています。 00347 void wallbotble::readCalibrated(){ 00348 unsigned short raw_values[4] = { _f_sensor1.read_u16(), 00349 _f_sensor2.read_u16(), 00350 _f_sensor3.read_u16(), 00351 _f_sensor4.read_u16() 00352 }; 00353 00354 #if DBG 00355 printf("raw(%d,%d,%d,%d)",raw_values[0],raw_values[1],raw_values[2],raw_values[3]); 00356 #endif 00357 00358 for(int i=0;i<4;i++) 00359 { 00360 unsigned int denominator = _calibratedMaximum[i] - _calibratedMinimum[i]; 00361 00362 signed int x = 0; 00363 if(denominator != 0) 00364 x = (((signed long)raw_values[i]) - _calibratedMinimum[i]) * 1000 / denominator; 00365 00366 if(x < 0) 00367 x = 0; 00368 else if(x > 1000) 00369 x = 1000; 00370 00371 sensor_values[i] = x; 00372 } 00373 00374 } 00375 00376 int wallbotble::GetSw(void) { 00377 return(_sw ^ 0x3); 00378 } 00379 00380
Generated on Tue Jul 12 2022 18:33:09 by 1.7.2