Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

autocar/autocar.cpp@21:093c8525349a, 2018-07-19 (annotated)

Committer:
cudaChen
Date:
Thu Jul 19 07:41:10 2018 +0000
Revision:
21:093c8525349a
Parent:
19:d06f5a3ed0bc 
[change] merge "PID control" and "obstacle avoidance" feature

Who changed what in which revision?

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