RobotMain.cpp

// RobotMain.cpp
// Top level code for robot program.

#include <iostream>
#include <iomanip>
#include <cmath>
#include "WPILib.h"
#include "DriveTrain.h"
#include <fstream>
#include "Config.h"
#include "BallCollector.h"
#include "Shooter.h"
#include "TargetServer.h"
#include "CowCatcher.h"
#include "Target.h"
#include "FireControl.h"
#include "LookupTable.h"

using namespace std;

void ParseIntoLookup(LookupTable * lt, std::string fname);
// Sorting comparator
bool ByHeight(Target a, Target b)
{
	return b.getMidY() < a.getMidY();
}

class RedAlertRobot : public IterativeRobot
{
private:
	DriveTrain* MyDrive;
	Joystick* LeftStick;
	Joystick* RightStick;
	Joystick* ThirdStick;
	BallCollector* MyBallCollection;
	DriverStationEnhancedIO* DriveSwitch;
	Compressor* MyCompressor;
	Gyro* MyGyro;
	ADXL345_SPI* Accelerometer;
	bool IsFieldOrientated;
	bool shooterUpManual;
	int Mode;
	ofstream fout;
	Shooter* shooter;
	TargetServer* target_server;
	FireControl* fire_control;
	double OldLeftX, OldLeftY, OldRightX, OldRightY;
	CowCatcher* Cow;
	Timer AutoTimer;
	Dashboard* db;
	Relay* ringLight;
	Timer LogTimer;
	LookupTable* speedByDistance;
	float target_x, target_y, target_dist;
	
	//Antonomus state
	int AutoState;
	int TargetingState;
	
	//AutoBalance state;
	int AutoBalanceState;
	int c4counter;
	
	// Peak acceleration
	float peak_accel;
	
	void SendDashBoardData(DriveTrain *,Shooter *);
	void LogJoystick(Joystick * stick, ofstream &fo) {
		fo << stick->GetX() << "," << stick->GetY() << "," << stick->GetZ() << ",";
		int buttons = 0;
		
		for (int i = 1; i <= 11; ++i) {
			int mask = 1 << (i-1); // Left shift operator
			if (stick->GetRawButton(i)) {
				buttons += mask;
			}
		}
		
		fo << buttons << ",";
	}
public:
	// Constructor, initializes variables
	RedAlertRobot(void){
		MyDrive=NULL;
		shooter=NULL;
		LeftStick=NULL;
		RightStick=NULL;
		ThirdStick=NULL;
		MyBallCollection=NULL;
		DriveSwitch=NULL;
		MyCompressor=NULL;
		MyGyro=NULL;
		Accelerometer=NULL;
		Mode=0;
		target_server=NULL;
		fire_control=NULL;
		Cow=NULL;
		AutoState=0;
		db=NULL;
		ringLight=NULL;
		speedByDistance=NULL;
		TargetingState = 0;
		target_x = target_y = target_dist = 0;
		AutoBalanceState=0;
		peak_accel = 0;
	}
	
	void AutoTarget(vector<Target> targets);
	bool LockedOnTarget() { return TargetingState > 0 && TargetingState <= 3; }
	bool ReadyToFire() { return TargetingState == 3; }
	void AutoBalance();
	void DisplayInitMessage();
	
	// Initialization function, "boot" sequence for Robot
	void RobotInit() {
		Mode=0;
		cout << "****Initializing Robot...****" << endl;
		
		cout << "2012 Red Alert Robot" << endl;
		cout << "Compiled on: ";
		cout << __DATE__ << " at " << __TIME__ << endl;
		
		cout<<"Loading Config File..... "<<endl;
		Config::LoadFromFile("config.txt");
		cout<<"Done!"<<endl;
		
		IsFieldOrientated = false;
		shooterUpManual = false;
		OldLeftX = 0.0;
		OldLeftY = 0.0;
		OldRightX = 0.0;
		OldRightY = 0.0;
		
		cout<<"Initializing Drive Train.... ";
		MyDrive=new DriveTrain(4,6,3,5);  //4=FL , 6=FR , 3=RL , 5=RR
		cout<<"Done!"<<endl;
		
		cout<<"Initializing JoySticks....";
		LeftStick=new Joystick(1);
		RightStick=new Joystick(2);
		ThirdStick=new Joystick(3);
		cout<<"Done!"<<endl;
		
		cout<<"Initializing DriveSwitch...."; //A switch for drives
		DriveSwitch=&(DriverStation::GetInstance()->GetEnhancedIO());
		cout<<"Done!"<<endl;
		
		cout<<"Initializing A Method By Which You Can Capture Mammallia....";
		Cow=new CowCatcher(7,8,3,4,5,12,6,7);
		cout<<"Done!"<<endl;
		
		cout<<"Initializing Ball Collection....";
		MyBallCollection=new BallCollector(11,10,9,12);
		cout<<"Done!"<<endl;
		
		cout<<"Initializing Shooter.....";
		shooter = new Shooter(7,8,5,6);
		cout<<"Done!"<<endl;
		
		cout<<"Initializing Compressor....";
		MyCompressor=new Compressor(11,1);
		cout<< "Done!"<<endl;
		
		cout<<"Initializing Accelerometer....";
		Accelerometer = new ADXL345_SPI(1,1,2,3,4,ADXL345_SPI::kRange_2G);
		cout<<"Done!"<<endl;
		
		cout<<"Initializing Gyro....";
		MyGyro = new Gyro(1,1);
		cout<<"Done!"<<endl;
		
		cout<<"Initializing Relay Light.....";
		ringLight = new Relay(2,Relay::kForwardOnly);
		cout<<"Done!"<<endl;
		
		cout<<"Initializing Target Server.....";
		target_server = new TargetServer();
		cout<<"Done"<<endl;
		
		cout<<"Initializing Fire Control...";
		fire_control = new FireControl();
		cout << "Done" << endl;
		cout<<"Loading from file....";
		fire_control->LoadRangeDataFromFile("range.txt");
		fire_control->LoadAltRangeDataFromFile("altrange.txt");
		cout<<"Success!"<<endl;
		
		cout<<"Initializing speed by distance table....";
		speedByDistance = new LookupTable(0.1,0.0);
		cout<<"Done!"<<endl;
		
		cout << "Initializing DriverStation connection....";
		db=new Dashboard(NULL);
		cout<<"Done!"<<endl;
		
		Config::Dump();
		cout << "****Robot Initialization Completed****" << endl;
		
		DisplayInitMessage();
	}
	
	// Initialization for when robot is first disabled
	void DisabledInit() 
	{
		Mode=1;
		//this->Logging(0,0,0,0,0,0,0);
		
		cout << "Reloading configuration file..." << endl;
		
		bool status = Config::LoadFromFile("config.txt");
		
		if (!status) {
			cout << "Uh oh. Config file didn't load. All constants will revert to defaults" << endl;
		} else {
			cout << "Config loaded." << endl;
			Config::Dump();
		}
		
		//MyDrive->ResetCANJaguars();
		
		cout<<"Loading from file....";
		fire_control->LoadRangeDataFromFile("range.txt");
		cout<<"Success!"<<endl;
		
		ringLight->Set(Relay::kOff);
	}
	
	// Initialization for when robot is first put in autonomous mode
	void AutonomousInit() 
	{
		cout << "Opening Logging.csv....";
		if (fout.is_open()) {
			cout << "Log file already open, continuing to write." << endl;
		} else {
			fout.open("Logging.csv");
			this->LogHeaders();
			cout << "Done!" << endl;
		}
		Mode=2;
		this->Logging(0.0,0.0,0.0,0.0,0,0,0.0,0.0,0.0,0.0);
		TargetingState = 0;
		//AutoTimer.Start();
		AutoState=1;
		MyDrive->ResetOdometer();
		c4counter =0;
		ringLight->Set(Relay::kOn);
		StartLogTimer();
		ParseIntoLookup(speedByDistance,"speedbydistance.txt");
		MyDrive->SwitchDrive(true,false);
	}
	
	void StartLogTimer()
	{
		LogTimer.Start();
	}
	// Initialization for when robot is first put in teleoperated mode
	void TeleopInit() {
		cout << "Opening Logging.csv....";
		if (fout.is_open()) {
			cout << "Log file already open, continuing to write." << endl;
		} else {
			fout.open("Logging.csv");
			this->LogHeaders();
			cout << "Done!" << endl;
		}
		MyDrive->SwitchDrive(true,false);
		shooter->SetPosition(false,true);
		this->Logging(0,0,0,0,0,0,0,0,0,0);
		Mode=3;
		
		StartLogTimer();
		ringLight->Set(Relay::kOn);
	}

	// Periodic function (called n times a second) when robot is disabled
	void DisabledPeriodic() {
		MyCompressor->Stop();
		//this->Logging(0,0,0,0,0,0,0,0,0,0);
		Mode=4;
	}
	
	// Periodic function (called n times a second) when robot is in auto mode
	void AutonomousPeriodic() {
		Mode=5;
		int auto_program = (int) Config::GetSetting("auto_program", 1);
		
		double SIMULATEDJOYSTICK = speedByDistance->Lookup(MyDrive->GetDist());
		double GyroAngle = MyGyro->GetAngle();
		double AxisX = Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_X);
		double AxisY = Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_Y);
		double AxisZ = Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_Z);
		double BumpThreshold = Config::GetSetting("auto_bump_threshold",2.0);
		// TODO: Which axis is forward? 
		double AccelMagnitude = ::fabs(AxisX);
		if (AccelMagnitude > peak_accel) {
			peak_accel = AccelMagnitude;
		}
		
		switch (auto_program) {
			case 1:
			{
				int shooter_leniance = (int) Config::GetSetting("shooter_leniance",25);
				double DISTANCETOFENDER=Config::GetSetting("auto_distancetofender",0);
				double shooter_RPM = Config::GetSetting("auto_shooter",1100);
				
				cout<<"Because we are at (x) "<<MyDrive->GetDist()<<" inches, we are travling at "<<speedByDistance->Lookup(MyDrive->GetDist())<<"%"<<endl;
				
				//Stars incase of alliance feeding
				MyBallCollection->SetRollerSpeed(-1 * (STARMOTORSPEED));
				//Start
				switch(AutoState)
				{
				case 0:
					//Idle state
					break;
				case 1:
					//Drive forward to fender
					MyDrive->Drive(0.0,SIMULATEDJOYSTICK*-1,0.0,SIMULATEDJOYSTICK*-1,0.0,false);
					AutoState=2;
					break;
				case 2:
					//Check odometer
					if(MyDrive->GetDist()<=DISTANCETOFENDER)
						AutoState=1;
					else
						AutoState=3;
					
					if (peak_accel >= BumpThreshold && (MyDrive->GetDist() > DISTANCETOFENDER * 0.75)) 
						AutoState = 3;
					
					break;
				case 3:
					//Set Shooter speed and checks actual speed
					MyDrive->Drive(0.0,0.0,0.0,0.0,0.0,false);
					shooter->SetWheelSpeed(shooter_RPM,shooter_RPM);
					if(shooter->GetTopWheelSpeed()>=(shooter_RPM-shooter_leniance) && shooter->GetTopWheelSpeed()<=(shooter_RPM+shooter_leniance))
						AutoState=4;
					break;
				case 4:
					MyBallCollection->SetTopBeltSpeed(SHOOTERBELTSPEED);
					MyBallCollection->SetMidBeltSpeed(BELTMOTORSPEED);
					
					AutoState=3;
					break;
				default:
					cout<<"YOU BROKE THE ROBOT"<<endl;
				}
				//End
				
				cout << "STATE: " << AutoState << "\tDIST: " << MyDrive->GetDist() << endl;
				
			}
			break;
			case 2:
			{
				vector<Target> targets = target_server->GetTargets();
				AutoTarget(targets);
				if (ReadyToFire()) {
					MyBallCollection->SetTopBeltSpeed(SHOOTERBELTSPEED);
					MyBallCollection->SetMidBeltSpeed(BELTMOTORSPEED);
				}
			}
			break;
			case 3:
			{
				int shooter_leniance = (int) Config::GetSetting("shooter_leniance",25);
				double DISTANCETOFENDER=Config::GetSetting("auto_distancetofender",0);
				double shooter_RPM = Config::GetSetting("main_shooter_middle_preset_speed",950);
				
				cout<<"Because we are at (x) "<<MyDrive->GetDist()<<" inches, we are travling at "<<speedByDistance->Lookup(MyDrive->GetDist())<<"%"<<endl;
				
				//Stars incase of alliance feeding
				MyBallCollection->SetRollerSpeed(-1 * (STARMOTORSPEED));
				//Start
				switch(AutoState)
				{
				case 0:
					//Idle state
					break;
				case 1:
					//Drive forward to fender
					MyDrive->Drive(0.0,SIMULATEDJOYSTICK*-1,0.0,SIMULATEDJOYSTICK*-1,0.0,false);
					AutoState=2;
					break;
				case 2:
					//Check odometer
					if(MyDrive->GetDist()<=DISTANCETOFENDER)
						AutoState=1;
					else
						AutoState=3;
					
					if (peak_accel >= BumpThreshold && (MyDrive->GetDist() > DISTANCETOFENDER * 0.75)) 
						AutoState = 3;
					
					break;
				case 3:
					//Set Shooter speed and checks actual speed
					MyDrive->Drive(0.0,0.0,0.0,0.0,0.0,false);
					shooter->SetWheelSpeed(shooter_RPM,shooter_RPM);
					if(shooter->GetTopWheelSpeed()>=(shooter_RPM-shooter_leniance) && shooter->GetTopWheelSpeed()<=(shooter_RPM+shooter_leniance))
						AutoState=4;
					break;
				case 4:
					MyBallCollection->SetTopBeltSpeed(SHOOTERBELTSPEED);
					MyBallCollection->SetMidBeltSpeed(BELTMOTORSPEED);
					
					AutoState=3;
					break;
				default:
					cout<<"YOU BROKE THE ROBOT"<<endl;
				}
				//End
				
				cout << "STATE: " << AutoState << "\tDIST: " << MyDrive->GetDist() << endl;
			}
			break;
			
			//Feed Balls
			case 4:
			{
				c4counter++;
				cout<<c4counter<<endl;
				if (c4counter>100)
				{
				MyDrive->SwitchDrive(false,true);
				MyBallCollection->SetRollerSpeed(STARMOTORSPEED);
				}
			}
			break;
		}
		Logging(SIMULATEDJOYSTICK,0.0,0.0,0.0,0,0,GyroAngle, AxisX, AxisY, AxisZ);
	}
	
	// Periodic function (called n times a second) when robot is 
	// in teleoperated mode
	void TeleopPeriodic();

	// Not used yet
	void DisabledContinuous() { }
	void AutonomousContinuous() { }
	void TeleopContinuous() { }
	void LogHeaders() 
	{
		char * joy_headers[4] = { "X", "Y", "Z", "Buttons" };
		char * joy_names[3] = {"Left", "Right","Third" };
		fout << "Time,";
		fout << "Mode," ;
		MyDrive->LogHeaders(fout);
		shooter->LogHeaders(fout);
		Cow->LogHeaders(fout);
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 4; j++) {
				fout << joy_names[i] << " " << joy_headers[j] << ",";
			}
		}
		
		fout << "RateLimited Left X,";
		fout << "RateLimited Left Y,";
		fout << "RateLimited Right X,";
		fout << "RateLimited Right Y,"; 
		fout << "Mecanum Switch,";
		fout << "Tank Switch,";
		fout << "Gyro Angle,";
		fout << "Accel X,";
		fout << "Accel Y,";
		fout << "Accel Z,";
		fout << "PeakAccel,";
		fout << "AutoState,";
		for (int i = 0; i < 4; i++) {
			fout << "Target " << i+1 << " X,";
			fout << "Target " << i+1 << " Y,";
			fout << "Target " << i+1 << " Dist,";
		}
		fout << "\n";
	}
	void Logging(double LeftXAxis,double LeftYAxis,double RightXAxis,double RightYAxis,int MecButtonDS,int TankButtonDS,double GyroAngle,double AxisX, double AxisY, double AxisZ) 
	{
		fout << LogTimer.Get() << ",";
		fout << Mode << ",";
		MyDrive->LogMotors(fout);
		shooter->LogMotors(fout);
		Cow->Log(fout);
		LogJoystick(LeftStick, fout);
		LogJoystick(RightStick, fout);
		LogJoystick(ThirdStick, fout);
		
		fout<< LeftXAxis << ",";
		fout<< LeftYAxis << ",";
		fout<< RightXAxis << ",";
		fout<< RightYAxis << ",";
		fout<< MecButtonDS << ",";
		fout<< TankButtonDS << ",";
		fout<< GyroAngle << ",";
		fout<< AxisX << ",";
		fout<< AxisY << ",";
		fout<< AxisZ << ",";
		fout << peak_accel << ",";
		fout << AutoState << ",";
		
		vector<Target> targets; // = target_server->GetTargets();
		for (int i = 0; i < 4; i++) {
			float x = 0;
			float y = 0;
			float dist = 0;
			if (i < ((int) targets.size())) {
				x = targets[i].getMidX();
				y = targets[i].getMidY();
				dist = targets[i].getDistance();
			}
			
			fout << x << "," << y << "," << dist << ",";
		}
		fout<< "\n";
	}
	void RateLimit(double &LeftXAxis, double &LeftYAxis, double &RightXAxis, double &RightYAxis)
	{
			const double MAX_RATE = (Config::GetSetting("main_joystickRateLimit",1.0))/(this->GetLoopsPerSec());
			if(LeftXAxis > (OldLeftX+MAX_RATE))
				LeftXAxis = OldLeftX+MAX_RATE;
			else if(LeftXAxis < (OldLeftX-MAX_RATE))
				LeftXAxis = OldLeftX-MAX_RATE;
			
			if(LeftYAxis > (OldLeftY+MAX_RATE))
				LeftYAxis = OldLeftY+MAX_RATE;
			else if(LeftYAxis < (OldLeftY-MAX_RATE))
				LeftYAxis = OldLeftY-MAX_RATE;
			
			if(RightXAxis > (OldRightX+MAX_RATE))
				RightXAxis = OldRightX+MAX_RATE;
			else if(RightXAxis < (OldRightX-MAX_RATE))
				RightXAxis = OldRightX-MAX_RATE;
			
			if(RightYAxis > (OldRightY+MAX_RATE))
				RightYAxis = OldRightY+MAX_RATE;
			else if(RightYAxis < (OldRightY-MAX_RATE))
				RightYAxis = OldRightY-MAX_RATE;
				
			OldLeftX = LeftXAxis;
			OldLeftY = LeftYAxis;
			OldRightX = RightXAxis;
			OldRightY = RightYAxis;
	}
	
};

void RedAlertRobot::AutoTarget(vector<Target> targets)
{
	double ROTATE_SCALE = Config::GetSetting("main_RotateScale4Targeting",0.25);
	static Target primaryTarget;
	static float targetHeading = 0;
	cout << "In targeting state: " << TargetingState << endl;
	switch (TargetingState) {
		case 0: // Start targeting
			if (targets.size() > 0) {
				double max_height = -1.0;
						
				//Select the target that is lowest in view as our primary target.
				for(int i=0;i< ( (int) targets.size());i++)
				{
					if(targets[i].getMidY() >= max_height)
					{
						primaryTarget = targets[i];
						max_height = targets[i].getMidY();
					}
				
				}
						
				cout << "Target [" << primaryTarget.getMidX() << "," << primaryTarget.getMidY() 
						<< "] located " << primaryTarget.getDistance() << " feet away" << endl;
				
				target_x = primaryTarget.getMidX();
				target_y = primaryTarget.getMidY();
				target_dist = primaryTarget.getDistance();
				// Calculate offset
				float currentAngle = MyGyro->GetAngle();
				
				float targetHeadingOffset = .5 * Config::GetSetting("main_CameraFOV", 57) * primaryTarget.getMidX();
				cout << "Primary target is located " << targetHeadingOffset << " degrees away from forward." << endl;
				targetHeading = currentAngle + targetHeadingOffset;
				cout << "Target heading is " << targetHeading << endl;
				
				TargetingState = 1;
				
			}
			break;
		case 1: // Target acquired
		{
			// Try to rotate to the target
			float currentTargetHeadingOffset = MyGyro->GetAngle() - targetHeading;
			float rotateInput = currentTargetHeadingOffset / (.5 * Config::GetSetting("main_CameraFOV", 57));
			target_x = rotateInput;
			if (fabs(rotateInput) <= Config::GetSetting("main_TargetTolerance", .05)) {
				cout << "ON TARGET" << endl;
				MyDrive->Rotate(0);
				TargetingState = 2;
			} else {
				cout << "Rotating: " << rotateInput << endl;
				MyDrive->Rotate(rotateInput*ROTATE_SCALE);
			}
		}
			break;
		case 2: // Spinning up shooter wheel
			// Might need to look at target again
		{
			
			bool shooter_position = false;
			int rpm_lower = (int) shooter->GetBottomWheelSpeed();
			int rpm_higher = (int) shooter->GetTopWheelSpeed();
					
					//Decide the position of the shooter and speeds of the motors based on the target information
			fire_control->Calculate(&primaryTarget, shooter_position, rpm_higher, rpm_lower);
					
			shooter->SetPosition(shooter_position,!shooter_position);
			shooter->SetWheelSpeed(rpm_higher,rpm_lower);
			
			if (fabs(shooter->GetTopWheelSpeed() - rpm_higher) <= Config::GetSetting("shooter_leniance", 25)) {
				cout << "Ready to fire." << endl;
				TargetingState = 3;
			}
		}
			break;
		case 3:
			cout << "SHOOT YOU FOOL! SHOOT!" << endl;
			break;
		default:
			cout << "Unknown state in autotarget: " << TargetingState << endl;
			break;
			
		}
}

void RedAlertRobot::TeleopPeriodic()
{
	this->SetPeriod(0.05);
	Mode=6;
	
	double LeftXAxis = LeftStick->GetX();
	double LeftYAxis = LeftStick->GetY();
	double RightXAxis = RightStick->GetX();
	double RightYAxis = RightStick->GetY(); //Only needed for Tank Drive
	double FirstZAxis = LeftStick->GetZ();
	double SecondZAxis = RightStick->GetZ();
	double ThirdZAxis = ThirdStick->GetZ();
	this->RateLimit(LeftXAxis,LeftYAxis,RightXAxis,RightYAxis);
	
	
	bool TargetButton = LeftStick->GetRawButton(1);
	bool resetShooterJagButton = LeftStick->GetRawButton(6);
	bool resetJagButton = LeftStick->GetRawButton(7);
	bool GyroResetButton = LeftStick->GetRawButton(9);
	bool FieldOrientatedOnButton = LeftStick->GetRawButton(10);
	bool FieldOrientatedOffButton = LeftStick->GetRawButton(11);
	
	bool CowCatcherLowerButton = RightStick->GetRawButton(2);
	bool CowCatcherRaiseButton = RightStick->GetRawButton(3);
	bool CowCatcherSwitch = RightStick->GetRawButton(1);
	bool CowCatcherManualRaise = RightStick->GetRawButton(6);
	bool CowCatcherManualLower = RightStick->GetRawButton(7);
	bool CowCatcherManualLock = RightStick->GetRawButton(11);
	bool CowCatcherManualUnlock = RightStick->GetRawButton(10);
	bool CowCatcherManualMode = RightStick->GetRawButton(8);

	bool TopBeltForwardButton = ThirdStick->GetRawButton(1);
	bool ShooterPresetMiddleSpeedButton = ThirdStick->GetRawButton(2);
	bool ShooterPresetTopSpeedButton = ThirdStick->GetRawButton(3);
	bool MidBeltForwardButton = ThirdStick->GetRawButton(6);
	bool StarsForwardButton = ThirdStick->GetRawButton(8);
	bool StarsReverseButton = ThirdStick->GetRawButton(9);
	bool ShooterDownButton = ThirdStick->GetRawButton(10);
	bool ShooterUpButton = ThirdStick->GetRawButton(11);

	//int MecButtonDS = !DriveSwitch->GetDigital(2); 
	int MecButtonDS = DriveSwitch->GetDigital(2); //There was a ! here
	int TankButtonDS = DriveSwitch->GetDigital(1);
	
	double GyroAngle = MyGyro->GetAngle();
	double AxisX = Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_X);
	double AxisY = Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_Y);
	double AxisZ = Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_Z);
	
	vector<Target> targets = target_server->GetTargets();
	
	//Start Cow Catcher Section
	if (CowCatcherManualMode) {
		Cow->ManualOperate(	    CowCatcherManualRaise, 
								CowCatcherManualLower,
								CowCatcherManualLock,
								CowCatcherManualUnlock );
	} else {
		Cow->RaiseLower(CowCatcherSwitch);
	}
	//End Cow Catcher Section
	
	//Start BallCollection Section
	if(TopBeltForwardButton)
		MyBallCollection->SetTopBeltSpeed(SHOOTERBELTSPEED);
	else
		MyBallCollection->SetTopBeltSpeed(0.0);
	
	if(MidBeltForwardButton || LeftStick->GetRawButton(2))
		MyBallCollection->SetMidBeltSpeed(BELTMOTORSPEED);
	else
		MyBallCollection->SetMidBeltSpeed(0.0);
	
	if(StarsForwardButton)
		MyBallCollection->SetRollerSpeed(STARMOTORSPEED);	
	else if(StarsReverseButton || LeftStick->GetRawButton(2))
		MyBallCollection->SetRollerSpeed(-1 * (STARMOTORSPEED));
	else
		MyBallCollection->SetRollerSpeed(0.0);
	//End BallCollection Section

	
	//Start Drive Train Section
	if(FieldOrientatedOnButton)
		IsFieldOrientated = true;
	else if(FieldOrientatedOffButton)
		IsFieldOrientated = false;
			
	if(GyroResetButton)
		MyGyro->Reset();
	
	if(resetJagButton)
		MyDrive->ResetCANJaguars();
	
	MyCompressor->Start();
	if(TargetButton){
		;
	}
	else if(LeftStick->GetRawButton(8))
		this->AutoBalance();
	else
		MyDrive->Drive(LeftXAxis,LeftYAxis,RightXAxis,RightYAxis,GyroAngle,IsFieldOrientated);
	
		//Drive switching thingy on the joystick thingy
	if(RightStick->GetRawButton(4))
		MyDrive->SwitchDrive(true,false);	//Mech
	else if(RightStick->GetRawButton(5))
		MyDrive->SwitchDrive(false,true);	//Tank
	else
		MyDrive->SwitchDrive(TankButtonDS,MecButtonDS);
	//End Drive Train Section
	
	
	//Shooter Section
	if(resetShooterJagButton)
		shooter->ResetCanJags();
	
	if(ShooterUpButton) 
		shooterUpManual=true;
	else if(ShooterDownButton)
		shooterUpManual=false;
	
	double shooter_speed_input_bottom = FirstZAxis;
	double shooter_speed_input_top    = SecondZAxis;
	double shooter_speed_input_both   = ThirdZAxis;

	if(TargetButton) //Auto Aiming
	{
		AutoTarget(targets);
	}
	else //Manual Aiming 
	{
		TargetingState = 0;
		shooter->SetPosition(ShooterUpButton,ShooterDownButton);
		
		if (ShooterPresetMiddleSpeedButton) {
			int rpm_low = 0;
			int rpm_high = 0;
			rpm_low = rpm_high = (int) Config::GetSetting("main_shooter_middle_preset_speed", 950);
			
			//Check if in tank to add extra height? RPM? You know what I mean.
			if(!MyDrive->GetMechanum())
				rpm_low = rpm_high = (rpm_low + (int) Config::GetSetting("main_shooter_tank_dif", 10));

			
			shooter->SetWheelSpeed(rpm_low, rpm_high);
		} 
		else if(ShooterPresetTopSpeedButton) {
			int rpm_low = 0;
			int rpm_high = 0;
			rpm_low = rpm_high = (int) Config::GetSetting("main_shooter_top_preset_speed", 1100);
			
			//Check if in tank to add extra height? RPM? You know what I mean.
			if(!MyDrive->GetMechanum())
				rpm_low = rpm_high = (rpm_low + (int) Config::GetSetting("main_shooter_tank_dif", 10));
			
			shooter->SetWheelSpeed(rpm_low, rpm_high);
		}
		else
			shooter->ManualControl(shooter_speed_input_both);
	}
	//End Shooter Section
	
	
	//Start Sending data to the dashboard
	this->SendDashBoardData(MyDrive,shooter);
	//End sending data to the dashboard
	
	
	//Logging Section
	this->Logging(LeftXAxis,LeftYAxis,RightXAxis,RightYAxis,MecButtonDS,TankButtonDS,GyroAngle,AxisX,AxisY,AxisZ);
	//End Logging Section
	
	//AutoBalance
	
	//EndAutoBalance
	
}
void RedAlertRobot::SendDashBoardData(DriveTrain* drive, Shooter* shoot)
{
	db=&(DriverStation::GetInstance())->GetLowPriorityDashboardPacker();
	db->AddCluster();
	db->AddFloat(shoot->GetTopWheelSpeed());
	db->AddFloat(shoot->GetBottomWheelSpeed());
	//db->AddFloat(shoot->GetTopWheelSetpoint());
	db->AddU8(UINT8(drive->GetMechanum()));
	db->AddU8(UINT8(IsFieldOrientated));
	db->AddU8(UINT8(drive->GetJagStatusPacked()) | (shooter->GetShooterStatusPacked() << 4));
	db->AddBoolean(LockedOnTarget());
	db->AddBoolean(ReadyToFire());
	db->AddFloat(target_x);
	db->AddFloat(target_y);
	db->AddFloat(target_dist);
	db->AddFloat(shoot->GetTopWheelSetpoint());
	db->AddFloat(shoot->GetBottomWheelSetpoint());
	db->FinalizeCluster();
	db->Finalize();
}

void RedAlertRobot::DisplayInitMessage()
{
	cout << 
			".______        ______   .______     ______   .___________.\n"
			"|   _  \\      /  __  \\  |   _  \\   /  __  \\  |           |\n"
			"|  |_)  |    |  |  |  | |  |_)  | |  |  |  | `---|  |----`\n"
			"|      /     |  |  |  | |   _  <  |  |  |  |     |  |     \n"
			"|  |\\  \\----.|  `--'  | |  |_)  | |  `--'  |     |  |     \n"
			"| _| `._____| \\______/  |______/   \\______/      |__|     \n"
			"\n"
			"\n"
			"     _______..___________.    ___      .______     .___________.\n"
			"    /       ||           |   /   \\     |   _  \\    |           |\n"
			"   |   (----``---|  |----`  /  ^  \\    |  |_)  |   `---|  |----`\n"
			"    \\   \\        |  |      /  /_\\  \\   |      /        |  |     \n"
			".----)   |       |  |     /  _____  \\  |  |\\  \\----.   |  |     \n"
			"|_______/        |__|    /__/     \\__\\ | _| `._____|   |__|     \n"
			"                                                                \n"
			"\n"
			".__   __.   ______   ____    __    ____\n"
			"|  \\ |  |  /  __  \\  \\   \\  /  \\  /   /\n"
			"|   \\|  | |  |  |  |  \\   \\/    \\/   / \n"
			"|  . `  | |  |  |  |   \\            /  \n"
			"|  |\\   | |  `--'  |    \\    /\\    /   \n"
			"|__| \\__|  \\______/      \\__/  \\__/\n"

					<< endl;
}

void ParseIntoLookup(LookupTable * lt, std::string fname)
{
	// Input stream
	ifstream in;
		
	// Open the file
	in.open(fname.c_str(), std::ios_base::in);

	lt->Clear();
	if (!in.is_open()) {
		cout << "UH OH! CAN'T LOAD RANGE FILE" << endl;
	}
	float x, y;
		
	while (in >> x >> y) {
		cout << "At " << x << " feet, rpm = " << y << endl;
		lt->AddEntry(x,y);
	}
	
	in.close();
		

}
void RedAlertRobot::AutoBalance()
{
	double Speed = Config::GetSetting("simulatedSpeed",0.1);
	
	if(Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_X) > Config::GetSetting("UpperRange",0.25))
		AutoBalanceState=1;
	else if(Accelerometer->GetAcceleration(ADXL345_SPI::kAxis_X) < Config::GetSetting("LowerRange",-0.15))
		AutoBalanceState=2;
	else
		AutoBalanceState=0;
	
	cout << "Auto Balance State:" << AutoBalanceState << endl;
	
	switch(AutoBalanceState)
	{
	case 0:
	{
		//Idle State and Set wheels to zero
		MyDrive->Drive(0.0,0.0,0.0,0.0,0.0,false);
	}
		break;
	case 1:
	{
		MyDrive->Drive(0.0,Speed,0.0,Speed,0.0,false);
	}
		break;
	case 2:
	{
		MyDrive->Drive(0.0,Speed*-1,0.0,Speed*-1,0.0,false);
	}
		break;
	}
}

START_ROBOT_CLASS(RedAlertRobot);