Light Seeking Robot

Light Seeking Robot_bb.png

light_seeking_robot.ino

/**
 * Light Seeking Robot
 * Date: November 9, 2017
 * Author: Shawn Hymel (SparkFun Electronics)
 * 
 * 2-wheeled robot moves toward the brightest light by first
 * checking left, right, and center.
 * 
 * Parts for this project can be found in the SparkFun
 * Inventor's Kit v4.0: https://www.sparkfun.com/products/14265
 * 
 * This sketch was written by SparkFun Electronics, with lots of
 * help from the Arduino community. This code is completely free
 * for any use.
 */

// Pins
const int PWMB = 8; 
const int BIN2 = 9;
const int BIN1 = 10;
const int AIN1 = 11;
const int AIN2 = 12;
const int PWMA = 13;
const int SW_PIN = 7;     // Switch to turn the motors on and off
const int LIGHT_PIN = A0; // Photocell

// Constants
const int SEARCH_DRIVE_TIME = 200;  // Time to run one motor while searching
const int TURN_DRIVE_TIME = 200;    // Time to turn in a direction
const int MOVE_DRIVE_TIME = 300;   // Time to drive in a direction
const int STOP_DRIVE_TIME = 200;    // Time to stop after moving
const int NUM_LIGHT_LEVELS = 3;

void setup() {

  // Set switch pin 
  pinMode(SW_PIN, INPUT_PULLUP);

  // Set the motor control pins as outputs
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  // Initialize Serial comms
  Serial.begin(9600);
  Serial.println("Feed me photons!"); 
}

void loop() {

  // Store light levels as array [left, center, right]
  int light_levels[NUM_LIGHT_LEVELS];

  // If switch is flipped, search for light
  if(digitalRead(SW_PIN) == LOW){

    // Record light value to the left
    drive(0, 255);
    delay(SEARCH_DRIVE_TIME);
    drive(0, 0);
    delay(STOP_DRIVE_TIME);
    light_levels[0] = analogRead(LIGHT_PIN);
    drive(0, -255);
    delay(SEARCH_DRIVE_TIME);
    drive(0, 0);
    delay(STOP_DRIVE_TIME);

    // Record light value to the right
    drive(255, 0);
    delay(SEARCH_DRIVE_TIME);
    drive(0, 0);
    delay(STOP_DRIVE_TIME);
    light_levels[2] = analogRead(LIGHT_PIN);
    drive(-255, 0);
    delay(SEARCH_DRIVE_TIME);
    drive(0, 0);
    delay(STOP_DRIVE_TIME);

    // Record light value in the center
    light_levels[1] = analogRead(LIGHT_PIN);

    // Find direction of max light
    int max_light = 0;
    int max_light_index = 0;
    for ( int i = 0; i < NUM_LIGHT_LEVELS; i++ ) {
      if ( light_levels[i] > max_light ) {
        max_light = light_levels[i];
        max_light_index = i;
      }
      Serial.print(light_levels[i]);
      Serial.print(" ");
    }
    Serial.println();
    Serial.print("Max light: ");
    Serial.println(max_light_index);

    // Move in the direction of max light
    if ( max_light_index == 0 ) {
      Serial.println("Chasing light to the left");
      drive(-100, 255);
      delay(TURN_DRIVE_TIME);
      drive(255, 255);
      delay(MOVE_DRIVE_TIME);
      drive(0, 0);
      delay(STOP_DRIVE_TIME);
    } else if ( max_light_index == 1 ) {
      Serial.println("Chasing light straight ahead");
      drive(255, 255);
      delay(MOVE_DRIVE_TIME);
      drive(0, 0);
      delay(STOP_DRIVE_TIME);
    } else {
      Serial.println("Chasing light to the right");
      drive(255, -100);
      delay(TURN_DRIVE_TIME);
      drive(255, 255);
      delay(MOVE_DRIVE_TIME);
      drive(0, 0);
      delay(STOP_DRIVE_TIME);
    }

  // If switch is not flipped, do nothing
  } else {
    drive(0, 0);
  }
}

void rightMotor(int motorSpeed)                       
{

  // If speed is positive, run the motor forward
  if (motorSpeed > 0) {
    digitalWrite(AIN1, HIGH); 
    digitalWrite(AIN2, LOW);

  // If it's negative, run the motor backward
  } else if (motorSpeed < 0) {
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);

  // If it's 0, brake the motor
  } else {
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, LOW);
  }
  analogWrite(PWMA, abs(motorSpeed));
}

void leftMotor(int motorSpeed)
{
  // If speed is positive, run the motor forward
  if (motorSpeed > 0) {
    digitalWrite(BIN1, HIGH); 
    digitalWrite(BIN2, LOW);

  // If it's negative, run the motor backward
  } else if (motorSpeed < 0) {
    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, HIGH);

  // If it's 0, brake the motor
  } else {
    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, LOW);
  }
  analogWrite(PWMB, abs(motorSpeed));
}

void drive(int leftSpeed, int rightSpeed) {
  leftMotor(leftSpeed);
  rightMotor(rightSpeed);
}