Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

main.cpp@0:190c4784b6f4, 2018-06-27 (annotated)

Committer:
cudaChen
Date:
Wed Jun 27 16:23:33 2018 +0000
Revision:
0:190c4784b6f4
Child:
1:ecbf974d0733
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cudaChen 0:190c4784b6f4 1 #include "mbed.h"
cudaChen 0:190c4784b6f4 2
cudaChen 0:190c4784b6f4 3 DigitalOut led1(LED1);
cudaChen 0:190c4784b6f4 4
cudaChen 0:190c4784b6f4 5 Serial pc(SERIAL_TX, SERIAL_RX);
cudaChen 0:190c4784b6f4 6
cudaChen 0:190c4784b6f4 7 // used for motor
cudaChen 0:190c4784b6f4 8 //AnalogOut M1_enable(D6);
cudaChen 0:190c4784b6f4 9 //AnalogOut M2_enable(D5);
cudaChen 0:190c4784b6f4 10 DigitalOut M1_enable(D6);
cudaChen 0:190c4784b6f4 11 DigitalOut M2_enable(D5);
cudaChen 0:190c4784b6f4 12 DigitalOut M1_in1(D12);
cudaChen 0:190c4784b6f4 13 DigitalOut M1_in2(D11);
cudaChen 0:190c4784b6f4 14 DigitalOut M2_in3(D10);
cudaChen 0:190c4784b6f4 15 DigitalOut M2_in4(D9);
cudaChen 0:190c4784b6f4 16 const int MOTOR_M1=0;
cudaChen 0:190c4784b6f4 17 const int MOTOR_M2=1;
cudaChen 0:190c4784b6f4 18 const int DIR_FORWARD= 0;
cudaChen 0:190c4784b6f4 19 const int DIR_BACKWARD=1;
cudaChen 0:190c4784b6f4 20 const int PWR_STOP = 0;
cudaChen 0:190c4784b6f4 21
cudaChen 0:190c4784b6f4 22 // used for IR sensors
cudaChen 0:190c4784b6f4 23 AnalogIn leftIR(A1);
cudaChen 0:190c4784b6f4 24 AnalogIn middleIR(A3);
cudaChen 0:190c4784b6f4 25 AnalogIn rightIR(A5);
cudaChen 0:190c4784b6f4 26
cudaChen 0:190c4784b6f4 27 long map(long x, long in_min, long in_max, long out_min, long out_max)
cudaChen 0:190c4784b6f4 28 {
cudaChen 0:190c4784b6f4 29 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cudaChen 0:190c4784b6f4 30 }
cudaChen 0:190c4784b6f4 31
cudaChen 0:190c4784b6f4 32 void DriveSingleMotor(int m, int speed, int dir);
cudaChen 0:190c4784b6f4 33
cudaChen 0:190c4784b6f4 34 // main() runs in its own thread in the OS
cudaChen 0:190c4784b6f4 35 int main()
cudaChen 0:190c4784b6f4 36 {
cudaChen 0:190c4784b6f4 37 bool left = false;
cudaChen 0:190c4784b6f4 38 bool middle = false;
cudaChen 0:190c4784b6f4 39 bool right = false;
cudaChen 0:190c4784b6f4 40
cudaChen 0:190c4784b6f4 41 while (true) {
cudaChen 0:190c4784b6f4 42 // here I use 500 as threshold
cudaChen 0:190c4784b6f4 43 // black: > 500
cudaChen 0:190c4784b6f4 44 // white: < 500
cudaChen 0:190c4784b6f4 45 left = leftIR.read_u16() > 500 ? true : false;
cudaChen 0:190c4784b6f4 46 middle = middleIR.read_u16() > 500 ? true : false;
cudaChen 0:190c4784b6f4 47 right = rightIR.read_u16() > 500 ? true : false;
cudaChen 0:190c4784b6f4 48
cudaChen 0:190c4784b6f4 49 led1 = 1; // light on on-board LED
cudaChen 0:190c4784b6f4 50
cudaChen 0:190c4784b6f4 51 // if middle detects black
cudaChen 0:190c4784b6f4 52 if(middle) {
cudaChen 0:190c4784b6f4 53 if(left && right) { // cliff
cudaChen 0:190c4784b6f4 54 // stop
cudaChen 0:190c4784b6f4 55 pc.printf("cliff\r\n");
cudaChen 0:190c4784b6f4 56 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 0:190c4784b6f4 57 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 0:190c4784b6f4 58 // pause for 0.5 sec
cudaChen 0:190c4784b6f4 59 wait_ms(1000);
cudaChen 0:190c4784b6f4 60
cudaChen 0:190c4784b6f4 61 // then go backward
cudaChen 0:190c4784b6f4 62 DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD);
cudaChen 0:190c4784b6f4 63 DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD);
cudaChen 0:190c4784b6f4 64 } else if(left) { // turn left
cudaChen 0:190c4784b6f4 65 pc.printf("turn left\r\n");
cudaChen 0:190c4784b6f4 66 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 0:190c4784b6f4 67 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 0:190c4784b6f4 68 } else if(right) { // turn right
cudaChen 0:190c4784b6f4 69 pc.printf("turn right\r\n");
cudaChen 0:190c4784b6f4 70 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 0:190c4784b6f4 71 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 0:190c4784b6f4 72 } else { // go straight
cudaChen 0:190c4784b6f4 73 pc.printf("go straight\r\n");
cudaChen 0:190c4784b6f4 74 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 0:190c4784b6f4 75 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 0:190c4784b6f4 76 }
cudaChen 0:190c4784b6f4 77
cudaChen 0:190c4784b6f4 78 //wait_ms(1000);
cudaChen 0:190c4784b6f4 79 }
cudaChen 0:190c4784b6f4 80
cudaChen 0:190c4784b6f4 81 // if all detects white, which means the car is out of track
cudaChen 0:190c4784b6f4 82 if(middle && left && right) { // go backward
cudaChen 0:190c4784b6f4 83 pc.printf("out of track, go backward\r\n");
cudaChen 0:190c4784b6f4 84 DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD);
cudaChen 0:190c4784b6f4 85 DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD);
cudaChen 0:190c4784b6f4 86 //wait_ms(1000);
cudaChen 0:190c4784b6f4 87 }
cudaChen 0:190c4784b6f4 88
cudaChen 0:190c4784b6f4 89 pc.printf("pause\r\n");
cudaChen 0:190c4784b6f4 90 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 0:190c4784b6f4 91 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 0:190c4784b6f4 92 led1 = 0; // turn of on-board LED
cudaChen 0:190c4784b6f4 93 wait_ms(1000);
cudaChen 0:190c4784b6f4 94 }
cudaChen 0:190c4784b6f4 95 }
cudaChen 0:190c4784b6f4 96
cudaChen 0:190c4784b6f4 97 // m: 0 => M1 1 => M2
cudaChen 0:190c4784b6f4 98 // power 0~10 dir: 0=>正向　1=>反向
cudaChen 0:190c4784b6f4 99 void DriveSingleMotor(int m, int speed, int dir)
cudaChen 0:190c4784b6f4 100 {
cudaChen 0:190c4784b6f4 101 /*int _speed = 0;
cudaChen 0:190c4784b6f4 102
cudaChen 0:190c4784b6f4 103 //　設定速度
cudaChen 0:190c4784b6f4 104 if (speed>10) speed=10;
cudaChen 0:190c4784b6f4 105 _speed = map(speed, 1, 10, 100, 255);
cudaChen 0:190c4784b6f4 106
cudaChen 0:190c4784b6f4 107 if (speed<=0) {
cudaChen 0:190c4784b6f4 108 speed=0;
cudaChen 0:190c4784b6f4 109 _speed=0;
cudaChen 0:190c4784b6f4 110 } else
cudaChen 0:190c4784b6f4 111 _speed = map(speed, 1, 10, 100, 255);*/
cudaChen 0:190c4784b6f4 112
cudaChen 0:190c4784b6f4 113 if (m == MOTOR_M1) {
cudaChen 0:190c4784b6f4 114 //　設定方向
cudaChen 0:190c4784b6f4 115 if (speed == PWR_STOP) {
cudaChen 0:190c4784b6f4 116 M1_in1 = 0;
cudaChen 0:190c4784b6f4 117 M1_in2 = 0;
cudaChen 0:190c4784b6f4 118 } else if (dir == DIR_FORWARD) {
cudaChen 0:190c4784b6f4 119 // right wheel go forward
cudaChen 0:190c4784b6f4 120 M1_in1 = 1;
cudaChen 0:190c4784b6f4 121 M1_in2 = 0;
cudaChen 0:190c4784b6f4 122 } else {
cudaChen 0:190c4784b6f4 123 // right wheel go backward
cudaChen 0:190c4784b6f4 124 M1_in1 = 0;
cudaChen 0:190c4784b6f4 125 M1_in2 = 1;
cudaChen 0:190c4784b6f4 126 }
cudaChen 0:190c4784b6f4 127 //M1_enable.write_u16(_speed); // 驅動馬達 右輪
cudaChen 0:190c4784b6f4 128 M1_enable.write(1);
cudaChen 0:190c4784b6f4 129
cudaChen 0:190c4784b6f4 130 } else if (m == MOTOR_M2) {
cudaChen 0:190c4784b6f4 131 //　設定方向
cudaChen 0:190c4784b6f4 132 if (speed == PWR_STOP) {
cudaChen 0:190c4784b6f4 133 M2_in3 = 0;
cudaChen 0:190c4784b6f4 134 M2_in4 = 0;
cudaChen 0:190c4784b6f4 135 } else if (dir == DIR_FORWARD) {
cudaChen 0:190c4784b6f4 136 //左輪前進
cudaChen 0:190c4784b6f4 137 M2_in3 = 1;
cudaChen 0:190c4784b6f4 138 M2_in4 = 0;
cudaChen 0:190c4784b6f4 139 } else {
cudaChen 0:190c4784b6f4 140 //左輪倒轉
cudaChen 0:190c4784b6f4 141 M2_in3 = 0;
cudaChen 0:190c4784b6f4 142 M2_in4 = 1;
cudaChen 0:190c4784b6f4 143 }
cudaChen 0:190c4784b6f4 144 //M2_enable.write_u16(_speed); // 驅動馬達 左輪
cudaChen 0:190c4784b6f4 145 M2_enable.write(1);
cudaChen 0:190c4784b6f4 146 } else {
cudaChen 0:190c4784b6f4 147 //M1_enable.write_u16(PWR_STOP); // right wheel
cudaChen 0:190c4784b6f4 148 //M2_enable.write_u16(PWR_STOP); // left wheel
cudaChen 0:190c4784b6f4 149 M1_enable.write(0);
cudaChen 0:190c4784b6f4 150 M2_enable.write(0);
cudaChen 0:190c4784b6f4 151 }
cudaChen 0:190c4784b6f4 152 }
cudaChen 0:190c4784b6f4 153