#include <platform.h>

#define BIT_RATE 38400  //Baud = 38.4k
#define BIT_TIME XS1_TIMER_HZ / BIT_RATE

//BitBash to Pin D37 in Proto Area
on stdcore[0] : out port TXD = XS1_PORT_1N;  //out port TXD = PORT_UART_TX; 

void txByte(out port TXD, int byte);

int main(void) 
{
   txByte(TXD, 0x55); //Send the AutoBauding Character
   txByte(TXD, 0x45); //CLS
   txByte(TXD, 0x42); // Set Background to
   txByte(TXD, 0xFF); // White LSB
   txByte(TXD, 0xFF); // White MSB
   return 0;
}

void txByte(out port TXD, int byte) 
{
  unsigned time;
  timer t;

  /* get initial time */
  t :> time;
   
  /* output start bit */
  TXD <: 0;
  time += BIT_TIME;
  t when timerafter(time) :> void;
      
  /* output data bits */
  for (int i=0; i<8; i++) 
  {
    TXD <: >> byte;
    time += BIT_TIME;
    t when timerafter(time) :> void;
  }

  /* output stop bit */
  TXD <: 1;
  time += BIT_TIME;
  t when timerafter(time) :> void;
}

/* Project: IRBot
 * Differential drive robot using continuous rotation hobby servos
 * and Sharp GP2D12 IR sensor for collision avoidance. 
 * Inspired by: Makey Robot, MAKE Magazine, Volume 19, p. 77, created: June 2009 Kris Magri
 * Modified: Oct 11, 2009 'MannyR7' mramirezjr7(AT)yahoo(DOT)com
 */
  #include <Servo.h> 
  
  #define BOUNDARY     10      // Avoid objects closer than ~24"

  Servo leftservo;  // create servo objects to control a servo 
  Servo rightservo;  // 0 = full speed forward, 90 = full stop, 180 = full reverse (ONE SERVO IS REVERSE ROTATION!!!)

  long distance1 = 0;   // First distance reading from rangefinder.
  long distance2 = 0;   // Second distance reading from rangefinder. 
  long distance3 = 0;   // Third distance reading from rangefinder.
  int irReader = 1;    // the analog input pin for the IR reader

void setup()                                                 
{
  leftservo.attach(9);  // attaches the servos on pins 9 and 10 to the servo objects 
  rightservo.attach(10);
}

// Main program
// Roam around while avoiding objects.
// 
// Set motors to move forward,
// Take distance readings over and over, 
// as long as no objects are too close (determined by BOUNDARY).
// If object is too close, avoid it -- back up and turn.
// Repeat.

void loop()                     
{    
  do 
  {
    forward(); // Robot moves forward continuously.    
    distance1 =  analogRead(irReader);// Take a distance reading from the IR sensor
    delay(30);
    distance2 =  analogRead(irReader);// Take a distance reading from the IR sensor
    delay(30);   
    distance3 = analogRead(irReader);// Take a distance reading from the IR sensor
    delay(30);
    distance1 = ((distance1 + distance2 + distance3)/3);
  }
  while(distance1 < BOUNDARY);      // Loop while no objects close-by.
    // Robot has sensed a nearby object and exited the while loop. Take evasive action to avoid object.     
    backward();    // Move backward 300ms.          
    delay(300);     
    rightTurn();   // Turn right 500ms. ~ 45 degrees  
    delay(500);     
 } // end Main program

void forward()
{ 
    leftservo.write(0);           // tell servo to go to position in variable 'pos'
    rightservo.write(180);        // tell servo to go to position in variable 'pos'
    delay(500);
}  

void backward()
{
    leftservo.write(180);           // tell servo to go to position in variable 'pos'
    rightservo.write(0);           // tell servo to go to position in variable 'pos'           
}

void rightTurn()
{
    leftservo.write(0);           // tell servo to go to position in variable 'pos'
    rightservo.write(0);           // tell servo to go to position in variable 'pos'
}