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;

1. 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);

} }