Curtis Sellier / Mbed 2 deprecated Project_Car_Curtis_Non_Tested

Dependencies:   Servo mbed

main.cpp

Committer:
csellier
Date:
2016-10-14
Revision:
8:07464acc8a85
Parent:
7:5da0ff9752d5
Child:
9:a543d0cdfce9

File content as of revision 8:07464acc8a85:

#include "mbed.h"
#include "Servo.h"

#define DISTANCE 0.45


Serial pc (USBTX, USBRX); // USB serial interface

Servo ServoRight(p21); // continuous rotation hobby servo right
Servo ServoLeft(p22); // continuous rotation hobby servo left

PwmOut LEDleft(p23);
PwmOut LEDright(p24);

AnalogIn IRfront(p15);
AnalogIn IRleft(p16);
AnalogIn IRright(p17);
AnalogIn LDRleft(p18);
AnalogIn LDRright(p19);

void init_servo() {
    // calibrate the servos for +/-5ms over +/-45deg
    ServoRight.calibrate(0.0005,45);
    ServoLeft.calibrate(0.0005,45);
}

int main() {
    float front;
    float left;
    float right;
    float LDRleft;
    float LDRright;

     // set the USB serial interface baud rate
     pc.baud(921600);
     init_servo();

 while(1) {
    
    // Auto drive
    front = IRfront.read();
    left  = IRleft.read();
    right = IRright.read();
    LDRvalueright = LDRright.read();
    LDRvalueleft = LDRleft.read()
    
    if (front > DISTANCE) {
        if (left > DISTANCE) {
            ServoRight.write(0.5);   //turn right
            ServoLeft.write(0.4);
        } else {
            ServoRight.write(0.6);   // turn left
            ServoLeft.write(0.5);    
        }
    } else if (left > DISTANCE) {
        if (right < DISTANCE) {
            ServoRight.write(0.5); // right
            ServoLeft.write(0.4);
        } else {
            ServoRight.write(0.6); // forward    
            ServoLeft.write(0.4);
        }
    }
    
    //Turn both LED's on for the line following feature
    LEDleft=1;
    LEDright=1;
    
    // send the servo command to the servos themselves
    ServoRight.write(0.6); // write to the continuous rotation servo1
    ServoLeft.write(0.4); // write to the continous servo2

    }
}