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