Wanderer1.ino

#include "RangeFinder.h"
#include "MotorDriver.h"
#include "Timer.h"

#include "Behavior.h"
#include "Motivator.h"
#include "Task.h"

#include "WandererMotivator.h"

//Motor users D8-13
#define TP 6      //Trig_pin
#define EP 7      //Echo_pin

WandererMotivator motivator;

void setup()
{
  Serial.println("*************************************");
  Serial.println("***************Startup***************");
  Serial.println("*************************************");

  //Setup serial
  Serial.begin(9600);      // init serial 9600

  //timer
  timer.init();

  //Configure the motor A to control the wheel at the left side.
  //Configure the motor B to control the wheel at the right side.
  motordriver.init();
  motordriver.setSpeed(200,MOTORA);
  motordriver.setSpeed(200,MOTORB);

  //configure rangefinder
  rangeFinder.init(TP, EP, 1);
}

void loop()
{
  //Serial.println("SL: ");
  timer.update();

  motivator.motivate();
  //Serial.println(":EL");
}


/*
void loop()
{
  //Sensor update
  distance = rangeFinder.getDistance();

  //Update Time
  timer.update();

  //Determine behavior
  activeBehavior = determineBehavior();

  //call behavior
  switch(activeBehavior)
  {
    case B_STOP:
    behaveStop();
    break;
    case B_DRIVE:
    behaveDrive();
    break;
    case B_AVOID:
    behaveAvoid();
    break;
    case B_SCAN:
    behaveScan();
    break;
    default:
    behaveStop();
    break;
  }
}

int determineBehavior()
{
  int result = B_INVALID;
  if (distance < T_DISTANCE)
  {
    result = B_AVOID;
  }
  else
  {
    result = B_DRIVE;
  }
  return result;
}

// --------------- behavior functions ---------------
void behaveStop()
{
  if (activeBehavior != B_STOP)
  {
    elapsedBehavior = 0;
    elapsedTask = 0;
  }
  motordriver.stop();
}

void behaveDrive()
{
  if (activeBehavior != B_DRIVE)
  {
    elapsedBehavior = 0;
    elapsedTask = 0;
  }
  motordriver.goForward();
}

void behaveAvoid()
{
  if (activeBehavior != B_AVOID)
  {
    elapsedBehavior = 0;
    elapsedTask = 0;
  }
  motordriver.goRight();
}

void behaveScan()
{
  int turnDelay = 1000;
  motordriver.goLeft();
  delay(turnDelay);
  motordriver.stop();
  motordriver.goRight();
  delay(turnDelay);
  delay(turnDelay);
  motordriver.goLeft();
  delay(turnDelay);
}

*/