//Goalie for RoboCup Junior soccer
//David Wyatt 25/02/2003

//Ideas:
//Use lidar to detect edge of goal
//Slow down motors gradually to prevent inadvertent turning
//Light sensor at end & in centre of rotation to detect orientation better

//N.B. Light sensors set to raw mode, where light == low value

#define LIGHT_FLOOR SENSOR_1 //Light sensor pointed at floor
#define BALL_SENSOR SENSOR_2 //Collimated light sensor and bumper sensor to detect ball
#define SURROUNDINGS SENSOR_3 //Collimated light sensor (for lidar) and bumper sensor to detect the walls
#define LEFT OUT_A
#define RIGHT OUT_C
#define KICKER OUT_B
#define ERROR 4
#define POS_ERROR 2 // Error allowed in locating darkest position out of a circle
#define GOALIEZONE 40 // Percentage of pitch in which goalie is allowed to roam
#define TOUCHSENSOR 60 //The raw value when a touch sensor is pressed

//All these in deciseconds
#define CHECKTIME 40
#define WOBBLETIME 0 //Time for oscillation in front-back bumper to die down
#define TIMEOUT 20
#define REVTIME 7
#define WHOLETURN 18
#define PARTTURN 4
#define SMALLPAUSE 10
#define BIGPAUSE 30
#define KICK_TIME 2

#define MAX_BUMPS 1
#define BALL_BRIGHTNESS 780 //Max - needs calibration, depending on surroundings and chargedness of ball

int home = 0;
int away = 0;
int position = 50;
int positionold;

task main()
{
 init();
 calibrate();
 
 until (SURROUNDINGS > TOUCHSENSOR);  //Wait until told to continue
 until (SURROUNDINGS <= TOUCHSENSOR);
 until (SURROUNDINGS > TOUCHSENSOR);
 Wait(WOBBLETIME * 10);

 kick();
 start trackball;
 while (true)
 {
  quicklocate();
  if (position > GOALIEZONE)
  {
   stop trackball;
   aim(); //Point at own goal
   OnFwd(LEFT + RIGHT); //Drive forward until hit wall
   until(position <= GOALIEZONE/2)
   {
    quicklocate();
   }
   OnFwd(LEFT); //Turn back to normal position
   OnRev(RIGHT);
   Wait(PARTTURN * 10);
   Off(LEFT + RIGHT);
   start trackball;
   continue;
  }
  if (BALL_SENSOR <= TOUCHSENSOR) //Ball has hit front bumper
  {
   kick();
   continue;
  }
 }
}

task trackball()
{
 int previous;
 int movingflag = 0;
 findball();
 /*Off(LEFT + RIGHT);*/
 while (true)
 {
  previous = BALL_SENSOR;
  Wait(10);
  if (SURROUNDINGS <= TOUCHSENSOR)
  {
   Toggle(LEFT + RIGHT);
   On(LEFT + RIGHT);
   Wait(20);
   /*Off(LEFT + RIGHT);
   Toggle(LEFT + RIGHT);*/
  }
  if (BALL_SENSOR > previous + ERROR)
  {
   /*if (movingflag == 0) */
   {
    On(LEFT + RIGHT);
    movingflag = 1;
    previous = BALL_SENSOR;
    Wait(10);
    if (BALL_SENSOR > previous + ERROR)
    {
     Float(LEFT + RIGHT);
     Toggle(LEFT + RIGHT);
     Wait(5);
     On(LEFT + RIGHT);
    }
   }
   /*else
   {
    Off(LEFT + RIGHT);
    movingflag = 0;
   }*/
  }
 }
}

void init()
{
 SetSensorType(LIGHT_FLOOR, SENSOR_TYPE_LIGHT);
 SetSensorMode(LIGHT_FLOOR, SENSOR_MODE_RAW);
 SetSensorType(BALL_SENSOR, SENSOR_TYPE_LIGHT);
 SetSensorMode(BALL_SENSOR, SENSOR_MODE_RAW);
 SetSensorType(SURROUNDINGS, SENSOR_TYPE_LIGHT);
 SetSensorMode(SURROUNDINGS, SENSOR_MODE_RAW);
}

void calibrate()
{
 until (abs(home - away) > ERROR)
 {
  until (SURROUNDINGS <= TOUCHSENSOR);
  until (SURROUNDINGS > TOUCHSENSOR);
  home = LIGHT_FLOOR;
  PlaySound(SOUND_CLICK);
  until (SURROUNDINGS <= TOUCHSENSOR);
  until (SURROUNDINGS > TOUCHSENSOR);
  away = LIGHT_FLOOR;
  if (abs(home - away) <= ERROR) {PlaySound(SOUND_DOWN);}
 }
 PlaySound(SOUND_UP);
}

void findball()
{
 int highest = 0, highest2;
 OnFwd(LEFT+RIGHT);
 until(SURROUNDINGS > TOUCHSENSOR);
 Wait(WOBBLETIME * 10);
 until(SURROUNDINGS <= TOUCHSENSOR);
 do
 {
  highest2 = 1023;
  Toggle(LEFT + RIGHT);
  until(SURROUNDINGS > TOUCHSENSOR);
  Wait(WOBBLETIME * 10);
  until(BALL_SENSOR <= highest + ERROR || SURROUNDINGS <= TOUCHSENSOR)
  {
   if (BALL_SENSOR <= highest2 - ERROR) {highest2 = BALL_SENSOR;}
  }
  highest = highest2;
 }
 while (SURROUNDINGS <= TOUCHSENSOR);
 Off(LEFT + RIGHT);
}

void aim()
{
 int lowest = 0, lowest2;
 OnFwd(LEFT);
 OnRev(RIGHT);
 do
 {
  lowest2 = 100;
  /*Toggle(LEFT + RIGHT);*/
  ClearTimer(0);
  do
  {
   quicklocate();
   if (position <= lowest2) {lowest2 = position;}
  }
  while (position >= lowest + POS_ERROR && Timer(0) <= WHOLETURN);
  lowest = lowest2;
 }
 while (Timer(0) >= WHOLETURN);
 Off(LEFT + RIGHT);
}

void aim2()
{
 int previous = 100;
 OnFwd(LEFT);
 OnRev(RIGHT);
 until (position >= previous + POS_ERROR)
 {
  quicklocate();
  previous = position;
 }
 Off (LEFT + RIGHT);
}

void quicklocate()
{
 position = (LIGHT_FLOOR - home) * 100 / (away - home);
}

void beepPosition()
{
 int tens = position / 10, units = position - (tens * 10), i;
 for (i=0; i < tens; i++)
 {
  PlaySound(SOUND_CLICK);
  Wait(20);
 }
 /*Wait(30);
 for (i=0; i < units; i++)
 {
  PlaySound(SOUND_CLICK);
  Wait(20);
 }*/
}

void wander()
{
 int i;
 OnRev(LEFT);
 OnFwd(RIGHT);
 Wait(Random(100));
 ClearTimer(0);
 Fwd(LEFT + RIGHT);
 for(i=0; i < MAX_BUMPS && Timer(0) < 30; i++)
 {
  until (SURROUNDINGS <= TOUCHSENSOR);
  until (SURROUNDINGS > TOUCHSENSOR);
 }
 reverse();
}

void reverse()
{
 OnRev(LEFT + RIGHT);
 Wait(REVTIME * 10);
 Off(LEFT + RIGHT);
}

void kick()
{
 OnFwd(KICKER);
 Wait(KICK_TIME * 10);
 OnRev(KICKER);
 Wait(KICK_TIME * 10);
 Off(KICKER);
}