Moto Sbk / wallbotble

wallbot control Library

Dependencies:   PID

wallbotble/wallbotble.h@0:7edbb986d2d7, 2018-06-04 (annotated)

Committer:
c201075
Date:
Mon Jun 04 08:32:43 2018 +0000
Revision:
0:7edbb986d2d7
library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
c201075 0:7edbb986d2d7 1 /* JKsoft Wallbot BLE Library
c201075 0:7edbb986d2d7 2 *
c201075 0:7edbb986d2d7 3 * wallbotble.h
c201075 0:7edbb986d2d7 4 *
c201075 0:7edbb986d2d7 5 * Copyright (c) 2010-2014 jksoft
c201075 0:7edbb986d2d7 6 *
c201075 0:7edbb986d2d7 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
c201075 0:7edbb986d2d7 8 * of this software and associated documentation files (the "Software"), to deal
c201075 0:7edbb986d2d7 9 * in the Software without restriction, including without limitation the rights
c201075 0:7edbb986d2d7 10 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
c201075 0:7edbb986d2d7 11 * copies of the Software, and to permit persons to whom the Software is
c201075 0:7edbb986d2d7 12 * furnished to do so, subject to the following conditions:
c201075 0:7edbb986d2d7 13 *
c201075 0:7edbb986d2d7 14 * The above copyright notice and this permission notice shall be included in
c201075 0:7edbb986d2d7 15 * all copies or substantial portions of the Software.
c201075 0:7edbb986d2d7 16 *
c201075 0:7edbb986d2d7 17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
c201075 0:7edbb986d2d7 18 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
c201075 0:7edbb986d2d7 19 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
c201075 0:7edbb986d2d7 20 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
c201075 0:7edbb986d2d7 21 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
c201075 0:7edbb986d2d7 22 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
c201075 0:7edbb986d2d7 23 * THE SOFTWARE.
c201075 0:7edbb986d2d7 24 */
c201075 0:7edbb986d2d7 25
c201075 0:7edbb986d2d7 26 #ifndef WALLBOT_BLE_H
c201075 0:7edbb986d2d7 27 #define WALLBOT_BLE_H
c201075 0:7edbb986d2d7 28
c201075 0:7edbb986d2d7 29 #include "mbed.h"
c201075 0:7edbb986d2d7 30 #include "TB6612.h"
c201075 0:7edbb986d2d7 31 //#include "QEI.h"
c201075 0:7edbb986d2d7 32 #include "PID.h"
c201075 0:7edbb986d2d7 33
c201075 0:7edbb986d2d7 34 #define PulsesPerRev 24
c201075 0:7edbb986d2d7 35
c201075 0:7edbb986d2d7 36 #define Kp 0.0001
c201075 0:7edbb986d2d7 37 #define Ki 0.001
c201075 0:7edbb986d2d7 38 #define Kd 0.0
c201075 0:7edbb986d2d7 39 #define RATE 0.01 // 制御周期(sec) 0.01=10ms
c201075 0:7edbb986d2d7 40
c201075 0:7edbb986d2d7 41 /** wallbot ble control class
c201075 0:7edbb986d2d7 42 *
c201075 0:7edbb986d2d7 43 * Example:
c201075 0:7edbb986d2d7 44 * @code
c201075 0:7edbb986d2d7 45 * // Drive the wwallbot forward, turn left, back, turn right, at half speed for half a second
c201075 0:7edbb986d2d7 46
c201075 0:7edbb986d2d7 47 #include "mbed.h"
c201075 0:7edbb986d2d7 48 #include "wallbotble.h"
c201075 0:7edbb986d2d7 49
c201075 0:7edbb986d2d7 50 wallbotble wb;
c201075 0:7edbb986d2d7 51
c201075 0:7edbb986d2d7 52 int main() {
c201075 0:7edbb986d2d7 53
c201075 0:7edbb986d2d7 54 wb.sensor_calibrate();
c201075 0:7edbb986d2d7 55
c201075 0:7edbb986d2d7 56 while(!wb.GetSw())
c201075 0:7edbb986d2d7 57 {
c201075 0:7edbb986d2d7 58 wb.set_led(wb.GetLinePosition());
c201075 0:7edbb986d2d7 59 }
c201075 0:7edbb986d2d7 60
c201075 0:7edbb986d2d7 61 wb.forward(1.0);
c201075 0:7edbb986d2d7 62 wait (1.0);
c201075 0:7edbb986d2d7 63 wb.left(1.0);
c201075 0:7edbb986d2d7 64 wait (1.0);
c201075 0:7edbb986d2d7 65 wb.backward(1.0);
c201075 0:7edbb986d2d7 66 wait (1.0);
c201075 0:7edbb986d2d7 67 wb.right(1.0);
c201075 0:7edbb986d2d7 68 wait (1.0);
c201075 0:7edbb986d2d7 69
c201075 0:7edbb986d2d7 70 wb.stop();
c201075 0:7edbb986d2d7 71
c201075 0:7edbb986d2d7 72 while(1);
c201075 0:7edbb986d2d7 73
c201075 0:7edbb986d2d7 74 }
c201075 0:7edbb986d2d7 75
c201075 0:7edbb986d2d7 76 * @endcode
c201075 0:7edbb986d2d7 77 */
c201075 0:7edbb986d2d7 78 class wallbotble {
c201075 0:7edbb986d2d7 79
c201075 0:7edbb986d2d7 80 // Public functions
c201075 0:7edbb986d2d7 81 public:
c201075 0:7edbb986d2d7 82
c201075 0:7edbb986d2d7 83 /** Create the wallbot object connected to the default pins
c201075 0:7edbb986d2d7 84 */
c201075 0:7edbb986d2d7 85 wallbotble();
c201075 0:7edbb986d2d7 86
c201075 0:7edbb986d2d7 87
c201075 0:7edbb986d2d7 88 /** Sensor calibrate
c201075 0:7edbb986d2d7 89 *
c201075 0:7edbb986d2d7 90 */
c201075 0:7edbb986d2d7 91 void f_sensor_calibrate (void);
c201075 0:7edbb986d2d7 92
c201075 0:7edbb986d2d7 93 /** Directly control the speed and direction of the left motor
c201075 0:7edbb986d2d7 94 *
c201075 0:7edbb986d2d7 95 * @param speed A normalised number -1.0 - 1.0 represents the full range.
c201075 0:7edbb986d2d7 96 */
c201075 0:7edbb986d2d7 97 void left_motor (float duty);
c201075 0:7edbb986d2d7 98
c201075 0:7edbb986d2d7 99 /** Directly control the speed and direction of the right motor
c201075 0:7edbb986d2d7 100 *
c201075 0:7edbb986d2d7 101 * @param speed A normalised number -1.0 - 1.0 represents the full range.
c201075 0:7edbb986d2d7 102 */
c201075 0:7edbb986d2d7 103 void right_motor (float duty);
c201075 0:7edbb986d2d7 104
c201075 0:7edbb986d2d7 105 /** Drive both motors forward as the same speed
c201075 0:7edbb986d2d7 106 *
c201075 0:7edbb986d2d7 107 * @param speed A normalised number 0 - 1.0 represents the full range.
c201075 0:7edbb986d2d7 108 */
c201075 0:7edbb986d2d7 109 void forward (float duty);
c201075 0:7edbb986d2d7 110
c201075 0:7edbb986d2d7 111 /** Drive both motors backward as the same speed
c201075 0:7edbb986d2d7 112 *
c201075 0:7edbb986d2d7 113 * @param speed A normalised number 0 - 1.0 represents the full range.
c201075 0:7edbb986d2d7 114 */
c201075 0:7edbb986d2d7 115 void backward (float duty);
c201075 0:7edbb986d2d7 116
c201075 0:7edbb986d2d7 117 /** Drive left motor backwards and right motor forwards at the same speed to turn on the spot
c201075 0:7edbb986d2d7 118 *
c201075 0:7edbb986d2d7 119 * @param speed A normalised number 0 - 1.0 represents the full range.
c201075 0:7edbb986d2d7 120 */
c201075 0:7edbb986d2d7 121 void left_turn (float duty);
c201075 0:7edbb986d2d7 122
c201075 0:7edbb986d2d7 123 /** Drive left motor forward and right motor backwards at the same speed to turn on the spot
c201075 0:7edbb986d2d7 124 * @param speed A normalised number 0 - 1.0 represents the full range.
c201075 0:7edbb986d2d7 125 */
c201075 0:7edbb986d2d7 126 void right_turn (float duty);
c201075 0:7edbb986d2d7 127
c201075 0:7edbb986d2d7 128 /** Stop both motors
c201075 0:7edbb986d2d7 129 *
c201075 0:7edbb986d2d7 130 */
c201075 0:7edbb986d2d7 131 void stop (void);
c201075 0:7edbb986d2d7 132
c201075 0:7edbb986d2d7 133 /** ラインの推定値を返す。 左端-1500～中央0～右端1500
c201075 0:7edbb986d2d7 134 * 一定以上の検出がない場合は前回値から右端、左端と判定
c201075 0:7edbb986d2d7 135 */
c201075 0:7edbb986d2d7 136 short GetLinePosition(void);
c201075 0:7edbb986d2d7 137
c201075 0:7edbb986d2d7 138 /** Get switch .(switch OFF:0 or ON:1 return.)
c201075 0:7edbb986d2d7 139 *
c201075 0:7edbb986d2d7 140 */
c201075 0:7edbb986d2d7 141 int GetSw(void);
c201075 0:7edbb986d2d7 142
c201075 0:7edbb986d2d7 143 /** Set status led .
c201075 0:7edbb986d2d7 144 * @param led (bit0:LEFT bit2:RIGHT)
c201075 0:7edbb986d2d7 145 */
c201075 0:7edbb986d2d7 146 void set_led(char bit);
c201075 0:7edbb986d2d7 147
c201075 0:7edbb986d2d7 148 /** Set led1 .
c201075 0:7edbb986d2d7 149 * @param led (0:off,1:on)
c201075 0:7edbb986d2d7 150 */
c201075 0:7edbb986d2d7 151 void set_led1(char bit);
c201075 0:7edbb986d2d7 152
c201075 0:7edbb986d2d7 153 /** Set led2 .
c201075 0:7edbb986d2d7 154 * @param led (0:off,1:on)
c201075 0:7edbb986d2d7 155 */
c201075 0:7edbb986d2d7 156 void set_led2(char bit);
c201075 0:7edbb986d2d7 157
c201075 0:7edbb986d2d7 158 // RPM指令で左右モータを制御する。
c201075 0:7edbb986d2d7 159 void SetRPM(float leftRPM, float rightRPM);
c201075 0:7edbb986d2d7 160
c201075 0:7edbb986d2d7 161 // キャリブレーションのリセット
c201075 0:7edbb986d2d7 162 void resetCalibration();
c201075 0:7edbb986d2d7 163
c201075 0:7edbb986d2d7 164 // 自動キャリブレーション
c201075 0:7edbb986d2d7 165 void auto_calibrate();
c201075 0:7edbb986d2d7 166
c201075 0:7edbb986d2d7 167 //ラインセンサ値を補正し0から1000にマッピングしてsensor_valuesにロードする。
c201075 0:7edbb986d2d7 168 void readCalibrated();
c201075 0:7edbb986d2d7 169
c201075 0:7edbb986d2d7 170 unsigned short sensor_values[4];
c201075 0:7edbb986d2d7 171
c201075 0:7edbb986d2d7 172
c201075 0:7edbb986d2d7 173 void control_enable(bool enable);
c201075 0:7edbb986d2d7 174
c201075 0:7edbb986d2d7 175 unsigned short _calibratedMinimum[4];
c201075 0:7edbb986d2d7 176 unsigned short _calibratedMaximum[4];
c201075 0:7edbb986d2d7 177
c201075 0:7edbb986d2d7 178 // 左右のモーター回転数を返す
c201075 0:7edbb986d2d7 179 float get_right_rpm();
c201075 0:7edbb986d2d7 180 float get_left_rpm();
c201075 0:7edbb986d2d7 181
c201075 0:7edbb986d2d7 182 unsigned short _right_pulses;
c201075 0:7edbb986d2d7 183 unsigned short _left_pulses;
c201075 0:7edbb986d2d7 184
c201075 0:7edbb986d2d7 185 private :
c201075 0:7edbb986d2d7 186 //ff tearm
c201075 0:7edbb986d2d7 187 float ff_r,ff_l;
c201075 0:7edbb986d2d7 188 //後退用フラグ
c201075 0:7edbb986d2d7 189 bool _right_back;
c201075 0:7edbb986d2d7 190 bool _left_back;
c201075 0:7edbb986d2d7 191
c201075 0:7edbb986d2d7 192 float _right_rpm;
c201075 0:7edbb986d2d7 193 float _left_rpm;
c201075 0:7edbb986d2d7 194
c201075 0:7edbb986d2d7 195
c201075 0:7edbb986d2d7 196 Timer _right_timer;
c201075 0:7edbb986d2d7 197 Timer _left_timer;
c201075 0:7edbb986d2d7 198 void right_count();
c201075 0:7edbb986d2d7 199 void left_count();
c201075 0:7edbb986d2d7 200
c201075 0:7edbb986d2d7 201 void tick_callback();
c201075 0:7edbb986d2d7 202
c201075 0:7edbb986d2d7 203 Ticker _control_tic;
c201075 0:7edbb986d2d7 204
c201075 0:7edbb986d2d7 205 TB6612 _right_motor;
c201075 0:7edbb986d2d7 206 TB6612 _left_motor;
c201075 0:7edbb986d2d7 207
c201075 0:7edbb986d2d7 208 BusIn _sw;
c201075 0:7edbb986d2d7 209 DigitalOut _outlow;
c201075 0:7edbb986d2d7 210 BusOut _statusled;
c201075 0:7edbb986d2d7 211 AnalogIn _f_sensor1;
c201075 0:7edbb986d2d7 212 AnalogIn _f_sensor2;
c201075 0:7edbb986d2d7 213 AnalogIn _f_sensor3;
c201075 0:7edbb986d2d7 214 AnalogIn _f_sensor4;
c201075 0:7edbb986d2d7 215 int _sensor_gain;
c201075 0:7edbb986d2d7 216
c201075 0:7edbb986d2d7 217 InterruptIn _right_enc;
c201075 0:7edbb986d2d7 218 InterruptIn _left_enc;
c201075 0:7edbb986d2d7 219
c201075 0:7edbb986d2d7 220
c201075 0:7edbb986d2d7 221 PID _ctrl_r;
c201075 0:7edbb986d2d7 222 PID _ctrl_l;
c201075 0:7edbb986d2d7 223 };
c201075 0:7edbb986d2d7 224
c201075 0:7edbb986d2d7 225 #endif