Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

main.cpp@11:3e9d4c345ebd, 2018-06-30 (annotated)

Committer:
cudaChen
Date:
Sat Jun 30 12:41:24 2018 +0000
Revision:
11:3e9d4c345ebd
Parent:
10:a14380381d86
Child:
12:e95ed962be7a
[add] add new function: readIR

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cudaChen 0:190c4784b6f4 1 #include "mbed.h"
cudaChen 0:190c4784b6f4 2
cudaChen 7:4edd049209a6 3 // trigger button and triggered LED
cudaChen 0:190c4784b6f4 4 DigitalOut led1(LED1);
cudaChen 7:4edd049209a6 5 DigitalIn pb(PC_13);
cudaChen 7:4edd049209a6 6 int lastButtonState = 0;
cudaChen 7:4edd049209a6 7 bool ledState = false;
cudaChen 0:190c4784b6f4 8
cudaChen 0:190c4784b6f4 9 Serial pc(SERIAL_TX, SERIAL_RX);
cudaChen 0:190c4784b6f4 10
cudaChen 0:190c4784b6f4 11 // used for motor
cudaChen 0:190c4784b6f4 12 //AnalogOut M1_enable(D6);
cudaChen 0:190c4784b6f4 13 //AnalogOut M2_enable(D5);
cudaChen 0:190c4784b6f4 14 DigitalOut M1_enable(D6);
cudaChen 0:190c4784b6f4 15 DigitalOut M2_enable(D5);
cudaChen 9:ce1dd89ff5e8 16 DigitalOut M1_in1(D3);
cudaChen 9:ce1dd89ff5e8 17 DigitalOut M1_in2(D2);
cudaChen 9:ce1dd89ff5e8 18 DigitalOut M2_in3(D1);
cudaChen 9:ce1dd89ff5e8 19 DigitalOut M2_in4(D0);
cudaChen 10:a14380381d86 20 const int MOTOR_M1 = 0;
cudaChen 10:a14380381d86 21 const int MOTOR_M2 = 1;
cudaChen 10:a14380381d86 22 const int DIR_FORWARD = 0;
cudaChen 10:a14380381d86 23 const int DIR_BACKWARD = 1;
cudaChen 0:190c4784b6f4 24 const int PWR_STOP = 0;
cudaChen 0:190c4784b6f4 25
cudaChen 0:190c4784b6f4 26 // used for IR sensors
cudaChen 0:190c4784b6f4 27 AnalogIn leftIR(A1);
cudaChen 0:190c4784b6f4 28 AnalogIn middleIR(A3);
cudaChen 0:190c4784b6f4 29 AnalogIn rightIR(A5);
cudaChen 11:3e9d4c345ebd 30 void readIR(bool* left, bool* middle, bool* right, int threshold);
cudaChen 10:a14380381d86 31
cudaChen 0:190c4784b6f4 32 long map(long x, long in_min, long in_max, long out_min, long out_max)
cudaChen 0:190c4784b6f4 33 {
cudaChen 0:190c4784b6f4 34 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cudaChen 0:190c4784b6f4 35 }
cudaChen 0:190c4784b6f4 36
cudaChen 11:3e9d4c345ebd 37 // used for controlling the direction of auto car
cudaChen 0:190c4784b6f4 38 void DriveSingleMotor(int m, int speed, int dir);
cudaChen 5:25faf509ee9b 39 void driveMotor(bool left, bool middle, bool right);
cudaChen 10:a14380381d86 40 void init();
cudaChen 11:3e9d4c345ebd 41 void forward();
cudaChen 11:3e9d4c345ebd 42 void turnLeft();
cudaChen 11:3e9d4c345ebd 43 void turnRight();
cudaChen 0:190c4784b6f4 44
cudaChen 0:190c4784b6f4 45 // main() runs in its own thread in the OS
cudaChen 0:190c4784b6f4 46 int main()
cudaChen 0:190c4784b6f4 47 {
cudaChen 0:190c4784b6f4 48 bool left = false;
cudaChen 0:190c4784b6f4 49 bool middle = false;
cudaChen 0:190c4784b6f4 50 bool right = false;
cudaChen 4:76b9213714cc 51
cudaChen 2:e0a553b64315 52 // here I use 500 as threshold
cudaChen 2:e0a553b64315 53 int threshold = 500;
cudaChen 0:190c4784b6f4 54
cudaChen 10:a14380381d86 55 // set two motors to stop
cudaChen 10:a14380381d86 56 init();
cudaChen 10:a14380381d86 57
cudaChen 4:76b9213714cc 58 while (true) {
cudaChen 7:4edd049209a6 59 int reading1 = pb.read();
cudaChen 10:a14380381d86 60
cudaChen 7:4edd049209a6 61 if(reading1 != lastButtonState) {
cudaChen 7:4edd049209a6 62 wait_ms(20);
cudaChen 10:a14380381d86 63
cudaChen 7:4edd049209a6 64 int reading2 = pb.read();
cudaChen 10:a14380381d86 65
cudaChen 7:4edd049209a6 66 if(reading2 == reading1) {
cudaChen 7:4edd049209a6 67 lastButtonState = reading2;
cudaChen 7:4edd049209a6 68 }
cudaChen 10:a14380381d86 69
cudaChen 7:4edd049209a6 70 if(lastButtonState == 1) {
cudaChen 7:4edd049209a6 71 ledState = !ledState;
cudaChen 7:4edd049209a6 72 }
cudaChen 7:4edd049209a6 73 }
cudaChen 10:a14380381d86 74
cudaChen 7:4edd049209a6 75 led1.write(ledState);
cudaChen 10:a14380381d86 76
cudaChen 7:4edd049209a6 77 if(ledState) {
cudaChen 10:a14380381d86 78 // not on track: > 500
cudaChen 10:a14380381d86 79 // on track (black): < 500
cudaChen 11:3e9d4c345ebd 80 /*left = leftIR.read_u16() < threshold ? true : false;
cudaChen 10:a14380381d86 81 middle = middleIR.read_u16() < threshold ? true : false;
cudaChen 11:3e9d4c345ebd 82 right = rightIR.read_u16() < threshold ? true : false;*/
cudaChen 11:3e9d4c345ebd 83 readIR(&left, &middle, &right, threshold);
cudaChen 0:190c4784b6f4 84
cudaChen 10:a14380381d86 85 driveMotor(left, middle, right);
cudaChen 7:4edd049209a6 86 }
cudaChen 0:190c4784b6f4 87 }
cudaChen 0:190c4784b6f4 88 }
cudaChen 0:190c4784b6f4 89
cudaChen 11:3e9d4c345ebd 90 void readIR(bool* left, bool* middle, bool* right, int threshold)
cudaChen 11:3e9d4c345ebd 91 {
cudaChen 11:3e9d4c345ebd 92 // not on track: > 500
cudaChen 11:3e9d4c345ebd 93 // on track (black): < 500
cudaChen 11:3e9d4c345ebd 94 *left = leftIR.read_u16() < threshold ? true : false;
cudaChen 11:3e9d4c345ebd 95 *middle = middleIR.read_u16() < threshold ? true : false;
cudaChen 11:3e9d4c345ebd 96 *right = rightIR.read_u16() < threshold ? true : false;
cudaChen 11:3e9d4c345ebd 97 }
cudaChen 11:3e9d4c345ebd 98
cudaChen 0:190c4784b6f4 99 // m: 0 => M1 1 => M2
cudaChen 0:190c4784b6f4 100 // power 0~10 dir: 0=>正向　1=>反向
cudaChen 0:190c4784b6f4 101 void DriveSingleMotor(int m, int speed, int dir)
cudaChen 0:190c4784b6f4 102 {
cudaChen 0:190c4784b6f4 103 /*int _speed = 0;
cudaChen 0:190c4784b6f4 104
cudaChen 0:190c4784b6f4 105 //　設定速度
cudaChen 0:190c4784b6f4 106 if (speed>10) speed=10;
cudaChen 0:190c4784b6f4 107 _speed = map(speed, 1, 10, 100, 255);
cudaChen 0:190c4784b6f4 108
cudaChen 0:190c4784b6f4 109 if (speed<=0) {
cudaChen 0:190c4784b6f4 110 speed=0;
cudaChen 0:190c4784b6f4 111 _speed=0;
cudaChen 0:190c4784b6f4 112 } else
cudaChen 0:190c4784b6f4 113 _speed = map(speed, 1, 10, 100, 255);*/
cudaChen 0:190c4784b6f4 114
cudaChen 0:190c4784b6f4 115 if (m == MOTOR_M1) {
cudaChen 0:190c4784b6f4 116 //　設定方向
cudaChen 0:190c4784b6f4 117 if (speed == PWR_STOP) {
cudaChen 0:190c4784b6f4 118 M1_in1 = 0;
cudaChen 0:190c4784b6f4 119 M1_in2 = 0;
cudaChen 0:190c4784b6f4 120 } else if (dir == DIR_FORWARD) {
cudaChen 0:190c4784b6f4 121 // right wheel go forward
cudaChen 0:190c4784b6f4 122 M1_in1 = 1;
cudaChen 0:190c4784b6f4 123 M1_in2 = 0;
cudaChen 0:190c4784b6f4 124 } else {
cudaChen 0:190c4784b6f4 125 // right wheel go backward
cudaChen 0:190c4784b6f4 126 M1_in1 = 0;
cudaChen 0:190c4784b6f4 127 M1_in2 = 1;
cudaChen 0:190c4784b6f4 128 }
cudaChen 0:190c4784b6f4 129 //M1_enable.write_u16(_speed); // 驅動馬達 右輪
cudaChen 0:190c4784b6f4 130 M1_enable.write(1);
cudaChen 0:190c4784b6f4 131
cudaChen 0:190c4784b6f4 132 } else if (m == MOTOR_M2) {
cudaChen 0:190c4784b6f4 133 //　設定方向
cudaChen 0:190c4784b6f4 134 if (speed == PWR_STOP) {
cudaChen 0:190c4784b6f4 135 M2_in3 = 0;
cudaChen 0:190c4784b6f4 136 M2_in4 = 0;
cudaChen 0:190c4784b6f4 137 } else if (dir == DIR_FORWARD) {
cudaChen 0:190c4784b6f4 138 //左輪前進
cudaChen 0:190c4784b6f4 139 M2_in3 = 1;
cudaChen 0:190c4784b6f4 140 M2_in4 = 0;
cudaChen 0:190c4784b6f4 141 } else {
cudaChen 0:190c4784b6f4 142 //左輪倒轉
cudaChen 0:190c4784b6f4 143 M2_in3 = 0;
cudaChen 0:190c4784b6f4 144 M2_in4 = 1;
cudaChen 0:190c4784b6f4 145 }
cudaChen 0:190c4784b6f4 146 //M2_enable.write_u16(_speed); // 驅動馬達 左輪
cudaChen 0:190c4784b6f4 147 M2_enable.write(1);
cudaChen 0:190c4784b6f4 148 } else {
cudaChen 0:190c4784b6f4 149 //M1_enable.write_u16(PWR_STOP); // right wheel
cudaChen 0:190c4784b6f4 150 //M2_enable.write_u16(PWR_STOP); // left wheel
cudaChen 0:190c4784b6f4 151 M1_enable.write(0);
cudaChen 0:190c4784b6f4 152 M2_enable.write(0);
cudaChen 0:190c4784b6f4 153 }
cudaChen 0:190c4784b6f4 154 }
cudaChen 0:190c4784b6f4 155
cudaChen 5:25faf509ee9b 156 void driveMotor(bool left, bool middle, bool right)
cudaChen 5:25faf509ee9b 157 {
cudaChen 6:f34538eea724 158 int status = left * 4 + middle * 2 + right;
cudaChen 6:f34538eea724 159 switch(status) {
cudaChen 6:f34538eea724 160 case 7: // go straight
cudaChen 6:f34538eea724 161 pc.printf("7. go straight\r\n");
cudaChen 10:a14380381d86 162 forward();
cudaChen 6:f34538eea724 163 break;
cudaChen 6:f34538eea724 164 case 6: // turn left
cudaChen 6:f34538eea724 165 pc.printf("6. turn left\r\n");
cudaChen 10:a14380381d86 166 turnLeft();
cudaChen 6:f34538eea724 167 break;
cudaChen 6:f34538eea724 168 case 5: // go straight
cudaChen 6:f34538eea724 169 pc.printf("5. go straight\r\n");
cudaChen 10:a14380381d86 170 forward();
cudaChen 6:f34538eea724 171 break;
cudaChen 6:f34538eea724 172 case 4: // turn left
cudaChen 6:f34538eea724 173 pc.printf("4. turn left\r\n");
cudaChen 10:a14380381d86 174 turnLeft();
cudaChen 6:f34538eea724 175 break;
cudaChen 6:f34538eea724 176 case 3: // turn right
cudaChen 6:f34538eea724 177 pc.printf("3. turn right\r\n");
cudaChen 10:a14380381d86 178 turnRight();
cudaChen 6:f34538eea724 179 break;
cudaChen 6:f34538eea724 180 case 2: // go straight
cudaChen 6:f34538eea724 181 pc.printf("2. go straight\r\n");
cudaChen 10:a14380381d86 182 forward();
cudaChen 6:f34538eea724 183 break;
cudaChen 6:f34538eea724 184 case 1: // turn right
cudaChen 6:f34538eea724 185 pc.printf("1. turn right\r\n");
cudaChen 10:a14380381d86 186 turnRight();
cudaChen 6:f34538eea724 187 break;
cudaChen 6:f34538eea724 188 case 0: // go straight
cudaChen 6:f34538eea724 189 pc.printf("0. go straight\r\n");
cudaChen 10:a14380381d86 190 forward();
cudaChen 6:f34538eea724 191 break;
cudaChen 6:f34538eea724 192 default:
cudaChen 6:f34538eea724 193 pc.printf("invalid\r\n");
cudaChen 6:f34538eea724 194 break;
cudaChen 6:f34538eea724 195 }
cudaChen 5:25faf509ee9b 196 }
cudaChen 5:25faf509ee9b 197
cudaChen 10:a14380381d86 198 void init()
cudaChen 10:a14380381d86 199 {
cudaChen 10:a14380381d86 200 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 10:a14380381d86 201 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 10:a14380381d86 202 }
cudaChen 10:a14380381d86 203
cudaChen 10:a14380381d86 204 void forward()
cudaChen 10:a14380381d86 205 {
cudaChen 10:a14380381d86 206 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 10:a14380381d86 207 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 10:a14380381d86 208 }
cudaChen 10:a14380381d86 209
cudaChen 10:a14380381d86 210 void turnLeft()
cudaChen 10:a14380381d86 211 {
cudaChen 10:a14380381d86 212 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 10:a14380381d86 213 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 10:a14380381d86 214 }
cudaChen 10:a14380381d86 215
cudaChen 10:a14380381d86 216 void turnRight()
cudaChen 10:a14380381d86 217 {
cudaChen 10:a14380381d86 218 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 10:a14380381d86 219 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 10:a14380381d86 220 }
cudaChen 10:a14380381d86 221
cudaChen 10:a14380381d86 222