Lu-Hsuan Chen / Mbed OS autocar_obstacle_mbed

autocar with cliff and obstacle avoidance feature

Dependencies:   Ping

autocar/autocar.cpp@1:c7d5b9662b4f, 2018-07-19 (annotated)

Committer:
cudaChen
Date:
Thu Jul 19 05:41:17 2018 +0000
Revision:
1:c7d5b9662b4f
Parent:
0:b9d5ff825f0f 
[milestone] first commit

Who changed what in which revision?

UserRevisionLine numberNew 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 1:c7d5b9662b4f 239 void obstacle(bool hasCliff, bool hasObstacle)
cudaChen 1:c7d5b9662b4f 240 {
cudaChen 1:c7d5b9662b4f 241 if(hasCliff || hasObstacle)
cudaChen 1:c7d5b9662b4f 242 {
cudaChen 1:c7d5b9662b4f 243 backward();
cudaChen 1:c7d5b9662b4f 244 wait_ms(300);
cudaChen 1:c7d5b9662b4f 245 stop();
cudaChen 1:c7d5b9662b4f 246 wait_ms(200);
cudaChen 1:c7d5b9662b4f 247 turnRight();
cudaChen 1:c7d5b9662b4f 248 wait_ms(200);
cudaChen 1:c7d5b9662b4f 249 }
cudaChen 1:c7d5b9662b4f 250
cudaChen 1:c7d5b9662b4f 251 forward();
cudaChen 1:c7d5b9662b4f 252 wait_ms(200);
cudaChen 1:c7d5b9662b4f 253 }
cudaChen 1:c7d5b9662b4f 254
cudaChen 0:b9d5ff825f0f 255 void init()
cudaChen 0:b9d5ff825f0f 256 {
cudaChen 0:b9d5ff825f0f 257 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 258 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 259 }
cudaChen 0:b9d5ff825f0f 260
cudaChen 0:b9d5ff825f0f 261 void stop()
cudaChen 0:b9d5ff825f0f 262 {
cudaChen 0:b9d5ff825f0f 263 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 264 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 265 }
cudaChen 0:b9d5ff825f0f 266
cudaChen 0:b9d5ff825f0f 267 void forward()
cudaChen 0:b9d5ff825f0f 268 {
cudaChen 0:b9d5ff825f0f 269 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 270 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 271 }
cudaChen 0:b9d5ff825f0f 272
cudaChen 0:b9d5ff825f0f 273 void backward()
cudaChen 0:b9d5ff825f0f 274 {
cudaChen 0:b9d5ff825f0f 275 DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD);
cudaChen 0:b9d5ff825f0f 276 DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD);
cudaChen 0:b9d5ff825f0f 277 }
cudaChen 0:b9d5ff825f0f 278
cudaChen 0:b9d5ff825f0f 279 void turnLeft()
cudaChen 0:b9d5ff825f0f 280 {
cudaChen 0:b9d5ff825f0f 281 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 282 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 283 }
cudaChen 0:b9d5ff825f0f 284
cudaChen 0:b9d5ff825f0f 285 void turnRight()
cudaChen 0:b9d5ff825f0f 286 {
cudaChen 0:b9d5ff825f0f 287 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 288 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 0:b9d5ff825f0f 289 }