Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

autocar/autocar.cpp@14:28cae134a304, 2018-07-01 (annotated)

Committer:
cudaChen
Date:
Sun Jul 01 13:12:37 2018 +0000
Revision:
14:28cae134a304
Parent:
13:87cd0ae37e06
Child:
15:1d440beb24d3
[add] add debug message in function 'readIRValues'

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cudaChen 12:e95ed962be7a 1 #include "mbed.h"
cudaChen 12:e95ed962be7a 2
cudaChen 12:e95ed962be7a 3 #include "autocar.h"
cudaChen 12:e95ed962be7a 4
cudaChen 12:e95ed962be7a 5 DigitalOut M1_enable(D6);
cudaChen 12:e95ed962be7a 6 DigitalOut M2_enable(D5);
cudaChen 12:e95ed962be7a 7 DigitalOut M1_in1(D3);
cudaChen 12:e95ed962be7a 8 DigitalOut M1_in2(D2);
cudaChen 12:e95ed962be7a 9 DigitalOut M2_in3(D1);
cudaChen 12:e95ed962be7a 10 DigitalOut M2_in4(D0);
cudaChen 12:e95ed962be7a 11 const int MOTOR_M1 = 0;
cudaChen 12:e95ed962be7a 12 const int MOTOR_M2 = 1;
cudaChen 12:e95ed962be7a 13 const int DIR_FORWARD = 0;
cudaChen 12:e95ed962be7a 14 const int DIR_BACKWARD = 1;
cudaChen 12:e95ed962be7a 15 const int PWR_STOP = 0;
cudaChen 12:e95ed962be7a 16
cudaChen 12:e95ed962be7a 17 // used for IR sensors
cudaChen 12:e95ed962be7a 18 AnalogIn leftIR(A1);
cudaChen 12:e95ed962be7a 19 AnalogIn middleIR(A3);
cudaChen 12:e95ed962be7a 20 AnalogIn rightIR(A5);
cudaChen 13:87cd0ae37e06 21 int left, middle, right;
cudaChen 13:87cd0ae37e06 22
cudaChen 13:87cd0ae37e06 23 // PID factors
cudaChen 13:87cd0ae37e06 24 int interror = 0;
cudaChen 13:87cd0ae37e06 25 int olderror = 0;
cudaChen 13:87cd0ae37e06 26 int limit = 4096; // 12-bit ADC in STM32
cudaChen 12:e95ed962be7a 27
cudaChen 12:e95ed962be7a 28 // used for output message via serial
cudaChen 12:e95ed962be7a 29 Serial pc(SERIAL_TX, SERIAL_RX);
cudaChen 12:e95ed962be7a 30
cudaChen 12:e95ed962be7a 31 long map(long x, long in_min, long in_max, long out_min, long out_max)
cudaChen 12:e95ed962be7a 32 {
cudaChen 12:e95ed962be7a 33 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cudaChen 12:e95ed962be7a 34 }
cudaChen 12:e95ed962be7a 35
cudaChen 12:e95ed962be7a 36 void readIR(bool* left, bool* middle, bool* right, int threshold)
cudaChen 12:e95ed962be7a 37 {
cudaChen 12:e95ed962be7a 38 // not on track: > 500
cudaChen 12:e95ed962be7a 39 // on track (black): < 500
cudaChen 12:e95ed962be7a 40 *left = leftIR.read_u16() < threshold ? true : false;
cudaChen 12:e95ed962be7a 41 *middle = middleIR.read_u16() < threshold ? true : false;
cudaChen 12:e95ed962be7a 42 *right = rightIR.read_u16() < threshold ? true : false;
cudaChen 12:e95ed962be7a 43 }
cudaChen 12:e95ed962be7a 44
cudaChen 13:87cd0ae37e06 45 int readIRValues()
cudaChen 13:87cd0ae37e06 46 {
cudaChen 13:87cd0ae37e06 47 left = leftIR.read_u16();
cudaChen 13:87cd0ae37e06 48 middle = middleIR.read_u16();
cudaChen 13:87cd0ae37e06 49 right = rightIR.read_u16();
cudaChen 13:87cd0ae37e06 50
cudaChen 13:87cd0ae37e06 51 int ret = left * (-1) + middle * 0 + right * 1;
cudaChen 13:87cd0ae37e06 52
cudaChen 14:28cae134a304 53 pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
cudaChen 13:87cd0ae37e06 54 pc.printf("IR values: %d\r\n", ret);
cudaChen 13:87cd0ae37e06 55
cudaChen 13:87cd0ae37e06 56 return ret;
cudaChen 13:87cd0ae37e06 57 }
cudaChen 13:87cd0ae37e06 58
cudaChen 12:e95ed962be7a 59 // m: 0 => M1 1 => M2
cudaChen 12:e95ed962be7a 60 // power 0~10 dir: 0=>正向　1=>反向
cudaChen 12:e95ed962be7a 61 void DriveSingleMotor(int m, int speed, int dir)
cudaChen 12:e95ed962be7a 62 {
cudaChen 12:e95ed962be7a 63 /*int _speed = 0;
cudaChen 12:e95ed962be7a 64
cudaChen 12:e95ed962be7a 65 //　設定速度
cudaChen 12:e95ed962be7a 66 if (speed>10) speed=10;
cudaChen 12:e95ed962be7a 67 _speed = map(speed, 1, 10, 100, 255);
cudaChen 12:e95ed962be7a 68
cudaChen 12:e95ed962be7a 69 if (speed<=0) {
cudaChen 12:e95ed962be7a 70 speed=0;
cudaChen 12:e95ed962be7a 71 _speed=0;
cudaChen 12:e95ed962be7a 72 } else
cudaChen 12:e95ed962be7a 73 _speed = map(speed, 1, 10, 100, 255);*/
cudaChen 12:e95ed962be7a 74
cudaChen 12:e95ed962be7a 75 if (m == MOTOR_M1) {
cudaChen 12:e95ed962be7a 76 //　設定方向
cudaChen 12:e95ed962be7a 77 if (speed == PWR_STOP) {
cudaChen 12:e95ed962be7a 78 M1_in1 = 0;
cudaChen 12:e95ed962be7a 79 M1_in2 = 0;
cudaChen 12:e95ed962be7a 80 } else if (dir == DIR_FORWARD) {
cudaChen 12:e95ed962be7a 81 // right wheel go forward
cudaChen 12:e95ed962be7a 82 M1_in1 = 1;
cudaChen 12:e95ed962be7a 83 M1_in2 = 0;
cudaChen 12:e95ed962be7a 84 } else {
cudaChen 12:e95ed962be7a 85 // right wheel go backward
cudaChen 12:e95ed962be7a 86 M1_in1 = 0;
cudaChen 12:e95ed962be7a 87 M1_in2 = 1;
cudaChen 12:e95ed962be7a 88 }
cudaChen 12:e95ed962be7a 89 //M1_enable.write_u16(_speed); // 驅動馬達 右輪
cudaChen 12:e95ed962be7a 90 M1_enable.write(1);
cudaChen 12:e95ed962be7a 91
cudaChen 12:e95ed962be7a 92 } else if (m == MOTOR_M2) {
cudaChen 12:e95ed962be7a 93 //　設定方向
cudaChen 12:e95ed962be7a 94 if (speed == PWR_STOP) {
cudaChen 12:e95ed962be7a 95 M2_in3 = 0;
cudaChen 12:e95ed962be7a 96 M2_in4 = 0;
cudaChen 12:e95ed962be7a 97 } else if (dir == DIR_FORWARD) {
cudaChen 12:e95ed962be7a 98 //左輪前進
cudaChen 12:e95ed962be7a 99 M2_in3 = 1;
cudaChen 12:e95ed962be7a 100 M2_in4 = 0;
cudaChen 12:e95ed962be7a 101 } else {
cudaChen 12:e95ed962be7a 102 //左輪倒轉
cudaChen 12:e95ed962be7a 103 M2_in3 = 0;
cudaChen 12:e95ed962be7a 104 M2_in4 = 1;
cudaChen 12:e95ed962be7a 105 }
cudaChen 12:e95ed962be7a 106 //M2_enable.write_u16(_speed); // 驅動馬達 左輪
cudaChen 12:e95ed962be7a 107 M2_enable.write(1);
cudaChen 12:e95ed962be7a 108 } else {
cudaChen 12:e95ed962be7a 109 //M1_enable.write_u16(PWR_STOP); // right wheel
cudaChen 12:e95ed962be7a 110 //M2_enable.write_u16(PWR_STOP); // left wheel
cudaChen 12:e95ed962be7a 111 M1_enable.write(0);
cudaChen 12:e95ed962be7a 112 M2_enable.write(0);
cudaChen 12:e95ed962be7a 113 }
cudaChen 12:e95ed962be7a 114 }
cudaChen 12:e95ed962be7a 115
cudaChen 12:e95ed962be7a 116 void driveMotor(bool left, bool middle, bool right)
cudaChen 12:e95ed962be7a 117 {
cudaChen 12:e95ed962be7a 118 int status = left * 4 + middle * 2 + right;
cudaChen 12:e95ed962be7a 119 switch(status) {
cudaChen 12:e95ed962be7a 120 case 7: // go straight
cudaChen 12:e95ed962be7a 121 pc.printf("7. go straight\r\n");
cudaChen 12:e95ed962be7a 122 forward();
cudaChen 12:e95ed962be7a 123 break;
cudaChen 12:e95ed962be7a 124 case 6: // turn left
cudaChen 12:e95ed962be7a 125 pc.printf("6. turn left\r\n");
cudaChen 12:e95ed962be7a 126 turnLeft();
cudaChen 12:e95ed962be7a 127 break;
cudaChen 12:e95ed962be7a 128 case 5: // go straight
cudaChen 12:e95ed962be7a 129 pc.printf("5. go straight\r\n");
cudaChen 12:e95ed962be7a 130 forward();
cudaChen 12:e95ed962be7a 131 break;
cudaChen 12:e95ed962be7a 132 case 4: // turn left
cudaChen 12:e95ed962be7a 133 pc.printf("4. turn left\r\n");
cudaChen 12:e95ed962be7a 134 turnLeft();
cudaChen 12:e95ed962be7a 135 break;
cudaChen 12:e95ed962be7a 136 case 3: // turn right
cudaChen 12:e95ed962be7a 137 pc.printf("3. turn right\r\n");
cudaChen 12:e95ed962be7a 138 turnRight();
cudaChen 12:e95ed962be7a 139 break;
cudaChen 12:e95ed962be7a 140 case 2: // go straight
cudaChen 12:e95ed962be7a 141 pc.printf("2. go straight\r\n");
cudaChen 12:e95ed962be7a 142 forward();
cudaChen 12:e95ed962be7a 143 break;
cudaChen 12:e95ed962be7a 144 case 1: // turn right
cudaChen 12:e95ed962be7a 145 pc.printf("1. turn right\r\n");
cudaChen 12:e95ed962be7a 146 turnRight();
cudaChen 12:e95ed962be7a 147 break;
cudaChen 12:e95ed962be7a 148 case 0: // go straight
cudaChen 12:e95ed962be7a 149 pc.printf("0. go straight\r\n");
cudaChen 12:e95ed962be7a 150 forward();
cudaChen 12:e95ed962be7a 151 break;
cudaChen 12:e95ed962be7a 152 default:
cudaChen 12:e95ed962be7a 153 pc.printf("invalid\r\n");
cudaChen 12:e95ed962be7a 154 break;
cudaChen 12:e95ed962be7a 155 }
cudaChen 12:e95ed962be7a 156 }
cudaChen 12:e95ed962be7a 157
cudaChen 13:87cd0ae37e06 158 void driveMotorPID(int values, float Kp, float Ki, float Kd)
cudaChen 13:87cd0ae37e06 159 {
cudaChen 13:87cd0ae37e06 160 int error = values; // 'P'
cudaChen 13:87cd0ae37e06 161 interror += error;
cudaChen 13:87cd0ae37e06 162 int lasterror = error - olderror;
cudaChen 13:87cd0ae37e06 163 olderror = error; // 注意olderror的順序
cudaChen 13:87cd0ae37e06 164 int power = error * Kp + interror * Ki + lasterror * Kd;
cudaChen 13:87cd0ae37e06 165 /* error(P值)、interror(I值)、lasterror(D值)、olderror用來取D值的 */
cudaChen 13:87cd0ae37e06 166
cudaChen 13:87cd0ae37e06 167 // limit PID output value (for 12-bit ADC in STM32)
cudaChen 13:87cd0ae37e06 168 if(power > limit) {
cudaChen 13:87cd0ae37e06 169 power = limit;
cudaChen 13:87cd0ae37e06 170 } else if(power < -limit) {
cudaChen 13:87cd0ae37e06 171 power = -limit;
cudaChen 13:87cd0ae37e06 172 }
cudaChen 13:87cd0ae37e06 173
cudaChen 13:87cd0ae37e06 174 // control the direction of auto car
cudaChen 13:87cd0ae37e06 175 if(power < -900) {
cudaChen 13:87cd0ae37e06 176 turnLeft();
cudaChen 13:87cd0ae37e06 177 } else if (power > 900) {
cudaChen 13:87cd0ae37e06 178 turnRight();
cudaChen 13:87cd0ae37e06 179 } else {
cudaChen 13:87cd0ae37e06 180 forward();
cudaChen 13:87cd0ae37e06 181 }
cudaChen 13:87cd0ae37e06 182 }
cudaChen 13:87cd0ae37e06 183
cudaChen 12:e95ed962be7a 184 void init()
cudaChen 12:e95ed962be7a 185 {
cudaChen 12:e95ed962be7a 186 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 12:e95ed962be7a 187 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 12:e95ed962be7a 188 }
cudaChen 12:e95ed962be7a 189
cudaChen 12:e95ed962be7a 190 void forward()
cudaChen 12:e95ed962be7a 191 {
cudaChen 12:e95ed962be7a 192 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 12:e95ed962be7a 193 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 12:e95ed962be7a 194 }
cudaChen 12:e95ed962be7a 195
cudaChen 12:e95ed962be7a 196 void turnLeft()
cudaChen 12:e95ed962be7a 197 {
cudaChen 12:e95ed962be7a 198 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 12:e95ed962be7a 199 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 12:e95ed962be7a 200 }
cudaChen 12:e95ed962be7a 201
cudaChen 12:e95ed962be7a 202 void turnRight()
cudaChen 12:e95ed962be7a 203 {
cudaChen 12:e95ed962be7a 204 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 12:e95ed962be7a 205 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 12:e95ed962be7a 206 }