PoseControl.cpp

#include "PoseControl.h"
#include "WPILib.h"
#include "Hardware.h"
#include "DriverInput.h"
#include "Everything.h"
#include "RobotArm.h"
#include "Shuttle.h"

//Position Buttons
bool PoseControl::Position_1;
bool PoseControl::Position_2;
bool PoseControl::Position_3;
bool PoseControl::Position_4;
bool PoseControl::Position_5;
bool PoseControl::Position_6;
bool PoseControl::Position_7;
bool PoseControl::Position_8;
bool PoseControl::Position_9;

//Misc Buttons
bool PoseControl::Pos_Ground;
bool PoseControl::Pos_Stow;
bool PoseControl::Pos_Slot;
bool PoseControl::Trav_Up;
bool PoseControl::Oper_Orient;

//Execute button
bool PoseControl::Go_To;

//Binary Variables
int PoseControl::BinInput;
int PoseControl::Config = 0;

//Position Variables
float PoseControl::ArmVar;
int PoseControl::ShutVar;
int PoseControl::DriveVar;
int PoseControl::PoseCommand = 0;
bool PoseControl::InvalidState = false;

int PoseControl::m_state;
float PoseControl::m_current_arm;
int PoseControl::m_current_shuttle;

#define MULTI_STEP_POSE 0

void PoseControl::Init() {
	Position_1 = 0;
	Position_2 = 0;
	Position_3 = 0;
	Position_4 = 0;
	Position_5 = 0;
	Position_6 = 0;
	Position_7 = 0;
	Position_8 = 0;
	Position_9 = 0;
	Pos_Ground = 0;
	Pos_Stow = 0;
	Pos_Slot = 0;
	Trav_Up = 0;
	Oper_Orient = 0;
	Go_To = 0;
	BinInput = 0;
	ArmVar = 0;
	ShutVar = 0;
	DriveVar = 0;

	m_state = 0;
	m_current_arm = 0;
	m_current_shuttle = 0;

}

void PoseControl::Process() {
	//Gets the buttons from the DriverInput class and stores them in Bools for use in our
	//switch statements
	PoseControl::Position_1 = DriverInput::GetIO1();
	PoseControl::Position_2 = DriverInput::GetIO2();
	PoseControl::Position_3 = DriverInput::GetIO3();
	PoseControl::Position_4 = DriverInput::GetIO4();
	PoseControl::Position_5 = DriverInput::GetIO5();
	PoseControl::Position_6 = DriverInput::GetIO6();
	PoseControl::Position_7 = DriverInput::GetIO7();
	PoseControl::Position_8 = DriverInput::GetIO8();
	PoseControl::Position_9 = DriverInput::GetIO9();
	PoseControl::Pos_Ground = DriverInput::GetPickUpG();
	PoseControl::Pos_Stow = DriverInput::GetSelStow();
	PoseControl::Pos_Slot = DriverInput::GetSelSlot();
	PoseControl::Trav_Up = DriverInput::GetTravelUp();
	PoseControl::Oper_Orient = DriverInput::GetSelOperOrient();

	PoseControl::Go_To = DriverInput::GoToButton();

#if MULTI_STEP_POSE
	switch (m_state) {
	case 0:
		ConfigurePose();
		if (BinInput != 0) { // Wait for command.
			m_state = 1;
		}
		break;
	case 1: // We are moving the arm to up.
		m_current_arm = 0;
		m_state = 4;
		break;
	case 4:
		if (RobotArm::DoneWithMove()) {
			m_state = 2;
		}
		break;
	case 2: // now the arm is done with up move. Now we is moveing teh shuttle.
		m_current_shuttle = ShutVar;
		m_state = 5;
		break;
	case 5:
		if (Shuttle::DoneWithMove()) {
			m_state = 3;
		}
		break;
	case 3: // Done with shuttle move. Now move arm to final position.
		m_current_arm = ArmVar;
		m_state = 6;
	break;
	case 6:
		if (RobotArm::DoneWithMove()) {
			m_state = 0;
		}
		break;
	}
#else
	ConfigurePose();
#endif
}

bool PoseControl::GetShut1() {
	int thing = ShutVar;
#if MULTI_STEP_POSE
	thing = m_current_shuttle;
#endif
	
	if (thing == 3) {
		return true;
	} else {
		return false;
	}
}

bool PoseControl::GetShut2() {
	int thing = ShutVar;
	#if MULTI_STEP_POSE
		thing = m_current_shuttle;
	#endif
		
	if (thing == 1) {
		return true;
	} else {
		return false;
	}
}

bool PoseControl::GetShut3() {
	int thing = ShutVar;
	#if MULTI_STEP_POSE
		thing = m_current_shuttle;
	#endif
	if (thing == 2) {
		return true;
	} else {
		return false;
	}
}

float PoseControl::GetArmPosition() {
	float thing = ArmVar; 
#if MULTI_STEP_POSE
		thing = m_current_arm;
#endif
	return thing;
}
/*Are these just for the logger?
 bool PoseControl::GetBinVar()
 {
 return PoseControl::BinInput;
 }*/

void PoseControl::GoToPose(int pose) {
	PoseCommand = pose;

}

void PoseControl::ConfigurePose()
{
	if (!PoseControl::Go_To && PoseCommand != 0) {

			if (PoseCommand == 9999) {
				Config = 9999;
			} else if (PoseCommand == 9998) {
				Config = 9998;
			} else {
				Config = 1 << PoseCommand; // Bit shift! Be careful how you pronounce that.
			}
			BinInput = Config;
			DPRINTF("PoseCommand set to %d, pose is effectively %d\n", PoseCommand, BinInput);

		} else {

			//Convert the above for inputting the buttons into a binary variable
			Config = Trav_Up*8192 + Pos_Slot*4096 + Pos_Stow*2048 + Pos_Ground*1024
					+ Position_9*512 + Position_8*256 + Position_7*128 + Position_6
					*64 + Position_5*32 + Position_4*16+ Position_3*8 + Position_2
					*4 + Position_1*2 + Oper_Orient;
			//Declares whether or not it is ready to execute the combo
			BinInput = Config * Go_To;

			if (Trav_Up) {
				BinInput = 8192;
			//} else if (Pos_Stow) {
			//	BinInput=2048;
			} else if (Pos_Ground) {
				BinInput=1024;
			}

			if (Go_To) {
				// Override previous auto command.
				PoseCommand = 0;
			}
		}
		InvalidState = false;

		//This is the switch statement for the Binary Variable
		switch (BinInput) {
		case 0:
			//NULL
			ArmVar = kNothing;
			ShutVar = 0;
			DriveVar = 0;
			break;
		case 1:
			//NULL
			ArmVar = kNothing;
			ShutVar = 0;
			DriveVar = 0;
			break;
		case 2:
			//7, 3, 1
			ArmVar = RobotConfiguration::GetArmPosition(7);
			ShutVar = 3;
			DriveVar = 1;
			break;
		case 3:
			//12, 3, 1
			ArmVar = RobotConfiguration::GetArmPosition(12);
			ShutVar = 3;
			DriveVar = 1;
			break;
		case 4:
			//8, 3, 2
			ArmVar = RobotConfiguration::GetArmPosition(8);
			ShutVar = 3;
			DriveVar = 2;
			break;
		case 5:
			//11, 3, 2
			ArmVar = RobotConfiguration::GetArmPosition(11);
			ShutVar = 3;
			DriveVar = 2;
			break;
		case 8:
			//7, 3, 3
			ArmVar = RobotConfiguration::GetArmPosition(7);
			ShutVar = 3;
			DriveVar = 3;
			break;
		case 9:
			//12, 3, 3
			ArmVar = RobotConfiguration::GetArmPosition(12);
			ShutVar = 3;
			DriveVar = 3;
			break;
		case 16:
			//5, 3, 1
			ArmVar = RobotConfiguration::GetArmPosition(5);
			ShutVar = 1;
			DriveVar = 1;
			break;
		case 17:
			//14, 3, 1
			ArmVar = RobotConfiguration::GetArmPosition(14);
			ShutVar = 1;
			DriveVar = 1;
			break;
		case 32:
			//6, 3, 2
			ArmVar = RobotConfiguration::GetArmPosition(6);
			ShutVar = 1;
			DriveVar = 2;
			break;
		case 33:
			//13, 3, 2
			ArmVar = RobotConfiguration::GetArmPosition(13);
			ShutVar = 1;
			DriveVar = 2;
			break;
		case 64:
			//5, 3, 3
			ArmVar = RobotConfiguration::GetArmPosition(5);
			ShutVar = 1;
			DriveVar = 3;
			break;
		case 65:
			//14, 3, 3
			ArmVar = RobotConfiguration::GetArmPosition(14);
			ShutVar = 1;
			DriveVar = 3;
			break;
		case 128:
			//3, 1, 1
			ArmVar = RobotConfiguration::GetArmPosition(3);
			ShutVar = 1;
			DriveVar = 1;
			break;
		case 129:
			//16, 1, 1
			ArmVar = RobotConfiguration::GetArmPosition(16);
			ShutVar = 1;
			DriveVar = 1;
			break;
		case 256:
			//4, 1, 2
			ArmVar = RobotConfiguration::GetArmPosition(4);
			ShutVar = 1;
			DriveVar = 2;
			break;
		case 257:
			//15, 1, 2
			ArmVar = RobotConfiguration::GetArmPosition(15);
			ShutVar = 1;
			DriveVar = 2;
			break;
		case 512:
			//3, 1, 3
			ArmVar = RobotConfiguration::GetArmPosition(3);
			ShutVar = 1;
			DriveVar = 3;
			break;
		case 513:
			//16, 1, 3
			ArmVar = RobotConfiguration::GetArmPosition(16);
			ShutVar = 1;
			DriveVar = 3;
			break;
		case 1024: // Pick up from ground
		case 1025:
			//1, 1, NULL
			ArmVar = RobotConfiguration::GetArmPosition(1);
			ShutVar = 1;
			DriveVar = 0;
			break;
			/*case 1025:
			 //NULL
			 ArmVar = kNothing;
			 ShutVar = 0;
			 DriveVar = 0;
			 break; */
		case 2048:
			//NULL
			ArmVar = kNothing;
			ShutVar = 0;
			DriveVar = 0;
			break;
		case 2049:
			//18, 2, NULL
			ArmVar = RobotConfiguration::GetArmPosition(18);
			ShutVar = 2;
			DriveVar = 0;
			break;
		case 4096:
			//2, 1, NULL
			ArmVar = RobotConfiguration::GetArmPosition(2);
			ShutVar = 1;
			DriveVar = 0;
			break;
		case 4097:
			//17, 1, NULL
			ArmVar = RobotConfiguration::GetArmPosition(17);
			ShutVar = 1;
			DriveVar = 0;
			break;
		case 8192:
			//9, 1, NULL
			ArmVar = RobotConfiguration::GetArmPosition(9);
			ShutVar = 1;
			DriveVar = 0;
			break;
		case 8193:
			//10, 1, NULL
			ArmVar = RobotConfiguration::GetArmPosition(10);
			ShutVar = 1;
			DriveVar = 0;
			break;
		case kScore:
			ArmVar = RobotConfiguration::GetArmPosition(0);
			ShutVar = 3;
			DriveVar = 1;
			break;
		case kScore2:
			ArmVar = RobotConfiguration::GetArmPosition(0);
			ShutVar = 2;
			DriveVar = 1;
			break;
		default:

			InvalidState = true;
		}
		
}