/* Final code to run robot, written 4am-6am 12-Mar-03
*    (compiled from previous versions, test code, and
*     strategy originally implemented
*  Comments added 8pm 12-Mar by Jon Daniels
*/

#include <stdio.h>
#include "timer.h"
#include "ses.h"
#include "pwm.h"
#include "pls.h"
#include "ad_lib.h"
#include "shiftreg.h"

/*---------------- Calibration Parms ------------------------*/
// for A/D readings: 0V is 0, 5V is 255
// how bright sensor has to be to be considered "on"
#define BRIGHT_SENSOR_THRESHOLD 50

// how low all sensors have to be for us to determine that we're not
//  facing beacon with any sensor
#define LOW_SENSOR_THRESHOLD    30

// sometimes we'd get odd readings, so since we've calibrated sensors
//   to cap out at about 90-100 we can ignore really high values
#define SPURIOUS_SENSOR 200

// time in ms for turret pulses (half-period of waveform)
#define TURRET_CLICK_PERIOD  50

// time motors drive in reverse to brake
#define STOPPING_TIME   500

// how long to run the catapult
#define FIRING_TIME     500

// how long to let ball settle after loading
#define SETTLING_TIME    500


// sensor codes (arbitrary)
#define FAR_RIGHT_SENSOR 4
#define RIGHT_SENSOR     3
#define CENTER_SENSOR    2
#define LEFT_SENSOR      1
#define FAR_LEFT_SENSOR  0
#define NO_SENSOR        10
#define RIGHT_FINE_SENSOR 6
#define LEFT_FINE_SENSOR 7
#define ERROR_SENSOR    14

// these are actually the pins on port G these are written on
#define TURRET1 0x01
#define TURRET2 0x02
#define TURRET3 0x04

// this is the pin on port f where we read the turn on switch
#define TURN_ON_SWITCH 0x08


/*---------------- Global Variables -------------------------*/
char CURRENT_STATE;


/*---------------- Function Prototypes ----------------------*/
unsigned char FindBrightestSensor(void);
void WasteMilliseconds(int milliseconds);
void LoadNextBall(void);


/*---------------- Helper Functions -----------------------------*/
// unsigned char FindBrightestSensor(void)
// Desc: returns the value of the sensor that sees the IR beacon the brightest,
//        or NO_SENSOR if none are above LOW_SENSOR_THRESHOLD
// Parms: none
// Calls: A/D functions
unsigned char FindBrightestSensor(void)
{
  int far_right, right, center, left, far_left, max_reading;
  unsigned char brightest_sensor;

  // get the dc values; each should be 0 to 255
  far_right = AD_Read(FAR_RIGHT_SENSOR);
  right = AD_Read(RIGHT_SENSOR);
  center = AD_Read(CENTER_SENSOR);
  left = AD_Read(LEFT_SENSOR);
  far_left = AD_Read(FAR_LEFT_SENSOR);
//  printf("far_right = %d\n", far_right);
//printf("right = %d\n", right);
//printf("center = %d\n", center);
//printf("left = %d\n", left);
//printf("far_left = %d\n", far_left);

  // determine brightest of those readings
  brightest_sensor = NO_SENSOR;
  max_reading = LOW_SENSOR_THRESHOLD;
  if(far_right > max_reading) { max_reading = far_right; brightest_sensor = FAR_RIGHT_SENSOR; }
  if(right > max_reading) { max_reading = right; brightest_sensor = RIGHT_SENSOR; }
  if(center > max_reading) { max_reading = center; brightest_sensor = CENTER_SENSOR; }
  if(left > max_reading) { max_reading = left; brightest_sensor = LEFT_SENSOR; }
  if(far_left > max_reading) { max_reading = far_left; brightest_sensor = FAR_LEFT_SENSOR; }
//  printf("Brightest_Sensor = %d\n", brightest_sensor);

  if( far_right > SPURIOUS_SENSOR || right > SPURIOUS_SENSOR ||  center > SPURIOUS_SENSOR ||  left > SPURIOUS_SENSOR || far_left > SPURIOUS_SENSOR)
  {
    return(NO_SENSOR);
  } 
  return(brightest_sensor);
}


void WasteMilliseconds(int milliseconds)
{
  int i, j;
  for(i=0; i<milliseconds-1; i++)
  {
    j=0;
    while(j<73) { j++; }
  } 
}


void LoadNextBall()
{
 static turret = TURRET1;
 char count;
 count = 40;
 while(count>0)
 {
   WritePORTG(turret);
//   printf("went hi with turret %d, count %d\n", turret, count);
   WasteMilliseconds(TURRET_CLICK_PERIOD);
   WritePORTG(0x00);
   WasteMilliseconds(TURRET_CLICK_PERIOD);
   count--;
 }
 
 if(turret==TURRET1) turret = TURRET2;
 else if(turret==TURRET2) turret = TURRET3;
 else turret = TURRET1;
}


/*---------------- Main -----------------------------*/
void main (void)
{
  unsigned char portf;
  unsigned char count;
 
  PLS_Init();
  PWM_Init();
  AD_Init();
 
  printf("waiting for turn on\n");
  WasteMilliseconds(1000);
  do
  {
    portf = ReadPORTF();
  }
  while(portf & TURN_ON_SWITCH);  // low for turn on, so as long as we're high keep waiting

  while(1)
  {
    while(FindBrightestSensor()!=CENTER_SENSOR)
    {
      WritePORTG(0x00);
      printf("spinning right\n");
      PWM_SetSpeed(100, PWM_CHAN1);
    }
    WritePORTG(0x80);  // stop platform by driving in reverse
    printf("reversing platform\n\n");
    PWM_SetSpeed(100, PWM_CHAN1);
    WasteMilliseconds(STOPPING_TIME);
    PWM_SetSpeed(0, PWM_CHAN1);
    printf("quit braking\n");
    WasteMilliseconds(100);
    for(count=3; count>0; count--)
    {
      printf("firing\n");
      PWM_SetSpeed(100, PWM_CHAN2); // fire
      WasteMilliseconds(FIRING_TIME);
      printf("stopping catapult\n");
      PWM_SetSpeed(0, PWM_CHAN2);
      printf("loading next ball\n");
      LoadNextBall();
      printf("finished reloading\n");
      WasteMilliseconds(SETTLING_TIME);
      if(FindBrightestSensor()!=CENTER_SENSOR) break;
         // if we're not still seeing sensor then exit back into spin
    }
    WritePORTG(0x00);
    printf("spinning right\n");
    PWM_SetSpeed(100, PWM_CHAN1);
    WasteMilliseconds(2000);  // spin for 2 secs before starting to look for sensor again
  }
}