main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "Motor.h"
00004 #include "ultrasonic.h"
00005 
00006 Thread distance, headlightT;
00007 Mutex motorSpeeds;
00008 
00009 // DEBUG
00010 Serial pc(USBTX, USBRX);
00011 PwmOut led(LED1);
00012 PwmOut led3(LED2);
00013 
00014 PwmOut headlights(p24);
00015 
00016 // Light sensor
00017 AnalogIn photocell(p15);
00018 
00019 // Bluetooth
00020 Serial blue(p28, p27);
00021 // Motors
00022 Motor leftMotor(p22, p8, p9); // pwm, fwd, rev
00023 Motor rightMotor(p21, p10, p11); // pwm, fwd, rev
00024  
00025 // Motor controls
00026 float leftMotorForwardMax = 1.0f, rightMotorForwardMax = 1.0f, 
00027     leftMotorBackwardMax = 1.0f, rightMotorBackwardMax = 1.0f,
00028     leftMotorCurrentSpeed = 0.0f, rightMotorCurrentSpeed = 0.0f,
00029     leftCompensationScale = 0.85f, rightCompensationScale = 1.0f;
00030 const float MAX_BRAKING_DISTANCE = 800.0f;
00031 
00032 void setMotorSpeeds(float leftScale = 1.0, float rightScale = 1.0) {
00033     leftScale *= leftCompensationScale;
00034     rightScale *= rightCompensationScale;
00035     leftMotorCurrentSpeed = (leftScale > 0.0) ? leftScale * leftMotorForwardMax : leftScale * leftMotorBackwardMax;
00036     rightMotorCurrentSpeed = (rightScale > 0.0) ? rightScale * rightMotorForwardMax : rightScale * rightMotorBackwardMax;
00037     //pc.printf("Setting left motor to %d\n", leftMotorCurrentSpeed);
00038 }
00039 
00040 void updateMotors() {
00041     // Set motors
00042     leftMotor.speed((leftMotorCurrentSpeed > leftMotorForwardMax) ? leftMotorForwardMax : leftMotorCurrentSpeed);
00043     rightMotor.speed((rightMotorCurrentSpeed > rightMotorForwardMax) ? rightMotorForwardMax : rightMotorCurrentSpeed);
00044     //pc.printf("Pushing %d to left motor\n", leftMotorCurrentSpeed);
00045 }
00046 
00047 void setForwardMaxSpeed(float left = 1.0, float right = 1.0) {
00048     leftMotorForwardMax = left;
00049     rightMotorForwardMax = right;
00050 }  
00051 
00052 void onDistanceChanged(int distance) {
00053     // Lock speeds before updating.
00054     motorSpeeds.lock();
00055     if (distance > MAX_BRAKING_DISTANCE) {
00056         // Reset motor max speeds
00057         setForwardMaxSpeed(1.0f, 1.0f);
00058         led = 1;
00059     } else if (distance > MAX_BRAKING_DISTANCE * 0.5){
00060         setForwardMaxSpeed(0.5f, 0.5f);
00061         led = 0.5;
00062     } else {
00063         setForwardMaxSpeed(0.0f, 0.0f);
00064         led = 0;
00065     }
00066     updateMotors();  
00067     // Unlock
00068     motorSpeeds.unlock();
00069 }
00070 
00071 ultrasonic mu(p6, p7, .1, 1, &onDistanceChanged);
00072 
00073 void distanceThread() {
00074     mu.startUpdates(); // start measuring the distance
00075     while (true) {
00076         // Update sonar
00077         mu.checkDistance(); 
00078     }
00079 }
00080 
00081 void headlightThread() {
00082     while (true) {
00083         // Automatic Headlights
00084         headlights = 0.8f - photocell;
00085     }
00086 }
00087 
00088 int main() {
00089     distance.start(distanceThread);
00090     headlightT.start(headlightThread);
00091      // Read data
00092     char bnum = 0;
00093     while (true) {
00094          // Read controller input
00095         if (blue.getc()=='!') {
00096             pc.printf("Seeing bluetooth input");
00097             if (blue.getc()=='B') { //button data
00098                 bnum = blue.getc(); //button number
00099                 // Locks motor speeds
00100                 motorSpeeds.lock();
00101                 led3 = 0.0f;
00102                 setMotorSpeeds(0.0f, 0.0f);
00103                 if (bnum == '5') {
00104                     // Up arrow, go forward
00105                     setMotorSpeeds(1.0, 1.0);
00106                     pc.printf("Forward Arrow");
00107                     led3 = 1.0f;
00108                 } 
00109                 if (bnum == '6') {
00110                     // Down arrow, go backwards
00111                     setMotorSpeeds(-1.0, -1.0);
00112                     led3 = 1.0f;
00113                 } 
00114                 if (bnum == '7') {
00115                     // Left arrow, turn left
00116                     setMotorSpeeds(-1.0, 1.0);
00117                     led3 = 1.0f;
00118                 } 
00119                 if (bnum == '8') {
00120                     // Right arrow, turn right
00121                     setMotorSpeeds(1.0, -1.0);
00122                     led3 = 1.0f;
00123                 }
00124                 // Unlock
00125                 motorSpeeds.unlock();
00126             } 
00127         }
00128         // Locks speeds before pushing to motors
00129         motorSpeeds.lock();
00130         updateMotors();
00131         motorSpeeds.unlock();
00132     }
00133 
00134 }