Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

main.cpp@6:f34538eea724, 2018-06-29 (annotated)

Committer:
cudaChen
Date:
Fri Jun 29 13:12:48 2018 +0000
Revision:
6:f34538eea724
Parent:
5:25faf509ee9b
Child:
7:4edd049209a6
add switch statement in function 'driveMotor' and remove some comments

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 5:25faf509ee9b 33 void driveMotor(bool left, bool middle, bool right);
cudaChen 0:190c4784b6f4 34
cudaChen 0:190c4784b6f4 35 // main() runs in its own thread in the OS
cudaChen 0:190c4784b6f4 36 int main()
cudaChen 0:190c4784b6f4 37 {
cudaChen 0:190c4784b6f4 38 bool left = false;
cudaChen 0:190c4784b6f4 39 bool middle = false;
cudaChen 0:190c4784b6f4 40 bool right = false;
cudaChen 4:76b9213714cc 41
cudaChen 2:e0a553b64315 42 // here I use 500 as threshold
cudaChen 2:e0a553b64315 43 int threshold = 500;
cudaChen 0:190c4784b6f4 44
cudaChen 4:76b9213714cc 45 while (true) {
cudaChen 2:e0a553b64315 46 // not on track: > 500
cudaChen 2:e0a553b64315 47 // on track (black): < 500
cudaChen 2:e0a553b64315 48 left = leftIR.read_u16() < threshold ? true : false;
cudaChen 2:e0a553b64315 49 middle = middleIR.read_u16() < threshold ? true : false;
cudaChen 2:e0a553b64315 50 right = rightIR.read_u16() < threshold ? true : false;
cudaChen 0:190c4784b6f4 51
cudaChen 5:25faf509ee9b 52 driveMotor(left, middle, right);
cudaChen 0:190c4784b6f4 53 }
cudaChen 0:190c4784b6f4 54 }
cudaChen 0:190c4784b6f4 55
cudaChen 0:190c4784b6f4 56 // m: 0 => M1 1 => M2
cudaChen 0:190c4784b6f4 57 // power 0~10 dir: 0=>正向　1=>反向
cudaChen 0:190c4784b6f4 58 void DriveSingleMotor(int m, int speed, int dir)
cudaChen 0:190c4784b6f4 59 {
cudaChen 0:190c4784b6f4 60 /*int _speed = 0;
cudaChen 0:190c4784b6f4 61
cudaChen 0:190c4784b6f4 62 //　設定速度
cudaChen 0:190c4784b6f4 63 if (speed>10) speed=10;
cudaChen 0:190c4784b6f4 64 _speed = map(speed, 1, 10, 100, 255);
cudaChen 0:190c4784b6f4 65
cudaChen 0:190c4784b6f4 66 if (speed<=0) {
cudaChen 0:190c4784b6f4 67 speed=0;
cudaChen 0:190c4784b6f4 68 _speed=0;
cudaChen 0:190c4784b6f4 69 } else
cudaChen 0:190c4784b6f4 70 _speed = map(speed, 1, 10, 100, 255);*/
cudaChen 0:190c4784b6f4 71
cudaChen 0:190c4784b6f4 72 if (m == MOTOR_M1) {
cudaChen 0:190c4784b6f4 73 //　設定方向
cudaChen 0:190c4784b6f4 74 if (speed == PWR_STOP) {
cudaChen 0:190c4784b6f4 75 M1_in1 = 0;
cudaChen 0:190c4784b6f4 76 M1_in2 = 0;
cudaChen 0:190c4784b6f4 77 } else if (dir == DIR_FORWARD) {
cudaChen 0:190c4784b6f4 78 // right wheel go forward
cudaChen 0:190c4784b6f4 79 M1_in1 = 1;
cudaChen 0:190c4784b6f4 80 M1_in2 = 0;
cudaChen 0:190c4784b6f4 81 } else {
cudaChen 0:190c4784b6f4 82 // right wheel go backward
cudaChen 0:190c4784b6f4 83 M1_in1 = 0;
cudaChen 0:190c4784b6f4 84 M1_in2 = 1;
cudaChen 0:190c4784b6f4 85 }
cudaChen 0:190c4784b6f4 86 //M1_enable.write_u16(_speed); // 驅動馬達 右輪
cudaChen 0:190c4784b6f4 87 M1_enable.write(1);
cudaChen 0:190c4784b6f4 88
cudaChen 0:190c4784b6f4 89 } else if (m == MOTOR_M2) {
cudaChen 0:190c4784b6f4 90 //　設定方向
cudaChen 0:190c4784b6f4 91 if (speed == PWR_STOP) {
cudaChen 0:190c4784b6f4 92 M2_in3 = 0;
cudaChen 0:190c4784b6f4 93 M2_in4 = 0;
cudaChen 0:190c4784b6f4 94 } else if (dir == DIR_FORWARD) {
cudaChen 0:190c4784b6f4 95 //左輪前進
cudaChen 0:190c4784b6f4 96 M2_in3 = 1;
cudaChen 0:190c4784b6f4 97 M2_in4 = 0;
cudaChen 0:190c4784b6f4 98 } else {
cudaChen 0:190c4784b6f4 99 //左輪倒轉
cudaChen 0:190c4784b6f4 100 M2_in3 = 0;
cudaChen 0:190c4784b6f4 101 M2_in4 = 1;
cudaChen 0:190c4784b6f4 102 }
cudaChen 0:190c4784b6f4 103 //M2_enable.write_u16(_speed); // 驅動馬達 左輪
cudaChen 0:190c4784b6f4 104 M2_enable.write(1);
cudaChen 0:190c4784b6f4 105 } else {
cudaChen 0:190c4784b6f4 106 //M1_enable.write_u16(PWR_STOP); // right wheel
cudaChen 0:190c4784b6f4 107 //M2_enable.write_u16(PWR_STOP); // left wheel
cudaChen 0:190c4784b6f4 108 M1_enable.write(0);
cudaChen 0:190c4784b6f4 109 M2_enable.write(0);
cudaChen 0:190c4784b6f4 110 }
cudaChen 0:190c4784b6f4 111 }
cudaChen 0:190c4784b6f4 112
cudaChen 5:25faf509ee9b 113 void driveMotor(bool left, bool middle, bool right)
cudaChen 5:25faf509ee9b 114 {
cudaChen 6:f34538eea724 115 int status = left * 4 + middle * 2 + right;
cudaChen 6:f34538eea724 116 switch(status) {
cudaChen 6:f34538eea724 117 case 7: // go straight
cudaChen 6:f34538eea724 118 pc.printf("7. go straight\r\n");
cudaChen 6:f34538eea724 119 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 120 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 121 break;
cudaChen 6:f34538eea724 122 case 6: // turn left
cudaChen 6:f34538eea724 123 pc.printf("6. turn left\r\n");
cudaChen 6:f34538eea724 124 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 6:f34538eea724 125 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 126 break;
cudaChen 6:f34538eea724 127 case 5: // go straight
cudaChen 6:f34538eea724 128 pc.printf("5. go straight\r\n");
cudaChen 6:f34538eea724 129 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 130 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 131 break;
cudaChen 6:f34538eea724 132 case 4: // turn left
cudaChen 6:f34538eea724 133 pc.printf("4. turn left\r\n");
cudaChen 6:f34538eea724 134 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
cudaChen 6:f34538eea724 135 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 136 break;
cudaChen 6:f34538eea724 137 case 3: // turn right
cudaChen 6:f34538eea724 138 pc.printf("3. turn right\r\n");
cudaChen 6:f34538eea724 139 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 140 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 6:f34538eea724 141 break;
cudaChen 6:f34538eea724 142 case 2: // go straight
cudaChen 6:f34538eea724 143 pc.printf("2. go straight\r\n");
cudaChen 6:f34538eea724 144 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 145 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 146 break;
cudaChen 6:f34538eea724 147 case 1: // turn right
cudaChen 6:f34538eea724 148 pc.printf("1. turn right\r\n");
cudaChen 6:f34538eea724 149 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 150 DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
cudaChen 6:f34538eea724 151 break;
cudaChen 6:f34538eea724 152 case 0: // go straight
cudaChen 6:f34538eea724 153 pc.printf("0. go straight\r\n");
cudaChen 6:f34538eea724 154 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 155 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
cudaChen 6:f34538eea724 156 break;
cudaChen 6:f34538eea724 157 default:
cudaChen 6:f34538eea724 158 pc.printf("invalid\r\n");
cudaChen 6:f34538eea724 159 break;
cudaChen 6:f34538eea724 160 }
cudaChen 5:25faf509ee9b 161 }
cudaChen 5:25faf509ee9b 162