/*
 * This is a sample main program that uses the WPI framework.
 *
 * This sample program is designed for a two wheel drive robot with
 * the right stick controlling the drive wheels and the left stick
 * controlling arms, which can be driven with either motors or servos
 * (make sure to uncomment the approporiate section for either motors
 * or servos, but not both).  Arm motors would be plugged into PWM Out
 * ports 3 and 4, while arm servos would be pluggin into PWM Out ports
 * 5 and 6.
 *
 * The program reads in the values from radio channels 1-4 and scales
 * them to be between -127 and 127.  The scaling values used are for
 * a Hitec Laser 4.  If you are using another radio, you will need to
 * determine your own values experimentally.
 * 
 * After it has the input values, it adds a dead band to the drive
 * motors to compensate for slop in the controller.  This can be
 * adjusted to your liking by changing the value of the deadBand
 * variable.
 * 
 * The program also checks limits switches to limit the travel of the
 * arms.  I will leave it as an excercise to the reader to figure out
 * which switches control which arms, and where they plug in.
 * 
 * The commented out motor section adds a dead band to compensate for
 * the lack of a spring return on the left stick and drives the motors
 * on ports 3 and 4.  The commented out servo section adds 127 to the
 * inputs (becuase the input values are -126 to 127, but the servo nees
 *  0 to 255) and tells to servos on ports 5 and 6 to go to those
 * positions.
 */

#include "WPILib.h"

// This is a sample autonomous mode
// The code runs for a fixed amount of time then exits back
// into the main program.
void autonomousMode(void)
{
	int i; // declare a counter variable
	ResetClock(); // reset the clock (you must do this if you are timing auto mode)

	for (i = 0; i < 4; i++) //repeat four times
	{
		Servo(5, 0); // drive arm 1 to position 0
		Drive(60, 0); // drive at speed 60 with 0 turning speed
		Wait(3000); // wait 3 seconds
		Servo(5, 255); // drive arm 1 to position 255
		Drive(60, 60); // drive at speed 60 with 60 turning speed
		Wait(750); // wait .75 seconds
	}
	Servo(1, 127); // reset arm 1 to position 127
}

void main (void)
{
	
	// declare the variables used in the main loop
	int modPWM1;
	int modPWM2;
	int modPWM3;
	int modPWM4;
	int Bottom1;
	int Top1;
	int Bottom2;
	int Top2;
	int rangeOfInput = 5;
	int rangeOfOutput = 7;
	int deadBand = 7;

	WPIInitialize(); // start the WPI framework
	TwoWheelDrive(1, 2); //drive the robot from ports 1 and 2 (the right stick)
	SetPortAsServo(5); // declare that port 5 has a servo on it
	SetPortAsServo(6); // declare that port 6 has a servo on it

	// Wait until the field starts the robot
	while (!IsEnabled()) Wait(100);

	// run the program
	if (IsAutonomous())
		autonomousMode();

	// Start the game - runs the autonomous code if necessary
	while (1)
	{
		// regular non-autonmous drive code goes here
		
		// read in PWM values
			modPWM1 = PWMIn(1); // right stick X axis (turning)
			modPWM2 = PWMIn(2); // right stick y axis (speed)
			modPWM3 = PWMIn(3); // left stick x axis (arm 1)
			modPWM4 = PWMIn(4); // left stick y axis (arm 2)

		//read in sensor values
			Bottom1 = GetPort(9); // lower limit switch for arm 1 on port 9
			Top1 = GetPort(10); // upper limit switch for arm 1 on port 10
			Bottom2 = GetPort(11); // lower limit switch for arm 2 on port 11
			Top2 = GetPort(12); // upper limit switch for arm 2 on port 12

		// scale PWM values
			modPWM1 = (int)((long)modPWM1 * rangeOfOutput / rangeOfInput);
			modPWM2 = (int)((long)modPWM2 * rangeOfOutput / rangeOfInput);
			modPWM3 = (int)((long)modPWM3 * rangeOfOutput / rangeOfInput);
			modPWM4 = (int)((long)modPWM4 * rangeOfOutput / rangeOfInput);

		// add dead band to drive motors
			if(-deadBand < modPWM1 && modPWM1 < deadBand) // is modPWM1 in dead band?
				modPWM1 = 0;
			else if (modPWM1 >= deadBand)  // is modPWM1 above the dead band?
				modPWM1 = modPWM1 + deadBand;
			else if (modPWM1 <= (-deadBand))  // is modPWM1 below the dead band?
				modPWM1 = modPWM1 - deadBand;

			if(-deadBand < modPWM2 && modPWM2 < deadBand) // is modPWM2 in dead band?
				modPWM2 = 0;
			else if (modPWM2 >= deadBand)  // is modPWM1 above the dead band?
				modPWM2 = modPWM2 + deadBand;
			else if (modPWM2 <= (-deadBand))  // is modPWM1 below the dead band?
				modPWM2 = modPWM2 - deadBand;

		// modify modPWM4 and modPWM5 so arms can't break themselves
			//check if arms are going down and bottom sensor is triggered
			if(modPWM3 < 0 && Bottom1 == 1)
				modPWM3 = 0;
			if(modPWM4 < 0 && Bottom2 == 1)
				modPWM4 = 0;

			//check if arms are going up and top sensor is triggered
			if(modPWM3 > 0 && Top1 == 1)
				modPWM3 = 0;
			if(modPWM4 > 0 && Top2 == 1)
				modPWM4 = 0;
			
		// drive the motors and servos
			Drive(modPWM2, modPWM1); // drive motors


		/* uncomment this section only if you are using the left stick control motors
			if(modPWM3 < 20 && modPWM4 > -20) // add dead band to arm
				modPWM4 = 0;
			if(modPWM4 < 20 && modPWM4 > -20) // add dead band to arm
				modPWM4 = 0;
			
			Motor(3, modPWM3); // drive motor on port 3
			Motor(4, modPWM4); // drive motor on port 4
		*/
			
		/* uncomment this section only if you are using the left stick to control servos
			Servo(5,(modPWM3 + 126)); // drive servo on port 5 with left stick x axis
			Servo(6,(modPWM4 + 126)); // drive servo on port 6 with left stick y axis
		*/
	}
}