Lu-Hsuan Chen
/
autocar_obstacle_mbed
autocar with cliff and obstacle avoidance feature
autocar/autocar.cpp@0:b9d5ff825f0f, 2018-07-19 (annotated)
- Committer:
- cudaChen
- Date:
- Thu Jul 19 05:17:01 2018 +0000
- Revision:
- 0:b9d5ff825f0f
- Child:
- 1:c7d5b9662b4f
[initial] initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cudaChen | 0:b9d5ff825f0f | 1 | #include "mbed.h" |
cudaChen | 0:b9d5ff825f0f | 2 | |
cudaChen | 0:b9d5ff825f0f | 3 | #include "Ping.h" |
cudaChen | 0:b9d5ff825f0f | 4 | #include "autocar.h" |
cudaChen | 0:b9d5ff825f0f | 5 | |
cudaChen | 0:b9d5ff825f0f | 6 | const int MOTOR_M1 = 0; |
cudaChen | 0:b9d5ff825f0f | 7 | const int MOTOR_M2 = 1; |
cudaChen | 0:b9d5ff825f0f | 8 | const int DIR_FORWARD = 0; |
cudaChen | 0:b9d5ff825f0f | 9 | const int DIR_BACKWARD = 1; |
cudaChen | 0:b9d5ff825f0f | 10 | const int PWR_STOP = 0; |
cudaChen | 0:b9d5ff825f0f | 11 | |
cudaChen | 0:b9d5ff825f0f | 12 | // used for IR sensors |
cudaChen | 0:b9d5ff825f0f | 13 | int left, middle, right; |
cudaChen | 0:b9d5ff825f0f | 14 | |
cudaChen | 0:b9d5ff825f0f | 15 | // PID factors |
cudaChen | 0:b9d5ff825f0f | 16 | int interror = 0; |
cudaChen | 0:b9d5ff825f0f | 17 | int olderror = 0; |
cudaChen | 0:b9d5ff825f0f | 18 | int limit = 4095; // 12-bit ADC in STM32 |
cudaChen | 0:b9d5ff825f0f | 19 | |
cudaChen | 0:b9d5ff825f0f | 20 | // used for output message via serial |
cudaChen | 0:b9d5ff825f0f | 21 | Serial pc(SERIAL_TX, SERIAL_RX); |
cudaChen | 0:b9d5ff825f0f | 22 | |
cudaChen | 0:b9d5ff825f0f | 23 | long map(long x, long in_min, long in_max, long out_min, long out_max) |
cudaChen | 0:b9d5ff825f0f | 24 | { |
cudaChen | 0:b9d5ff825f0f | 25 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cudaChen | 0:b9d5ff825f0f | 26 | } |
cudaChen | 0:b9d5ff825f0f | 27 | |
cudaChen | 0:b9d5ff825f0f | 28 | void readIR(bool* left, bool* middle, bool* right, int threshold) |
cudaChen | 0:b9d5ff825f0f | 29 | { |
cudaChen | 0:b9d5ff825f0f | 30 | // not on track: > 500 |
cudaChen | 0:b9d5ff825f0f | 31 | // on track (black): < 500 |
cudaChen | 0:b9d5ff825f0f | 32 | *left = leftIR.read_u16() < threshold ? true : false; |
cudaChen | 0:b9d5ff825f0f | 33 | *middle = middleIR.read_u16() < threshold ? true : false; |
cudaChen | 0:b9d5ff825f0f | 34 | *right = rightIR.read_u16() < threshold ? true : false; |
cudaChen | 0:b9d5ff825f0f | 35 | } |
cudaChen | 0:b9d5ff825f0f | 36 | |
cudaChen | 0:b9d5ff825f0f | 37 | int readIRValues() |
cudaChen | 0:b9d5ff825f0f | 38 | { |
cudaChen | 0:b9d5ff825f0f | 39 | left = leftIR.read_u16(); |
cudaChen | 0:b9d5ff825f0f | 40 | middle = middleIR.read_u16(); |
cudaChen | 0:b9d5ff825f0f | 41 | right = rightIR.read_u16(); |
cudaChen | 0:b9d5ff825f0f | 42 | |
cudaChen | 0:b9d5ff825f0f | 43 | int ret = left * (-1) + middle * 0 + right * 1; |
cudaChen | 0:b9d5ff825f0f | 44 | |
cudaChen | 0:b9d5ff825f0f | 45 | //pc.printf("left middle right: %d %d %d\r\n", left, middle, right); |
cudaChen | 0:b9d5ff825f0f | 46 | pc.printf("IR values: %d\r\n", ret); |
cudaChen | 0:b9d5ff825f0f | 47 | |
cudaChen | 0:b9d5ff825f0f | 48 | return ret; |
cudaChen | 0:b9d5ff825f0f | 49 | } |
cudaChen | 0:b9d5ff825f0f | 50 | |
cudaChen | 0:b9d5ff825f0f | 51 | void readSensor(bool* left, bool* middle, bool* right, bool* hasObstacle, int threshold, int range) |
cudaChen | 0:b9d5ff825f0f | 52 | { |
cudaChen | 0:b9d5ff825f0f | 53 | int distance; |
cudaChen | 0:b9d5ff825f0f | 54 | ultrasonic.Send(); |
cudaChen | 0:b9d5ff825f0f | 55 | wait_ms(30); |
cudaChen | 0:b9d5ff825f0f | 56 | distance = ultrasonic.Read_cm(); |
cudaChen | 0:b9d5ff825f0f | 57 | *hasObstacle = distance < range ? true : false; |
cudaChen | 0:b9d5ff825f0f | 58 | |
cudaChen | 0:b9d5ff825f0f | 59 | // not on track: > 500 |
cudaChen | 0:b9d5ff825f0f | 60 | // on track (black): < 500 |
cudaChen | 0:b9d5ff825f0f | 61 | *left = leftIR.read_u16() < threshold ? true : false; |
cudaChen | 0:b9d5ff825f0f | 62 | *middle = middleIR.read_u16() < threshold ? true : false; |
cudaChen | 0:b9d5ff825f0f | 63 | *right = rightIR.read_u16() < threshold ? true : false; |
cudaChen | 0:b9d5ff825f0f | 64 | } |
cudaChen | 0:b9d5ff825f0f | 65 | |
cudaChen | 0:b9d5ff825f0f | 66 | // m: 0 => M1 1 => M2 |
cudaChen | 0:b9d5ff825f0f | 67 | // power 0~10 dir: 0=>正向 1=>反向 |
cudaChen | 0:b9d5ff825f0f | 68 | void DriveSingleMotor(int m, int speed, int dir) |
cudaChen | 0:b9d5ff825f0f | 69 | { |
cudaChen | 0:b9d5ff825f0f | 70 | /*int _speed = 0; |
cudaChen | 0:b9d5ff825f0f | 71 | |
cudaChen | 0:b9d5ff825f0f | 72 | // 設定速度 |
cudaChen | 0:b9d5ff825f0f | 73 | if (speed>10) speed=10; |
cudaChen | 0:b9d5ff825f0f | 74 | _speed = map(speed, 1, 10, 100, 255); |
cudaChen | 0:b9d5ff825f0f | 75 | |
cudaChen | 0:b9d5ff825f0f | 76 | if (speed<=0) { |
cudaChen | 0:b9d5ff825f0f | 77 | speed=0; |
cudaChen | 0:b9d5ff825f0f | 78 | _speed=0; |
cudaChen | 0:b9d5ff825f0f | 79 | } else |
cudaChen | 0:b9d5ff825f0f | 80 | _speed = map(speed, 1, 10, 100, 255);*/ |
cudaChen | 0:b9d5ff825f0f | 81 | |
cudaChen | 0:b9d5ff825f0f | 82 | if (m == MOTOR_M1) { |
cudaChen | 0:b9d5ff825f0f | 83 | // 設定方向 |
cudaChen | 0:b9d5ff825f0f | 84 | if (speed == PWR_STOP) { |
cudaChen | 0:b9d5ff825f0f | 85 | M1_in1 = 0; |
cudaChen | 0:b9d5ff825f0f | 86 | M1_in2 = 0; |
cudaChen | 0:b9d5ff825f0f | 87 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:b9d5ff825f0f | 88 | // right wheel go forward |
cudaChen | 0:b9d5ff825f0f | 89 | M1_in1 = 1; |
cudaChen | 0:b9d5ff825f0f | 90 | M1_in2 = 0; |
cudaChen | 0:b9d5ff825f0f | 91 | } else { |
cudaChen | 0:b9d5ff825f0f | 92 | // right wheel go backward |
cudaChen | 0:b9d5ff825f0f | 93 | M1_in1 = 0; |
cudaChen | 0:b9d5ff825f0f | 94 | M1_in2 = 1; |
cudaChen | 0:b9d5ff825f0f | 95 | } |
cudaChen | 0:b9d5ff825f0f | 96 | //M1_enable.write_u16(_speed); // 驅動馬達 右輪 |
cudaChen | 0:b9d5ff825f0f | 97 | M1_enable.write(1); |
cudaChen | 0:b9d5ff825f0f | 98 | |
cudaChen | 0:b9d5ff825f0f | 99 | } else if (m == MOTOR_M2) { |
cudaChen | 0:b9d5ff825f0f | 100 | // 設定方向 |
cudaChen | 0:b9d5ff825f0f | 101 | if (speed == PWR_STOP) { |
cudaChen | 0:b9d5ff825f0f | 102 | M2_in3 = 0; |
cudaChen | 0:b9d5ff825f0f | 103 | M2_in4 = 0; |
cudaChen | 0:b9d5ff825f0f | 104 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:b9d5ff825f0f | 105 | //左輪前進 |
cudaChen | 0:b9d5ff825f0f | 106 | M2_in3 = 1; |
cudaChen | 0:b9d5ff825f0f | 107 | M2_in4 = 0; |
cudaChen | 0:b9d5ff825f0f | 108 | } else { |
cudaChen | 0:b9d5ff825f0f | 109 | //左輪倒轉 |
cudaChen | 0:b9d5ff825f0f | 110 | M2_in3 = 0; |
cudaChen | 0:b9d5ff825f0f | 111 | M2_in4 = 1; |
cudaChen | 0:b9d5ff825f0f | 112 | } |
cudaChen | 0:b9d5ff825f0f | 113 | //M2_enable.write_u16(_speed); // 驅動馬達 左輪 |
cudaChen | 0:b9d5ff825f0f | 114 | M2_enable.write(1); |
cudaChen | 0:b9d5ff825f0f | 115 | } else { |
cudaChen | 0:b9d5ff825f0f | 116 | //M1_enable.write_u16(PWR_STOP); // right wheel |
cudaChen | 0:b9d5ff825f0f | 117 | //M2_enable.write_u16(PWR_STOP); // left wheel |
cudaChen | 0:b9d5ff825f0f | 118 | M1_enable.write(0); |
cudaChen | 0:b9d5ff825f0f | 119 | M2_enable.write(0); |
cudaChen | 0:b9d5ff825f0f | 120 | } |
cudaChen | 0:b9d5ff825f0f | 121 | } |
cudaChen | 0:b9d5ff825f0f | 122 | |
cudaChen | 0:b9d5ff825f0f | 123 | void driveMotor(bool left, bool middle, bool right) |
cudaChen | 0:b9d5ff825f0f | 124 | { |
cudaChen | 0:b9d5ff825f0f | 125 | int status = left * 4 + middle * 2 + right; |
cudaChen | 0:b9d5ff825f0f | 126 | switch(status) { |
cudaChen | 0:b9d5ff825f0f | 127 | case 7: // go straight |
cudaChen | 0:b9d5ff825f0f | 128 | forward(); |
cudaChen | 0:b9d5ff825f0f | 129 | break; |
cudaChen | 0:b9d5ff825f0f | 130 | case 6: // turn left |
cudaChen | 0:b9d5ff825f0f | 131 | turnLeft(); |
cudaChen | 0:b9d5ff825f0f | 132 | break; |
cudaChen | 0:b9d5ff825f0f | 133 | case 5: // go straight |
cudaChen | 0:b9d5ff825f0f | 134 | forward(); |
cudaChen | 0:b9d5ff825f0f | 135 | break; |
cudaChen | 0:b9d5ff825f0f | 136 | case 4: // turn left |
cudaChen | 0:b9d5ff825f0f | 137 | turnLeft(); |
cudaChen | 0:b9d5ff825f0f | 138 | break; |
cudaChen | 0:b9d5ff825f0f | 139 | case 3: // turn right |
cudaChen | 0:b9d5ff825f0f | 140 | turnRight(); |
cudaChen | 0:b9d5ff825f0f | 141 | break; |
cudaChen | 0:b9d5ff825f0f | 142 | case 2: // go straight |
cudaChen | 0:b9d5ff825f0f | 143 | forward(); |
cudaChen | 0:b9d5ff825f0f | 144 | break; |
cudaChen | 0:b9d5ff825f0f | 145 | case 1: // turn right |
cudaChen | 0:b9d5ff825f0f | 146 | turnRight(); |
cudaChen | 0:b9d5ff825f0f | 147 | break; |
cudaChen | 0:b9d5ff825f0f | 148 | case 0: // go straight |
cudaChen | 0:b9d5ff825f0f | 149 | forward(); |
cudaChen | 0:b9d5ff825f0f | 150 | break; |
cudaChen | 0:b9d5ff825f0f | 151 | default: |
cudaChen | 0:b9d5ff825f0f | 152 | pc.printf("invalid\r\n"); |
cudaChen | 0:b9d5ff825f0f | 153 | break; |
cudaChen | 0:b9d5ff825f0f | 154 | } |
cudaChen | 0:b9d5ff825f0f | 155 | } |
cudaChen | 0:b9d5ff825f0f | 156 | |
cudaChen | 0:b9d5ff825f0f | 157 | void driveMotor(bool left, bool middle, bool right, bool hasObstacle) |
cudaChen | 0:b9d5ff825f0f | 158 | { |
cudaChen | 0:b9d5ff825f0f | 159 | if(hasObstacle) |
cudaChen | 0:b9d5ff825f0f | 160 | { |
cudaChen | 0:b9d5ff825f0f | 161 | turnRight(); |
cudaChen | 0:b9d5ff825f0f | 162 | wait(0.3); |
cudaChen | 0:b9d5ff825f0f | 163 | forward(); |
cudaChen | 0:b9d5ff825f0f | 164 | wait(0.5); |
cudaChen | 0:b9d5ff825f0f | 165 | turnLeft(); |
cudaChen | 0:b9d5ff825f0f | 166 | wait(0.3); |
cudaChen | 0:b9d5ff825f0f | 167 | forward(); |
cudaChen | 0:b9d5ff825f0f | 168 | wait(0.5); |
cudaChen | 0:b9d5ff825f0f | 169 | turnLeft(); |
cudaChen | 0:b9d5ff825f0f | 170 | wait(0.3); |
cudaChen | 0:b9d5ff825f0f | 171 | forward(); |
cudaChen | 0:b9d5ff825f0f | 172 | wait(0.5); |
cudaChen | 0:b9d5ff825f0f | 173 | turnRight(); |
cudaChen | 0:b9d5ff825f0f | 174 | wait(0.3); |
cudaChen | 0:b9d5ff825f0f | 175 | |
cudaChen | 0:b9d5ff825f0f | 176 | return; |
cudaChen | 0:b9d5ff825f0f | 177 | } |
cudaChen | 0:b9d5ff825f0f | 178 | |
cudaChen | 0:b9d5ff825f0f | 179 | int status = left * 4 + middle * 2 + right; |
cudaChen | 0:b9d5ff825f0f | 180 | switch(status) { |
cudaChen | 0:b9d5ff825f0f | 181 | case 7: // go straight |
cudaChen | 0:b9d5ff825f0f | 182 | forward(); |
cudaChen | 0:b9d5ff825f0f | 183 | break; |
cudaChen | 0:b9d5ff825f0f | 184 | case 6: // turn left |
cudaChen | 0:b9d5ff825f0f | 185 | turnLeft(); |
cudaChen | 0:b9d5ff825f0f | 186 | break; |
cudaChen | 0:b9d5ff825f0f | 187 | case 5: // go straight |
cudaChen | 0:b9d5ff825f0f | 188 | forward(); |
cudaChen | 0:b9d5ff825f0f | 189 | break; |
cudaChen | 0:b9d5ff825f0f | 190 | case 4: // turn left |
cudaChen | 0:b9d5ff825f0f | 191 | turnLeft(); |
cudaChen | 0:b9d5ff825f0f | 192 | break; |
cudaChen | 0:b9d5ff825f0f | 193 | case 3: // turn right |
cudaChen | 0:b9d5ff825f0f | 194 | turnRight(); |
cudaChen | 0:b9d5ff825f0f | 195 | break; |
cudaChen | 0:b9d5ff825f0f | 196 | case 2: // go straight |
cudaChen | 0:b9d5ff825f0f | 197 | forward(); |
cudaChen | 0:b9d5ff825f0f | 198 | break; |
cudaChen | 0:b9d5ff825f0f | 199 | case 1: // turn right |
cudaChen | 0:b9d5ff825f0f | 200 | turnRight(); |
cudaChen | 0:b9d5ff825f0f | 201 | break; |
cudaChen | 0:b9d5ff825f0f | 202 | case 0: // go straight |
cudaChen | 0:b9d5ff825f0f | 203 | forward(); |
cudaChen | 0:b9d5ff825f0f | 204 | break; |
cudaChen | 0:b9d5ff825f0f | 205 | default: |
cudaChen | 0:b9d5ff825f0f | 206 | pc.printf("invalid\r\n"); |
cudaChen | 0:b9d5ff825f0f | 207 | break; |
cudaChen | 0:b9d5ff825f0f | 208 | } |
cudaChen | 0:b9d5ff825f0f | 209 | } |
cudaChen | 0:b9d5ff825f0f | 210 | |
cudaChen | 0:b9d5ff825f0f | 211 | void driveMotorPID(int values, float Kp, float Ki, float Kd) |
cudaChen | 0:b9d5ff825f0f | 212 | { |
cudaChen | 0:b9d5ff825f0f | 213 | int error = values; // 'P' |
cudaChen | 0:b9d5ff825f0f | 214 | interror += error; |
cudaChen | 0:b9d5ff825f0f | 215 | int lasterror = error - olderror; |
cudaChen | 0:b9d5ff825f0f | 216 | olderror = error; // 注意olderror的順序 |
cudaChen | 0:b9d5ff825f0f | 217 | int power = error * Kp + interror * Ki + lasterror * Kd; |
cudaChen | 0:b9d5ff825f0f | 218 | /* error(P值)、interror(I值)、lasterror(D值)、olderror用來取D值的 */ |
cudaChen | 0:b9d5ff825f0f | 219 | |
cudaChen | 0:b9d5ff825f0f | 220 | // limit PID output value (for 12-bit ADC in STM32) |
cudaChen | 0:b9d5ff825f0f | 221 | if(power > limit) { |
cudaChen | 0:b9d5ff825f0f | 222 | power = limit; |
cudaChen | 0:b9d5ff825f0f | 223 | } else if(power < -limit) { |
cudaChen | 0:b9d5ff825f0f | 224 | power = -limit; |
cudaChen | 0:b9d5ff825f0f | 225 | } |
cudaChen | 0:b9d5ff825f0f | 226 | |
cudaChen | 0:b9d5ff825f0f | 227 | //pc.printf("%d \r\n", power); |
cudaChen | 0:b9d5ff825f0f | 228 | |
cudaChen | 0:b9d5ff825f0f | 229 | // control the direction of auto car |
cudaChen | 0:b9d5ff825f0f | 230 | if(power < -800) { |
cudaChen | 0:b9d5ff825f0f | 231 | turnLeft(); |
cudaChen | 0:b9d5ff825f0f | 232 | } else if (power > 800) { |
cudaChen | 0:b9d5ff825f0f | 233 | turnRight(); |
cudaChen | 0:b9d5ff825f0f | 234 | } else { |
cudaChen | 0:b9d5ff825f0f | 235 | forward(); |
cudaChen | 0:b9d5ff825f0f | 236 | } |
cudaChen | 0:b9d5ff825f0f | 237 | } |
cudaChen | 0:b9d5ff825f0f | 238 | |
cudaChen | 0:b9d5ff825f0f | 239 | void init() |
cudaChen | 0:b9d5ff825f0f | 240 | { |
cudaChen | 0:b9d5ff825f0f | 241 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 242 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 243 | } |
cudaChen | 0:b9d5ff825f0f | 244 | |
cudaChen | 0:b9d5ff825f0f | 245 | void stop() |
cudaChen | 0:b9d5ff825f0f | 246 | { |
cudaChen | 0:b9d5ff825f0f | 247 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 248 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 249 | } |
cudaChen | 0:b9d5ff825f0f | 250 | |
cudaChen | 0:b9d5ff825f0f | 251 | void forward() |
cudaChen | 0:b9d5ff825f0f | 252 | { |
cudaChen | 0:b9d5ff825f0f | 253 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 254 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 255 | } |
cudaChen | 0:b9d5ff825f0f | 256 | |
cudaChen | 0:b9d5ff825f0f | 257 | void backward() |
cudaChen | 0:b9d5ff825f0f | 258 | { |
cudaChen | 0:b9d5ff825f0f | 259 | DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD); |
cudaChen | 0:b9d5ff825f0f | 260 | DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD); |
cudaChen | 0:b9d5ff825f0f | 261 | } |
cudaChen | 0:b9d5ff825f0f | 262 | |
cudaChen | 0:b9d5ff825f0f | 263 | void turnLeft() |
cudaChen | 0:b9d5ff825f0f | 264 | { |
cudaChen | 0:b9d5ff825f0f | 265 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 266 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 267 | } |
cudaChen | 0:b9d5ff825f0f | 268 | |
cudaChen | 0:b9d5ff825f0f | 269 | void turnRight() |
cudaChen | 0:b9d5ff825f0f | 270 | { |
cudaChen | 0:b9d5ff825f0f | 271 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 272 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:b9d5ff825f0f | 273 | } |