DriverInput.cpp

#include "DriverInput.h"
#include "DriverStationLCD.h"

float DriverInput::m_left_speed = 0;
float DriverInput::m_right_speed = 0;
float DriverInput::m_upper_speed = 0;
float DriverInput::m_lower_speed = 0;
Joystick * DriverInput::m_left_stick = NULL;
Joystick * DriverInput::m_right_stick = NULL;
Joystick * DriverInput::m_third_stick = NULL;
bool DriverInput::b_line_follow = false;

bool DriverInput::retsetDIP_button = false;

bool DriverInput::reloadConfig = false;

bool DriverInput::m_capture = false;
bool DriverInput::m_release = false;
bool DriverInput::m_rotate_up = false;
bool DriverInput::m_rotate_down = false;

bool DriverInput::m_toggle_direction = false;

float DriverInput::shuttleSpeed = 0;
float DriverInput::armSpeed = 0;

//GTP Buttons for the shuttle
bool DriverInput::b_shuttle_button_1 = false;
bool DriverInput::b_shuttle_button_2 = false;
bool DriverInput::b_shuttle_button_3 = false;

//More buttons for stuff
bool DriverInput::b_arm_button_1 = false;
bool DriverInput::b_arm_button_2 = false;

//And even more buttons. These are... combination buttons?
bool DriverInput::b_select_position_1 = false;
bool DriverInput::b_select_position_2 = false;
bool DriverInput::b_select_position_3 = false;
bool DriverInput::b_select_position_4 = false;
bool DriverInput::b_select_position_5 = false;
bool DriverInput::b_select_position_6 = false;
bool DriverInput::b_select_position_7 = false;
bool DriverInput::b_select_position_8 = false;
bool DriverInput::b_select_position_9 = false;
bool DriverInput::b_pick_up_ground = false;
bool DriverInput::b_select_position_stow = false;
bool DriverInput::b_select_position_slot = false;
bool DriverInput::b_travel_straight_up = false;
bool DriverInput::b_operating_orientation_selector = false;
bool DriverInput::b_goto_button = false;
bool DriverInput::b_gyro_reset_button;
bool DriverInput::b_manual_override = false;

//MOAR BUTTONS
bool DriverInput::b_deploy_extend = false;
bool DriverInput::b_deploy_retract = false;
bool DriverInput::b_alignment_release = false; 

const float DriverInput::kDeadband = 0.1;

DriverStationEnhancedIO * DriverInput::dseio = NULL;

MultiToggle * DriverInput::m_signal_light_toggle = NULL;

//This needs a button call. Can't find any other code to look at for the same features.

void DriverInput::Init()
{
	dseio = &(DriverStation::GetInstance()->GetEnhancedIO());
	
	if (m_left_stick == NULL) {
		m_left_stick = new Joystick(1);
	}
	
	if (m_right_stick == NULL) {
		m_right_stick = new Joystick(2);
	}
	
	if(m_third_stick == NULL){
		m_third_stick = new Joystick(3);
	}
	
	m_left_speed = m_right_speed = 0;
	b_line_follow = false;
	
	m_signal_light_toggle = new MultiToggle(4);

}

void DriverInput::Process()
{
	m_left_speed = DeadbandJoystick(m_left_stick->GetY());
	m_right_speed = DeadbandJoystick(m_right_stick->GetY());
	
	if (m_left_speed < -.9) {
		m_left_speed = -.9;
	}
	if (m_right_speed < -.9) {
		m_right_speed = -.9;
	}
	b_line_follow = m_right_stick->GetTrigger();
	retsetDIP_button = m_left_stick->GetTrigger(); 
	reloadConfig = m_left_stick->GetRawButton(6)&&m_left_stick->GetRawButton(7)&&m_left_stick->GetRawButton(10)&&m_left_stick->GetRawButton(11);
	
	//m_upper_speed = m_third_stick->GetX();
	//m_lower_speed = m_third_stick->GetY();
	
	
	m_rotate_up = m_third_stick->GetRawButton(6);
	m_rotate_down = m_third_stick->GetRawButton(7);
	m_capture = m_third_stick->GetRawButton(10);
	m_release = m_third_stick->GetRawButton(11);
	
	//This has double !'s so it doesn't come undone.
	//m_toggle_direction = !(dseio->GetDigital(7) != 0);
	
	shuttleSpeed = 0;
	armSpeed = 0;
	if (m_third_stick->GetRawButton(8)) {
		shuttleSpeed =m_third_stick->GetY() * -1; 
	} else if (m_third_stick->GetRawButton(9)) {
		armSpeed = m_third_stick->GetY(); 		
	}
	
	
	//Shuttle Buttons
	b_shuttle_button_1 = m_third_stick->GetRawButton(5);
	b_shuttle_button_2 = m_third_stick->GetRawButton(4);
	b_shuttle_button_3 = m_third_stick->GetRawButton(3);
	
	//Buttons!
	b_arm_button_1 = m_third_stick->GetRawButton(8);
	b_arm_button_2 = m_third_stick->GetRawButton(9);
	
	//Buttons for the deployer... I mean... MOAR BUTTONS!
	b_deploy_extend = m_left_stick->GetRawButton(6);
	b_deploy_retract = m_left_stick->GetRawButton(7);
	b_alignment_release = m_left_stick->GetRawButton(11);
	
	
	//More buttons!
	//These have double !'s, so they don't come undone.
	// That was NOT A HELPFUL COMMENT. >:0
	//TODO: Make all comments humourous.
	b_select_position_1 = (dseio->GetDigital(1) != 0);
	b_select_position_2 = (dseio->GetDigital(2) != 0);
	b_select_position_3 = (dseio->GetDigital(3) != 0);
	b_select_position_4 = (dseio->GetDigital(4) != 0);
	b_select_position_5 = (dseio->GetDigital(5) != 0);
	b_select_position_6 = (dseio->GetDigital(6) != 0);
	b_select_position_7 = (dseio->GetDigital(7) != 0);
	b_select_position_8 = (dseio->GetDigital(8) != 0);
	b_select_position_9 = (dseio->GetDigital(9) != 0);
	b_pick_up_ground = (dseio->GetDigital(10) != 0);
	b_select_position_stow = (dseio->GetDigital(11) != 0);
	b_select_position_slot = (dseio->GetDigital(12) != 0);
	b_travel_straight_up = (dseio->GetDigital(13) != 0);
	b_operating_orientation_selector = (dseio->GetDigital(14) != 0);
	b_goto_button = (dseio->GetDigital(15) != 0);
	b_gyro_reset_button = m_left_stick->GetRawButton(2);
	
	//b_manual_override = (dseio->GetDigital(16));
	
	if (b_goto_button || b_pick_up_ground || b_travel_straight_up) {
		b_manual_override = false; 
	} else if (dseio->GetDigital(11) != 0) {
		b_manual_override = true;
	}
	
	m_signal_light_toggle->Process(m_third_stick->GetRawButton(2));

	/*
	if (m_third_stick->GetTop()) {
		//10011100001111
		
	}*/
}

float DriverInput::GetLeftSpeed()
{
	return m_left_speed;
}

float DriverInput::GetRightSpeed()
{
	return m_right_speed;
}

bool DriverInput::GetLineFollowButton()
{
	return b_line_follow;
}

bool DriverInput::GetPIDTesterButton()
{
	//return retsetDIP_button;
	// DO NOT ENABLE.
	return false;
}

float DriverInput::GetUpperGrabberSpeed()
{
	return m_upper_speed;
}

float DriverInput::GetLowerGrabberSpeed()
{
	return m_lower_speed;
}

Joystick * DriverInput::GetLeftJoy()
{
	return m_left_stick;
}

Joystick * DriverInput::GetRightJoy()
{
	return m_right_stick;
}

Joystick * DriverInput::GetThirdJoy()
{
	return m_third_stick;
}

bool DriverInput::LineToggle()
{	
	return m_toggle_direction;
}

bool DriverInput::ReloadConfig()
{
	return reloadConfig;
}

bool DriverInput::GetShut1()
{
	return b_shuttle_button_1;
}

bool DriverInput::GetShut2()
{
	return b_shuttle_button_2;
}

bool DriverInput::GetShut3()
{
	return b_shuttle_button_3;
}

float DriverInput::GetShuttleSpeed()
{
	return shuttleSpeed;
}

float DriverInput::GetArmSpeed()
{
	//return armSpeed * 45;
	return armSpeed;
}

bool DriverInput::GetArmPosition1()
{
	return b_arm_button_1;
}

bool DriverInput::GetArmPosition2()
{
	return b_arm_button_2;
}

bool DriverInput::GetIO1()
{
	return b_select_position_1;
}

bool DriverInput::GetIO2()
{
	return b_select_position_2;
}

bool DriverInput::GetIO3()
{
	return b_select_position_3;
}

bool DriverInput::GetIO4()
{
	return b_select_position_4;
}

bool DriverInput::GetIO5()
{
	return b_select_position_5;
}

bool DriverInput::GetIO6()
{
	return b_select_position_6;
}

bool DriverInput::GetIO7()
{
	return b_select_position_7;
}

bool DriverInput::GetIO8()
{
	return b_select_position_8;
}

bool DriverInput::GetIO9()
{
	return b_select_position_9;
}

bool DriverInput::GetPickUpG()
{
	return b_pick_up_ground;
}

bool DriverInput::GetSelStow()
{
	return b_select_position_stow;
}

bool DriverInput::GetSelSlot()
{
	return b_select_position_slot;
}

bool DriverInput::GetTravelUp()
{
	return b_travel_straight_up;
}

bool DriverInput::GetSelOperOrient()
{
	return b_operating_orientation_selector;
}

bool DriverInput::GoToButton()
{
	return b_goto_button;
}

bool DriverInput::GetResetGyroButton()
{
	return b_gyro_reset_button;
}

bool DriverInput::GetManualOverride()
{
	return b_manual_override;
}

bool DriverInput::GetDeployExtend()
{
	return b_deploy_extend;
}

bool DriverInput::GetDeployRetract()
{
	return b_deploy_retract;
}

bool DriverInput::GetAlignmentRelease()
{
	return b_alignment_release;
}