Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
#define DEBUG
const int speedPin_M1 = 5; //M1 Speed Control
const int speedPin_M2 = 6; //M2 Speed Control
const int directionPin_M1 = 4; //M1 Direction Control
const int directionPin_M2 = 7; //M1 Direction Control
const int LDR0 = A0; // LDR sensor to GND and 1k to 1k resistor
const int LDR1 = A1; // LDR sensor to GND and 1k to 1k resistor
const int slow = 30;
const int fast = 120;
const int dark = 800;
const int bright = 150;
const int hyst = 50;
const int filterVal = 0.9;
int initialLight;
int smoothLight0, smoothLight1;
void setup() {
#ifdef DEBUG
Serial.begin(9600);
#endif
initialLight = (analogRead(LDR0) + analogRead(LDR1) ) / 2;
}
void loop() {
int light0 = analogRead(LDR0);
int light1 = analogRead(LDR1);
smoothLight0 = smooth(light0, filterVal, smoothLight0); // apply smoothing lowpass filter
smoothLight1 = smooth(light1, filterVal, smoothLight1);
#ifdef DEBUG
Serial.print( light0 );
Serial.print( " " );
Serial.println( light1 );
#endif
if ( (smoothLight0 > (initialLight + hyst)) && (smoothLight1 > (initialLight + hyst))) {
carBack( fast, fast );
}
else if ( (light0 < ( initialLight - hyst)) || ( (light1 < ( initialLight - hyst)) ) ) {
int speedMotor0 = map(smoothLight0, dark, bright, slow, fast);
int speedMotor1 = map(smoothLight1, dark, bright, slow, fast);
carAdvance( speedMotor0, speedMotor1);
}
else {
carStop();
}
delay(10);
}
void carStop() { // Motor Stop
digitalWrite(speedPin_M2, 0);
digitalWrite(directionPin_M1, LOW);
digitalWrite(speedPin_M1, 0);
digitalWrite(directionPin_M2, LOW);
}
void carBack(int leftSpeed, int rightSpeed) { //Move backward
analogWrite (speedPin_M2, leftSpeed); //PWM Speed Control
digitalWrite(directionPin_M1, HIGH);
analogWrite (speedPin_M1, rightSpeed);
digitalWrite(directionPin_M2, HIGH);
}
void carAdvance(int leftSpeed, int rightSpeed) { //Move forward
analogWrite (speedPin_M2, leftSpeed);
digitalWrite(directionPin_M1, LOW);
analogWrite (speedPin_M1, rightSpeed);
digitalWrite(directionPin_M2, LOW);
}
void carTurnLeft(int leftSpeed, int rightSpeed) { //Turn Left
analogWrite (speedPin_M2, leftSpeed);
digitalWrite(directionPin_M1, LOW);
analogWrite (speedPin_M1, rightSpeed);
digitalWrite(directionPin_M2, HIGH);
}
void carTurnRight(int leftSpeed, int rightSpeed) { //Turn Right
analogWrite (speedPin_M2, leftSpeed);
digitalWrite(directionPin_M1, HIGH);
analogWrite (speedPin_M1, rightSpeed);
digitalWrite(directionPin_M2, LOW);
}
int smooth(int data, float filterVal, float smoothedVal){
if (filterVal > 1){ // check to make sure param's are within range
filterVal = .99;
}
else if (filterVal <= 0){
filterVal = 0;
}
smoothedVal = (data * (1 - filterVal)) + (smoothedVal * filterVal);
return (int)smoothedVal;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment