Pranav Marathe / Mbed 2 deprecated BluetoothControlledRCAutomaticBraking

A bluetooth controlled RC car with automatic braking and automatic headlights

Dependencies:   HC_SR04_Ultrasonic_Library Motor mbed-rtos mbed

main.cpp@0:f1fab1a647f6, 2017-12-13 (annotated)

Committer:
pmarathe25
Date:
Wed Dec 13 21:44:47 2017 +0000
Revision:
0:f1fab1a647f6
Initial Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmarathe25 0:f1fab1a647f6 1 #include "mbed.h"
pmarathe25 0:f1fab1a647f6 2 #include "rtos.h"
pmarathe25 0:f1fab1a647f6 3 #include "Motor.h"
pmarathe25 0:f1fab1a647f6 4 #include "ultrasonic.h"
pmarathe25 0:f1fab1a647f6 5
pmarathe25 0:f1fab1a647f6 6 Thread distance, headlightT;
pmarathe25 0:f1fab1a647f6 7 Mutex motorSpeeds;
pmarathe25 0:f1fab1a647f6 8
pmarathe25 0:f1fab1a647f6 9 // DEBUG
pmarathe25 0:f1fab1a647f6 10 Serial pc(USBTX, USBRX);
pmarathe25 0:f1fab1a647f6 11 PwmOut led(LED1);
pmarathe25 0:f1fab1a647f6 12 PwmOut led3(LED2);
pmarathe25 0:f1fab1a647f6 13
pmarathe25 0:f1fab1a647f6 14 PwmOut headlights(p24);
pmarathe25 0:f1fab1a647f6 15
pmarathe25 0:f1fab1a647f6 16 // Light sensor
pmarathe25 0:f1fab1a647f6 17 AnalogIn photocell(p15);
pmarathe25 0:f1fab1a647f6 18
pmarathe25 0:f1fab1a647f6 19 // Bluetooth
pmarathe25 0:f1fab1a647f6 20 Serial blue(p28, p27);
pmarathe25 0:f1fab1a647f6 21 // Motors
pmarathe25 0:f1fab1a647f6 22 Motor leftMotor(p22, p8, p9); // pwm, fwd, rev
pmarathe25 0:f1fab1a647f6 23 Motor rightMotor(p21, p10, p11); // pwm, fwd, rev
pmarathe25 0:f1fab1a647f6 24
pmarathe25 0:f1fab1a647f6 25 // Motor controls
pmarathe25 0:f1fab1a647f6 26 float leftMotorForwardMax = 1.0f, rightMotorForwardMax = 1.0f,
pmarathe25 0:f1fab1a647f6 27 leftMotorBackwardMax = 1.0f, rightMotorBackwardMax = 1.0f,
pmarathe25 0:f1fab1a647f6 28 leftMotorCurrentSpeed = 0.0f, rightMotorCurrentSpeed = 0.0f,
pmarathe25 0:f1fab1a647f6 29 leftCompensationScale = 0.85f, rightCompensationScale = 1.0f;
pmarathe25 0:f1fab1a647f6 30 const float MAX_BRAKING_DISTANCE = 800.0f;
pmarathe25 0:f1fab1a647f6 31
pmarathe25 0:f1fab1a647f6 32 void setMotorSpeeds(float leftScale = 1.0, float rightScale = 1.0) {
pmarathe25 0:f1fab1a647f6 33 leftScale *= leftCompensationScale;
pmarathe25 0:f1fab1a647f6 34 rightScale *= rightCompensationScale;
pmarathe25 0:f1fab1a647f6 35 leftMotorCurrentSpeed = (leftScale > 0.0) ? leftScale * leftMotorForwardMax : leftScale * leftMotorBackwardMax;
pmarathe25 0:f1fab1a647f6 36 rightMotorCurrentSpeed = (rightScale > 0.0) ? rightScale * rightMotorForwardMax : rightScale * rightMotorBackwardMax;
pmarathe25 0:f1fab1a647f6 37 //pc.printf("Setting left motor to %d\n", leftMotorCurrentSpeed);
pmarathe25 0:f1fab1a647f6 38 }
pmarathe25 0:f1fab1a647f6 39
pmarathe25 0:f1fab1a647f6 40 void updateMotors() {
pmarathe25 0:f1fab1a647f6 41 // Set motors
pmarathe25 0:f1fab1a647f6 42 leftMotor.speed((leftMotorCurrentSpeed > leftMotorForwardMax) ? leftMotorForwardMax : leftMotorCurrentSpeed);
pmarathe25 0:f1fab1a647f6 43 rightMotor.speed((rightMotorCurrentSpeed > rightMotorForwardMax) ? rightMotorForwardMax : rightMotorCurrentSpeed);
pmarathe25 0:f1fab1a647f6 44 //pc.printf("Pushing %d to left motor\n", leftMotorCurrentSpeed);
pmarathe25 0:f1fab1a647f6 45 }
pmarathe25 0:f1fab1a647f6 46
pmarathe25 0:f1fab1a647f6 47 void setForwardMaxSpeed(float left = 1.0, float right = 1.0) {
pmarathe25 0:f1fab1a647f6 48 leftMotorForwardMax = left;
pmarathe25 0:f1fab1a647f6 49 rightMotorForwardMax = right;
pmarathe25 0:f1fab1a647f6 50 }
pmarathe25 0:f1fab1a647f6 51
pmarathe25 0:f1fab1a647f6 52 void onDistanceChanged(int distance) {
pmarathe25 0:f1fab1a647f6 53 // Lock speeds before updating.
pmarathe25 0:f1fab1a647f6 54 motorSpeeds.lock();
pmarathe25 0:f1fab1a647f6 55 if (distance > MAX_BRAKING_DISTANCE) {
pmarathe25 0:f1fab1a647f6 56 // Reset motor max speeds
pmarathe25 0:f1fab1a647f6 57 setForwardMaxSpeed(1.0f, 1.0f);
pmarathe25 0:f1fab1a647f6 58 led = 1;
pmarathe25 0:f1fab1a647f6 59 } else if (distance > MAX_BRAKING_DISTANCE * 0.5){
pmarathe25 0:f1fab1a647f6 60 setForwardMaxSpeed(0.5f, 0.5f);
pmarathe25 0:f1fab1a647f6 61 led = 0.5;
pmarathe25 0:f1fab1a647f6 62 } else {
pmarathe25 0:f1fab1a647f6 63 setForwardMaxSpeed(0.0f, 0.0f);
pmarathe25 0:f1fab1a647f6 64 led = 0;
pmarathe25 0:f1fab1a647f6 65 }
pmarathe25 0:f1fab1a647f6 66 updateMotors();
pmarathe25 0:f1fab1a647f6 67 // Unlock
pmarathe25 0:f1fab1a647f6 68 motorSpeeds.unlock();
pmarathe25 0:f1fab1a647f6 69 }
pmarathe25 0:f1fab1a647f6 70
pmarathe25 0:f1fab1a647f6 71 ultrasonic mu(p6, p7, .1, 1, &onDistanceChanged);
pmarathe25 0:f1fab1a647f6 72
pmarathe25 0:f1fab1a647f6 73 void distanceThread() {
pmarathe25 0:f1fab1a647f6 74 mu.startUpdates(); // start measuring the distance
pmarathe25 0:f1fab1a647f6 75 while (true) {
pmarathe25 0:f1fab1a647f6 76 // Update sonar
pmarathe25 0:f1fab1a647f6 77 mu.checkDistance();
pmarathe25 0:f1fab1a647f6 78 }
pmarathe25 0:f1fab1a647f6 79 }
pmarathe25 0:f1fab1a647f6 80
pmarathe25 0:f1fab1a647f6 81 void headlightThread() {
pmarathe25 0:f1fab1a647f6 82 while (true) {
pmarathe25 0:f1fab1a647f6 83 // Automatic Headlights
pmarathe25 0:f1fab1a647f6 84 headlights = 0.8f - photocell;
pmarathe25 0:f1fab1a647f6 85 }
pmarathe25 0:f1fab1a647f6 86 }
pmarathe25 0:f1fab1a647f6 87
pmarathe25 0:f1fab1a647f6 88 int main() {
pmarathe25 0:f1fab1a647f6 89 distance.start(distanceThread);
pmarathe25 0:f1fab1a647f6 90 headlightT.start(headlightThread);
pmarathe25 0:f1fab1a647f6 91 // Read data
pmarathe25 0:f1fab1a647f6 92 char bnum = 0;
pmarathe25 0:f1fab1a647f6 93 while (true) {
pmarathe25 0:f1fab1a647f6 94 // Read controller input
pmarathe25 0:f1fab1a647f6 95 if (blue.getc()=='!') {
pmarathe25 0:f1fab1a647f6 96 pc.printf("Seeing bluetooth input");
pmarathe25 0:f1fab1a647f6 97 if (blue.getc()=='B') { //button data
pmarathe25 0:f1fab1a647f6 98 bnum = blue.getc(); //button number
pmarathe25 0:f1fab1a647f6 99 // Locks motor speeds
pmarathe25 0:f1fab1a647f6 100 motorSpeeds.lock();
pmarathe25 0:f1fab1a647f6 101 led3 = 0.0f;
pmarathe25 0:f1fab1a647f6 102 setMotorSpeeds(0.0f, 0.0f);
pmarathe25 0:f1fab1a647f6 103 if (bnum == '5') {
pmarathe25 0:f1fab1a647f6 104 // Up arrow, go forward
pmarathe25 0:f1fab1a647f6 105 setMotorSpeeds(1.0, 1.0);
pmarathe25 0:f1fab1a647f6 106 pc.printf("Forward Arrow");
pmarathe25 0:f1fab1a647f6 107 led3 = 1.0f;
pmarathe25 0:f1fab1a647f6 108 }
pmarathe25 0:f1fab1a647f6 109 if (bnum == '6') {
pmarathe25 0:f1fab1a647f6 110 // Down arrow, go backwards
pmarathe25 0:f1fab1a647f6 111 setMotorSpeeds(-1.0, -1.0);
pmarathe25 0:f1fab1a647f6 112 led3 = 1.0f;
pmarathe25 0:f1fab1a647f6 113 }
pmarathe25 0:f1fab1a647f6 114 if (bnum == '7') {
pmarathe25 0:f1fab1a647f6 115 // Left arrow, turn left
pmarathe25 0:f1fab1a647f6 116 setMotorSpeeds(-1.0, 1.0);
pmarathe25 0:f1fab1a647f6 117 led3 = 1.0f;
pmarathe25 0:f1fab1a647f6 118 }
pmarathe25 0:f1fab1a647f6 119 if (bnum == '8') {
pmarathe25 0:f1fab1a647f6 120 // Right arrow, turn right
pmarathe25 0:f1fab1a647f6 121 setMotorSpeeds(1.0, -1.0);
pmarathe25 0:f1fab1a647f6 122 led3 = 1.0f;
pmarathe25 0:f1fab1a647f6 123 }
pmarathe25 0:f1fab1a647f6 124 // Unlock
pmarathe25 0:f1fab1a647f6 125 motorSpeeds.unlock();
pmarathe25 0:f1fab1a647f6 126 }
pmarathe25 0:f1fab1a647f6 127 }
pmarathe25 0:f1fab1a647f6 128 // Locks speeds before pushing to motors
pmarathe25 0:f1fab1a647f6 129 motorSpeeds.lock();
pmarathe25 0:f1fab1a647f6 130 updateMotors();
pmarathe25 0:f1fab1a647f6 131 motorSpeeds.unlock();
pmarathe25 0:f1fab1a647f6 132 }
pmarathe25 0:f1fab1a647f6 133
pmarathe25 0:f1fab1a647f6 134 }