Input.cpp

#include "WPILib.h"
#include "Input.h"
#include "DriverStationLCD.h"
#include <assert.h>

Input::Input()
: right_stick(2),
	left_stick(1),
	arcade_stick(1),
	dseio(DriverStation::GetInstance()->GetEnhancedIO())
{
	
}
void Input::Initialize()
{
	//dseio = &(DriverStation::GetInstance()->GetEnhancedIO());
	
}

bool Input::ShouldManualDriveArm()
{
	return right_stick.GetRawButton(6) && right_stick.GetRawButton(11);
}

double Input::GetKickPower()
{
	return dseio.GetAnalogIn(1) / 3.3; // Normalized.
}

float Input::GetArcadeLeft()
{
	float left = arcade_stick.GetY() + arcade_stick.GetX();
	return left;
}

float Input::GetArcadeRight()
{

	float right = arcade_stick.GetY() - arcade_stick.GetX();
	return right;
}

bool Input::ShouldKick()
{
	return right_stick.GetTrigger();
}

float Input::GetLeftDrive()
{
	return left_stick.GetY();
}

float Input::GetRightDrive()
{
	return right_stick.GetY();
}

bool Input::MagicButton()
{
	return false;//dseio->GetButton(0);
}

UINT16 Input::GetAllButtons()
{
	return dseio.GetButtons();
}


bool Input::ScorpButton()
{
	return !(dseio.GetDigital(7) != 0);
}

bool Input::ClimbButton()
{
	return !(dseio.GetDigital(1) != 0);
}
bool Input::shouldReverse()
{
	return !dseio.GetDigital(5);	
	//return false;
}

bool Input::GetDigitalIn(UINT32 channel)
{
	return !dseio.GetDigital(channel); // This is just for testing.
}

bool Input::shouldFullSuck(){
	return !dseio.GetDigital(3);
	//return false;
}

bool Input::ShouldResetWheelEncoders()
{
	return false;
}

bool Input::ShouldTarget()
{
	//assert(GetLevel() == 6);
	if(GetLevel()==6 || GetLevel() == 10){
		return left_stick.GetTrigger();
	}
	else
		return false;
}

bool Input::ShouldExtendClimber()
{
	//return right_stick.GetRawButton(3);
	return this->ScorpButton();
	//return false;
}

bool Input::ShouldStopExtendingClimber()
{
	return right_stick.GetRawButton(6);
}

bool Input::ShouldClimb()
{
	return right_stick.GetRawButton(11);
}

bool Input::ShouldStopClimbing()
{
	return !right_stick.GetRawButton(11);
}

bool Input::ShouldStartClimb()
{
	return this->ClimbButton();
}

bool Input::ShouldResetClimber()
{
	return (right_stick.GetRawButton(8) && right_stick.GetRawButton(9));
}

bool Input::ShouldArmKicker()
{
	//return right_stick.GetRawButton(4);
	//return false;
	
	
	return dseio.GetDigital(6) == 0;
}

bool Input::ShouldStopArmingKicker()
{
	return right_stick.GetRawButton(7);
}

bool Input::ShouldLaunchKicker()
{
	//return right_stick.GetRawButton(5);
	//return right_stick.GetRawButton(5);
	//return false;
	
	return dseio.GetDigital(8) == 0;
	
}

bool Input::ShouldClimbUp()
{
	return right_stick.GetRawButton(11);
}

bool Input::ShouldClimbDown()
{
	return right_stick.GetRawButton(10);
}

bool Input::ShouldWindUp() 
{
	return right_stick.GetRawButton(6);
}

bool Input::ShouldWindDown()
{
	return right_stick.GetRawButton(7);
}

bool Input::ShouldResetKicker()
{
	return ( right_stick.GetRawButton(8) && right_stick.GetRawButton(9) );
}



// THIS IS FOR TESTING ONLY
// When we're done with our modules, we'll write functions that avoid raw joystiq exposure.
/*Joystick * Input::GetStick(int stick)
{
	if(stick == 0)
		return (Joystick*)&left_stick;
	else if(stick == 1)
		return (Joystick*)&right_stick;
	else 
		return (Joystick*)&arcade_stick;
//	else
		//dslcd->Printf(DriverStationLCD::kUser_Line1, 1, "!ERROR Joystick not Recognised");	
}*/

//bool Input::ShouldTarget()
//{
//	return right_stick.GetTrigger();
//}