Climber.h

#ifndef CLIMBER_H__
#define CLIMBER_H__
#include "Input.h"
#include "CANJaguar.h"
#include "KickerReleaser.h"
#include "WPILib.h"
#include "LimitSwitch.h"
#include "Accelerometer.h"

#include "RedAlertJaguar.h"

#define MANUAL_CLIMB_POWER (1.0)
#define MANUAL_WENCH_POWER (0.4)
#define ARM_STOP_POSITION (-.37)

// OVER HERE! THIS IS THE NUMBER OF REVOLUTIONS TO CLIMB!
// CHANGE ME ON SCORPION.
#define kWenchRevs         (13.75		)
#define CLIMB_POWER        (1.0)

union ClimberStateMachine_Tag {
	
	char ClimberStateVariable;
	struct {
		unsigned Bit7:1;
		unsigned Bit6:1;
		unsigned Bit5:1;
		unsigned Bit4:1;
		unsigned Bit3:1;
		unsigned Bit2:1;
		unsigned Bit1:1;
		unsigned Bit0:1;
	    } Bits;
};

class Climber
{
	public:
		Climber();
		void ClimberInit(Input * in);
		void LevelZero();
		void LevelOne();
		void LevelTwo();
		void LevelThree();
		void LevelFour();
		void LevelFive();
		void Climb();
		void Extend();
		bool Latched();
		void Release();
		void GetData();
		void ClimberStateMachine();
		void ManualMode();
		bool Process();
		int GetState();
		
		inline CANJaguar * GetWenchMotor() { return LiftyJaguar; }
		inline CANJaguar * GetArmMotor() { return NULL; }
		
		inline KickerReleaser * GetReleaser() { return the_deploy_valve; } 
		
		float GetArmOutputCurrent();
		float GetArmEncoder();
		
		float GetWenchOutputCurrent();
		float GetWenchEncoder();
		
		float GetXAccel();
		float GetYAccel();
		float GetZAccel();
	private:
		bool processesing;
		bool CheckCurrentDropped;
		int counter;
		float position;
		
		Input *input;
		LimitSwitch * ClimbSwitch;
		Accelerometer *accel_x;
		Accelerometer *accel_y;
		Accelerometer *accel_z;
		
		KickerReleaser * the_deploy_valve;
		//static const float kManualArmPower = 0.25;
		//ClimberReleaser1 *releaser;
		//KickerReleaser *releaser_stage1;
		//KickerReleaser *releaser_stage2;
		//CANJaguar * ArmJaguar;
		CANJaguar * LiftyJaguar; /*
		RedAlertJaguar * ArmJaguar; 
		RedAlertJaguar * LiftyJaguar; */
		
		//Encoder   * ArmEncoder;
		union ClimberStateMachine_Tag ClimberSM;
};
#define ClimberState ClimberSM.ClimberStateVariable


#endif