Diff: main.cpp

Revision:

0:394261695392

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 18 07:42:11 2016 +0000
@@ -0,0 +1,328 @@
+//ADJUST VARIABLES SECTION
+
+const int preRunLength = 1600;
+const int postRunLength = 0;
+const int turnLength = 850;
+const float leftAngleTurnThreshhold = -.045;
+const float rightAngleTurnThreshhold = -.06;
+const float baseSpeed = 0.25;
+
+#include "PinDefinitions.h"
+
+//PROTOTYPES
+void motorCorrections();
+float getLSreading();
+float getLAreading();
+float getRAreading();
+float getRSreading();
+float pidCorrection(int error);
+float proportion(int error);
+int derivative(int error);
+float integral(int error);
+void setRightPwm(float speed);
+void setLeftPwm(float speed);
+void postTurnRun();
+void preTurnRun();
+
+Serial pc(PA_11, PA_12); //SERIAL TX/RX
+DigitalOut myled(LED1);
+
+Ticker PIDcorrectionTicker;
+Ticker gyroOffsetCalc;
+
+float baselineRS;
+float baselineLS;
+float baselineLA;
+float baselineRA;
+
+
+bool safeToRunSystick;
+int rightPulses;
+int leftPulses;
+float currentAngle = 0;
+float refreshTimeSeconds = 0.05; //How fast the systicker is going
+const int flashTime = 20; //The amount of time sensors are on before we take measurment (millisecond)
+
+int prevError;
+int integrator = 0;
+int decayFactor = 1.1;
+float Kp = 0.001;
+//Kd =
+float Ki = 0.002;
+
+//FOR THE SENSORS RUN WHEN DARK TO FIND AMBIENT LIGHT
+//HOW LONG DO I NEED TO TURN ON SENSORS TO GET A READING? 1MS? NO TIME AT ALL?
+
+
+
+void motorCorrections()  //Three things, I should probably break into three functions!!
+{
+    if(safeToRunSystick) {
+        int rightPulses = RENC.getPulses();
+        int leftPulses = LENC.getPulses();
+        //we want the difference which is the error
+        int errorEncoder = rightPulses - leftPulses;
+        float correction = pidCorrection(errorEncoder);
+        if(correction > 0.5)
+            correction = 0.5;
+        else if(correction < -0.5)
+            correction = -0.5;
+        setRightPwm(baseSpeed-correction);
+        setLeftPwm(baseSpeed+correction);
+        //pc.printf("Error:%.6f  \r\i", correction);
+        //pc.printf("Gyro Raw: %.6f  \r\n", currentAngle);
+        RENC.reset();
+        LENC.reset();
+        myled = !myled;
+    }
+}
+
+
+float getLSreading()
+{
+    int ambient = IRSensorLS.read(); //Ambient Reading
+    IRledLS.write(1);
+    wait_ms(flashTime); //Maybe we can decrease this amount
+    float sensorReading = IRSensorLS.read(); //Will get the reading
+    IRledLS.write(0);
+    return sensorReading - ambient; //Ideally we will get pure IR reading
+
+}
+
+float getLAreading()
+{
+    int ambient = IRSensorLA.read(); //Ambient Reading
+    IRledLA.write(1);
+    wait_ms(flashTime); //Maybe we can decrease this amount
+    float sensorReading = IRSensorLA.read(); //Will get the reading
+    IRledLA.write(0);
+    return sensorReading - ambient; //Ideally we will get pure IR reading
+}
+
+float getRAreading()
+{
+    int ambient = IRSensorRA.read(); //Ambient Reading
+    IRledRA.write(1);
+    wait_ms(flashTime); //Maybe we can decrease this amount
+    float sensorReading = IRSensorRA.read(); //Will get the reading
+    IRledRA.write(0);
+    return sensorReading - ambient; //Ideally we will get pure IR reading
+}
+
+float getRSreading()
+{
+    int ambient = IRSensorRS.read(); //Ambient Reading
+    IRledRS.write(1);
+    wait_ms(flashTime); //Maybe we can decrease this amount
+    float sensorReading = IRSensorRS.read(); //Will get the reading
+    IRledRS.write(0);
+    return sensorReading - ambient; //Ideally we will get pure IR reading
+}
+//The correction will be given in difference that power should be writen to the motors. We will chose baseline speed for both
+float pidCorrection(int error)
+{
+    return proportion(error) + integral(error);
+    // +derivative(error)
+}
+
+float proportion(int error)
+{
+    return Kp*error;
+}
+/*
+int derivative(int error){
+  int dError = error - prevError;
+  int dt = timer.read_us();
+  timer.reset();
+  prevError = error;
+  int correctionD = -(Kd*dError)/dt;
+  return correctionD;
+}
+*/
+float integral(int error)
+{
+    integrator +=error;
+    integrator /= decayFactor;
+    return Ki*integrator;
+}
+
+
+void setRightPwm(float speed)
+{
+    if (speed == 0) {
+        RMOTORA = 1.0;
+        RMOTORB = 1.0;
+    }
+
+    if (speed > 0) {
+        RMOTORA = speed;
+        RMOTORB = 0;
+    } else {
+        RMOTORA = 0;
+        RMOTORB = -speed;
+    }
+}
+
+void setLeftPwm(float speed)
+{
+    if (speed == 0) {
+        LMOTORA = 1.0;
+        LMOTORB = 1.0;
+    }
+
+    if (speed > 0) {
+        LMOTORA = speed;
+        LMOTORB = 0;
+    } else {
+        LMOTORA = 0;
+        LMOTORB = -speed;
+    }
+}
+
+void setBaselines()
+{
+    baselineLS = getLSreading();
+    baselineRS = getRSreading();
+    baselineLA = getLAreading();
+    baselineRA = getRAreading();
+}
+
+
+void turnAround()
+{
+    safeToRunSystick = false;   
+    RENC.reset();
+    LENC.reset();
+    setRightPwm(-.1);
+    setLeftPwm(.1);
+    myled = !myled;
+    while(RENC.getPulses() < 1700 && LENC.getPulses() < 1700) {
+    }
+    pc.printf("WE MADE IT OUT  \r\n");
+    myled = 0;
+    setRightPwm(0);
+    setLeftPwm(0);
+    wait(0.2);
+    RENC.reset();
+    LENC.reset();
+    safeToRunSystick = true;
+
+}
+
+void turnLeft()
+{
+    safeToRunSystick = false;
+    preTurnRun();
+    //Now should be in position to turn
+    setRightPwm(.1);
+    setLeftPwm(-.1);
+    RENC.reset();
+    LENC.reset();
+    myled = !myled;
+    while(RENC.getPulses() < turnLength && LENC.getPulses() < turnLength) {}
+    setRightPwm(0);
+    setLeftPwm(0);
+    wait(0.2);
+    safeToRunSystick = true;
+    //At this point the car should be pivoted
+    postTurnRun();
+}
+
+void turnRight()
+{
+    safeToRunSystick = false;
+    preTurnRun();
+    //Now should be in position to turn
+    setRightPwm(-.1);
+    setLeftPwm(+.1);
+    RENC.reset();
+    LENC.reset();
+    myled = !myled;
+    while(RENC.getPulses() < turnLength && LENC.getPulses() < turnLength) {}
+    setRightPwm(0);
+    setLeftPwm(0);
+    wait(0.2);
+    postTurnRun();
+    safeToRunSystick = true;
+    //At this point the car should be pivoted
+}
+
+
+
+void preTurnRun(){  //this code runs before and after every turn to ensure that the car is positioned at the right spot when it turns
+        RENC.reset();
+        LENC.reset();
+       int rightPulses = 0;
+       int leftPulses = 0;
+        //we want the difference which is the error
+        while(RENC.getPulses() < preRunLength && LENC.getPulses() < preRunLength) {
+        rightPulses = RENC.getPulses() - rightPulses;
+        leftPulses = LENC.getPulses() - leftPulses;
+        int errorEncoder = rightPulses - leftPulses;
+        float correction = proportion(errorEncoder);
+        if(correction > 0.5)
+            correction = 0.5;
+        else if(correction < -0.5)
+            correction = -0.5;
+        setRightPwm(baseSpeed-correction);
+        setLeftPwm(baseSpeed+correction);
+        }
+}
+
+void postTurnRun(){  //this code runs before and after every turn to ensure that the car is positioned at the right spot when it turns
+        RENC.reset();
+        LENC.reset();
+       int rightPulses = 0;
+       int leftPulses = 0;
+        //we want the difference which is the error
+        while(RENC.getPulses() < postRunLength && LENC.getPulses() < postRunLength) {
+        rightPulses = RENC.getPulses() - rightPulses;
+        leftPulses = LENC.getPulses() - leftPulses;
+        int errorEncoder = rightPulses - leftPulses;
+        float correction = proportion(errorEncoder);
+        if(correction > 0.5)
+            correction = 0.5;
+        else if(correction < -0.5)
+            correction = -0.5;
+        setRightPwm(baseSpeed-correction);
+        setLeftPwm(baseSpeed+correction);
+        }
+}
+
+int main()
+{
+    wait(0.2);
+    myled =1;
+    wait(0.5);
+    myled =0;
+    wait(0.2);
+    setBaselines();
+    safeToRunSystick = true;
+    PIDcorrectionTicker.attach(&motorCorrections, refreshTimeSeconds);
+    while(1) {
+        //Turns if theres a wall infront of it
+        myled = !myled;
+        float leftSSensor = getLSreading();
+        float rightSSensor = getRSreading();
+        if((leftSSensor > 0.1 + baselineLS) || (rightSSensor > 0.1 + baselineRS)) {
+       //     turnAround();
+        }
+        float leftASensor = getLAreading();
+        float rightASensor = getRAreading();
+        
+        float leftAStrig = leftASensor- baselineLA;
+        float rightAStrig = rightASensor-baselineRA;
+       
+        if(rightAStrig < rightAngleTurnThreshhold)
+            turnRight();
+        else if(leftAStrig < leftAngleTurnThreshhold)
+            turnLeft();
+        
+      
+        //pc.printf("Left angle: %.6f  \r\n", leftASensor);
+       // pc.printf("Right angle: %.6f  Left angle: %.6f \r\n", rightAStrig, leftAStrig);
+       // pc.printf("Gyro accum: %.6f  \r\n", accumGyro);
+      
+        
+    }
+}