wallbotble.h

00001 /* JKsoft Wallbot BLE Library
00002  *
00003  * wallbotble.h
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 #ifndef WALLBOT_BLE_H
00027 #define WALLBOT_BLE_H
00028 
00029 #include "mbed.h"
00030 #include "TB6612.h"
00031 //#include "QEI.h"
00032 #include "PID.h"
00033 
00034 #define PulsesPerRev    24
00035 
00036 #define Kp  0.0001
00037 #define Ki  0.001
00038 #define Kd  0.0
00039 #define RATE 0.01 // 制御周期(sec) 0.01=10ms
00040 
00041 /** wallbot ble control class
00042  *
00043  * Example:
00044  * @code
00045  * // Drive the wwallbot forward, turn left, back, turn right, at half speed for half a second
00046 
00047 #include "mbed.h"
00048 #include "wallbotble.h"
00049 
00050 wallbotble wb;
00051  
00052 int main() {
00053 
00054     wb.sensor_calibrate();
00055 
00056     while(!wb.GetSw())
00057     {
00058         wb.set_led(wb.GetLinePosition());
00059     }
00060     
00061     wb.forward(1.0);
00062     wait (1.0);
00063     wb.left(1.0);
00064     wait (1.0);
00065     wb.backward(1.0);
00066     wait (1.0);
00067     wb.right(1.0);
00068     wait (1.0);
00069     
00070     wb.stop();
00071 
00072     while(1);
00073 
00074  }
00075 
00076  * @endcode
00077  */
00078 class wallbotble  {
00079 
00080     // Public functions
00081 public:
00082 
00083     /** Create the wallbot object connected to the default pins
00084      */
00085     wallbotble();
00086     
00087     
00088     /** Sensor calibrate
00089      *
00090      */
00091     void f_sensor_calibrate (void);
00092 
00093     /** Directly control the speed and direction of the left motor
00094      *
00095      * @param speed A normalised number -1.0 - 1.0 represents the full range.
00096      */
00097     void left_motor (float duty);
00098 
00099     /** Directly control the speed and direction of the right motor
00100      *
00101      * @param speed A normalised number -1.0 - 1.0 represents the full range.
00102      */
00103     void right_motor (float duty);
00104 
00105     /** Drive both motors forward as the same speed
00106      *
00107      * @param speed A normalised number 0 - 1.0 represents the full range.
00108      */
00109     void forward (float duty);
00110 
00111     /** Drive both motors backward as the same speed
00112      *
00113      * @param speed A normalised number 0 - 1.0 represents the full range.
00114      */
00115     void backward (float duty);
00116 
00117     /** Drive left motor backwards and right motor forwards at the same speed to turn on the spot
00118      *
00119      * @param speed A normalised number 0 - 1.0 represents the full range.
00120      */
00121     void left_turn (float duty);
00122 
00123     /** Drive left motor forward and right motor backwards at the same speed to turn on the spot
00124      * @param speed A normalised number 0 - 1.0 represents the full range.
00125      */
00126     void right_turn (float duty);
00127 
00128     /** Stop both motors
00129      *
00130      */
00131     void stop (void);
00132     
00133     /** ラインの推定値を返す。 左端-1500～中央0～右端1500
00134      * 一定以上の検出がない場合は前回値から右端、左端と判定
00135      */ 
00136     short GetLinePosition(void);
00137     
00138     /** Get switch .(switch OFF:0 or ON:1 return.)
00139      *
00140      */ 
00141     int GetSw(void);
00142     
00143     /** Set status led .
00144      * @param led (bit0:LEFT bit2:RIGHT)
00145      */ 
00146     void set_led(char bit);
00147 
00148     /** Set led1 .
00149      * @param led (0:off,1:on)
00150      */ 
00151     void set_led1(char bit);
00152 
00153     /** Set led2 .
00154      * @param led (0:off,1:on)
00155      */ 
00156     void set_led2(char bit);
00157     
00158     // RPM指令で左右モータを制御する。
00159     void SetRPM(float leftRPM, float rightRPM);    
00160         
00161     // キャリブレーションのリセット
00162     void resetCalibration();
00163 
00164     // 自動キャリブレーション
00165     void auto_calibrate();
00166     
00167     //ラインセンサ値を補正し0から1000にマッピングしてsensor_valuesにロードする。
00168     void readCalibrated();
00169     
00170     unsigned short sensor_values[4];  
00171     
00172     
00173     void control_enable(bool enable);   
00174 
00175     unsigned short _calibratedMinimum[4];  
00176     unsigned short _calibratedMaximum[4];  
00177     
00178     // 左右のモーター回転数を返す
00179     float get_right_rpm();
00180     float get_left_rpm();
00181     
00182     unsigned short _right_pulses;
00183     unsigned short _left_pulses;
00184     
00185 private :
00186     //ff tearm
00187     float ff_r,ff_l;
00188     //後退用フラグ
00189     bool _right_back;
00190     bool _left_back;
00191     
00192     float _right_rpm;
00193     float _left_rpm;
00194     
00195 
00196     Timer _right_timer;
00197     Timer _left_timer;
00198     void right_count();
00199     void left_count();
00200     
00201     void tick_callback();
00202     
00203     Ticker _control_tic;
00204 
00205     TB6612 _right_motor;
00206     TB6612 _left_motor;
00207     
00208     BusIn _sw;
00209     DigitalOut _outlow;
00210     BusOut _statusled;
00211     AnalogIn _f_sensor1;
00212     AnalogIn _f_sensor2;
00213     AnalogIn _f_sensor3;
00214     AnalogIn _f_sensor4;
00215     int _sensor_gain;
00216     
00217     InterruptIn _right_enc;
00218     InterruptIn _left_enc;
00219     
00220     
00221     PID _ctrl_r;
00222     PID _ctrl_l;
00223 };
00224 
00225 #endif