16. November 2011

Unsere Aufgabe bestand darin, mittels Arduino mit Hilfe von einem Servo und einem Sensor einen kleinen Roboter oder ähnliches zu bauen. Wir entschieden und für den Bewegungssensor, welcher mit Infrarot den Input gibt. Den Output wäre dann der Servo, der sich jeweils um 180° drehen kann, und so einer Bewegung folgen kann. Auf dem Video sieht man den kleinen Prototypen, der sich schon zur Kamera hin ausrichtet und dieser mit seinen "Augen" folgt. Die Software des Trackers is so aufgebaut das die beiden Sensoren sich untereinander abwechseln und dann als Haupt bzw. Wächter Sensor fungieren. Haut der Hauptsensor ein Objekt erkannt, und verliert es wider dreht er sich in die Richtung des Sensors. der Wächtersensor der "hinterher" dreht fungiert jetzt als "BackUp" Sensor. Erkennt der Hauptsensor das Objekt wieder, hält der Roboter wieder an. Erkennt jedoch der Wächter Sensor das Objekt, tauschen Sie sich und der Wächter wird zum Hauptsensor.  

// Libraries
#include <Servo.h>

// Pins
#define SERVO 9
#define LEFTSENS 0
#define RIGHTSENS 5
#define LED1 3
#define LED2 5
#define LED3 7

// Settings
#define TARGET_TRESHHOLD 150
#define LED_DURATION 200

// Objects
Servo servo;

// Data Stores
int leftSensNew = 0;
int leftSensOld = 0;
int rightSensNew = 0;
int rightSensOld = 0;
int servoPosition = 0;

// Stores
boolean hasTargetNew = false;
boolean hasTargetOld = false;

// Majorness
boolean leftIsMajor = true;
boolean rightIsMajor = false;

// LED
int ledActive = 1;
double ledLastChange = 0;

void setup()
{
  // Setup Serial
  Serial.begin(9600);
  Serial.println("START TRACKING");

  // Setup Servo
  servo.attach(SERVO);

  // Setep LED's
  pinMode(LED1,OUTPUT);
  pinMode(LED2,OUTPUT);
  pinMode(LED3,OUTPUT);

  // Test LEDS
  digitalWrite(LED1,HIGH);
  digitalWrite(LED2,HIGH);
  digitalWrite(LED3,HIGH);

  // Turn to Start
  servo.write(servoPosition);
  delay(1000);

  // Turn OFF LED's
  digitalWrite(LED1,LOW);
  digitalWrite(LED2,LOW);
  digitalWrite(LED3,LOW);
}

void loop()
{
  // Get Data
  leftSensNew = analogRead(LEFTSENS);
  rightSensNew = analogRead(RIGHTSENS);

  // Smoth Input
  leftSensNew = (0.98*leftSensOld)+(0.02*leftSensNew);
  rightSensNew = (0.98*rightSensOld)+(0.02*rightSensNew);

  // Debug
  Serial.print("L: ");
  Serial.print(leftSensNew);
  Serial.print(" - R: ");
  Serial.print(rightSensNew);

  // Set Old
  leftSensOld = leftSensNew;
  rightSensOld = rightSensNew;

  // Check for Target
  boolean leftHasTarget = (leftSensNew > TARGET_TRESHHOLD);
  boolean rightHasTarget = (rightSensNew > TARGET_TRESHHOLD);



  // Set Major Sensor
  if(leftHasTarget && !rightHasTarget) {
    leftIsMajor = true;
    rightIsMajor = false;
  } else if(!leftHasTarget && rightHasTarget) {
    leftIsMajor = false;
    rightIsMajor = true;
  }

  // Set Target
  hasTargetNew = (leftHasTarget || rightHasTarget);

  // Debug
  if(hasTargetNew && !hasTargetOld) {
    Serial.print(" - Object Found!");
  }

  if(!hasTargetNew && hasTargetOld) {
    Serial.print(" - Object Lost!");
  }

  // Search
  if(!hasTargetNew) {
    search();
  } else {
    animateLED();
  }

  // Save Data
  hasTargetOld = hasTargetNew;

  // End Debug
  Serial.println();

}

void search()
{
  // Check for Majorness
  if(leftIsMajor) {
    servoPosition += 2;
  } else if(rightIsMajor) {
    servoPosition -= 2;
  }

  // Check for End
  if(leftIsMajor && servoPosition >= 179) {
    rightIsMajor = true;
    leftIsMajor = false;
  }

  // Check for End
  if(rightIsMajor && servoPosition == 0) {
    rightIsMajor = false;
    leftIsMajor = true;
  }

  // Turn Servo
  servo.write(servoPosition);
}

void animateLED()
{
  // Check for Change
  if(millis() - ledLastChange > LED_DURATION) {

    // Increment
    ledActive++;

    // Check for last
    if(ledActive > 2) {
      ledActive = 0;
    }

    ledLastChange = millis();

    // All to LOW
    digitalWrite(LED1,LOW);
    digitalWrite(LED2,LOW);
    digitalWrite(LED3,LOW);

    // Set Current Active
    switch(ledActive)
    {
      case 0: digitalWrite(LED1,HIGH); break;
      case 1: digitalWrite(LED2,HIGH); break;
      case 2: digitalWrite(LED3,HIGH); break;
    }
  }

}