Tinkerbot Program 1

Tinkerbot Program 1

#include <NewPing.h>
#include <Servo.h> 

#define ROTATION_SPEED 150
#define DEFAULT_SPEED 150

// Motor pins
const int MOTOR_RIGHT_IN1 = 2;
const int MOTOR_RIGHT_IN2 = 3;
const int MOTOR_LEFT_IN3 = 4;
const int MOTOR_LEFT_IN4 = 5;

// Ultrasonic sensor pins
const int ULTRASONIC_TRIGGER = 6;
const int ULTRASONIC_ECHO = 7;
#define MAX_DISTANCE 100

NewPing sonar(ULTRASONIC_TRIGGER, ULTRASONIC_ECHO, MAX_DISTANCE);

// Photointerrupter pins
const int PHOTOINTERRUPTER_LEFT = 8;
const int PHOTOINTERRUPTER_RIGHT = 9;

// Servo
const int SERVO_PIN = 10;
Servo distanceServo; 

// Limit switches
const int LIMIT_SWITCH_LEFT = 11;
const int LIMIT_SWITCH_RIGHT = 12;

// Free pins
// pin 13. It's already connected to an on-board LED
// Pin 0 and 1 is also free but preferably left unused

void setup() {
  pinMode(MOTOR_RIGHT_IN1, OUTPUT);
  pinMode(MOTOR_RIGHT_IN2, OUTPUT);
  pinMode(MOTOR_LEFT_IN3, OUTPUT);
  pinMode(MOTOR_LEFT_IN4, OUTPUT);
  
  pinMode(ULTRASONIC_TRIGGER, OUTPUT);
  pinMode(ULTRASONIC_ECHO, INPUT);
  
  pinMode(PHOTOINTERRUPTER_LEFT, INPUT);
  pinMode(PHOTOINTERRUPTER_RIGHT, INPUT);

  pinMode(SERVO_PIN, OUTPUT);
  
  pinMode(LIMIT_SWITCH_LEFT, INPUT);
  digitalWrite(LIMIT_SWITCH_LEFT, HIGH);
  pinMode(LIMIT_SWITCH_RIGHT, INPUT);
  digitalWrite(LIMIT_SWITCH_RIGHT,HIGH);

  pinMode(13, OUTPUT);
  distanceServo.attach(SERVO_PIN);
  Serial.begin(9600);
}

void loop() {

  int left_bumper = digitalRead(LIMIT_SWITCH_LEFT);
  int right_bumper = digitalRead(LIMIT_SWITCH_RIGHT);
  
  if (left_bumper) {
    robotForward();
    delay(1000);
    robotRotateLeft();
    delay(100);
    robotRotateRight();
    delay(300);
    robotBack();
    delay(2000);
    robotStop();
  }
  
  if (right_bumper) {
    sweepForBestAngle();
  }
}

/**************************************************/
// Don't change the code below unless 
// you know what you're doing :)
/**************************************************/


int sweepForBestAngle() {
  int pos;
  int bestDistance = 0;
  int bestAngle = 0;
  
  // Move to starting position
  distanceServo.write(0);              
  delay(240);                        

  for(pos = 0; pos <= 180; pos += 30) {  
    distanceServo.write(pos);              
    delay(300);
    int distance = sonar.ping_cm();
    if (distance == 0) {
      distance = MAX_DISTANCE;
    }
    if (distance > bestDistance) {
      bestAngle = pos;
      bestDistance = distance;  
    }
  } 
  
  distanceServo.write(bestAngle);
  delay(240);
}

void robotRotateLeft() {
  digitalWrite(MOTOR_RIGHT_IN1, HIGH);
  analogWrite(MOTOR_RIGHT_IN2, 255 - ROTATION_SPEED);
  digitalWrite(MOTOR_LEFT_IN3, LOW);
  analogWrite(MOTOR_LEFT_IN4, ROTATION_SPEED);
}

void robotRotateLeft(int angle) {
  if (angle < 0) {
    robotRotateRight();
    angle = -angle;
  } else {
    robotRotateLeft();
  }
  delay(angle*4);
  robotStop();
}

void robotRotateRight() {
  digitalWrite(MOTOR_RIGHT_IN1, LOW);
  analogWrite(MOTOR_RIGHT_IN2, ROTATION_SPEED);
  digitalWrite(MOTOR_LEFT_IN3, HIGH);
  analogWrite(MOTOR_LEFT_IN4, 255 - ROTATION_SPEED);
}

void robotRotateRight(int angle) {
  if (angle < 0) {
    robotRotateRight();
    angle = -angle;
  } else {
    robotRotateLeft();
  }
  delay(angle*4);
  robotStop();
}

void robotForward() {
  robotForward(DEFAULT_SPEED);
}

void robotBack() {
  robotForward(-DEFAULT_SPEED);
}

void robotForward(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed == 0) {
    robotStop();
  } else if (speed > 0) {
    // Move forward
    digitalWrite(MOTOR_RIGHT_IN1, HIGH);
    analogWrite(MOTOR_RIGHT_IN2, 255 - speed);
    digitalWrite(MOTOR_LEFT_IN3, HIGH);
    analogWrite(MOTOR_LEFT_IN4, 255 - speed);    
  } else {
    // Move backwards
    digitalWrite(MOTOR_RIGHT_IN1, LOW);
    analogWrite(MOTOR_RIGHT_IN2, -speed);
    digitalWrite(MOTOR_LEFT_IN3, LOW);
    analogWrite(MOTOR_LEFT_IN4, -speed);
  }
}

void robotForward(long duration, int speed) {
  robotForward(speed);
  delay(duration);
  robotStop();
}

void robotStop() {
  // Stop
  digitalWrite(MOTOR_RIGHT_IN1, LOW);
  analogWrite(MOTOR_RIGHT_IN2, LOW);
  digitalWrite(MOTOR_LEFT_IN3, LOW);
  analogWrite(MOTOR_LEFT_IN4, LOW);
}