#pragma config(Sensor, S1,     touchSensor1,        sensorTouch)
#pragma config(Sensor, S2,     touchSensor2,        sensorTouch)
#pragma config(Sensor, S3,     touchSensor3,        sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
Original Code by Ray McNamara (www.rjmcnamara.com)

This program is free software: you can redistribute it and/or modify it under the terms
of the GNU General Public License as published by the Free Software Foundation, either
version 3 of the License, or (at your option) any later version. In particular, you must
report the name of the original author.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU General Public License for more details.

You can get a copy of the GNU General Public License from www.gnu.org.

*/


/**********************************************************************************
*              Set Constants and Variables                                        *
**********************************************************************************/

float  V1, V2, V3;
int    Vx, Vy, PowerA, PowerB, PowerC, oldVx, oldVy;


/**********************************************************************************
*              Change Robots Direction of travel                             *
***********************************************************************************/
void ChangeDirection()
{
  Vx = random(200)-100;                 // Object Found, Change Vx Direction
  Vy = random(200)-100;                 // Object Found, Change Vy Direction
  if (Vx < 30 && Vx > -30 && Vy < 30 && Vy > -30)
  { Vx = 30;
    Vx = -30;
  }             // Set Minimum Vx Direction
}




/**********************************************************************************
*              Monitor the 3x Utrasonic Sonar Sensors                             *
***********************************************************************************/
task MonitorSensors()
{
  while (true)
  {

    // Sonar 1
    if (SensorValue(touchSensor1) == 1 && SensorValue(touchSensor2) == 0 && SensorValue(touchSensor3) == 0)
    {
      Vx = 100;             		// Object Found, Change Vy Direction
      Vy = 0;             			// Object Found, Change Vy Direction

    }

    // Sonar 2
    if (SensorValue(touchSensor1) == 0 && SensorValue(touchSensor2) == 1 && SensorValue(touchSensor3) == 0)
    {
      Vx = 0;             			// Object Found, Change Vy Direction
      Vy = 100;              		// Object Found, Change Vy Direction

    }

    // Sonar 3
    if (SensorValue(touchSensor1) == 0 && SensorValue(touchSensor2) == 0 && SensorValue(touchSensor3) == 1)
    {
      Vx = 0;                		// Object Found, Change Vy Direction
      Vy = -100;                // Object Found, Change Vy Direction

    }


    // Sonar 1 & 2
    if (SensorValue(touchSensor1) == 1 && SensorValue(touchSensor2) == 1 && SensorValue(touchSensor3) == 0)
    {
      Vx = 100;                 // Object Found, Change Vy Direction
      Vy = 100;                 // Object Found, Change Vy Direction

    }


    // Sonar 2 & 3
    if (SensorValue(touchSensor1) == 0 && SensorValue(touchSensor2) == 1 && SensorValue(touchSensor3) == 1)
    {
      Vx = -100;                // Object Found, Change Vy Direction
      Vy = 0;                 // Object Found, Change Vy Direction

    }


    // Sonar 3 & 1
    if (SensorValue(touchSensor1) == 1 && SensorValue(touchSensor2) == 0 && SensorValue(touchSensor3) == 1)
    {
      Vx = -100;                 // Object Found, Change Vy Direction
      Vy = -100;                               // Object Found, Change Vy Direction

    }
    wait1Msec(50);
  }
}



/**********************************************************************************
*              Main Killough Platform Control                                     *
***********************************************************************************/
task main()
{

  StartTask(MonitorSensors);                     // Start Monitoring the Ultrasonic Sonar Sensors

  ChangeDirection();

  while (true)
  {
    int count = 0;
    oldVx = Vx; oldVy = Vy;

    V1 = Vx;                                // Vector Calculation for MotorA(V1)'s Power

    V2 = -Vx/2 - sqrt(3)/2 * Vy;        // Vector Calculation for MotorB(V2)'s Power

    V3 = -Vx/2 + sqrt(3)/2 * Vy;        // Vector Calculation for MotorC(V3)'s Power

    PowerA = V2;                            // Convert to an Integer Value for MotorA
    PowerB = V1;                            // Convert to an Integer Value for MotorB
    PowerC = V3;                            // Convert to an Integer Value for MotorC

    motor[motorA] = PowerA;                 // Set MotorA's Velocity (Power Setting)
    motor[motorB] = PowerB;                 // Set MotorB's Velocity (Power Setting)
    motor[motorC] = PowerC;                 // Set MotorC's Velocity (Power Setting)

    while (oldVx == Vx && oldVy == Vy)
    {
      wait1Msec(50);
      count = count + 1;
      if (count == 50) {ChangeDirection(); count = 0;}
    }
  }
}