wallbot control Library
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Mon Aug 22 2022 09:01:58 by 1.7.2