Shuttle.h

#ifndef SHUTTLE_H__
#define SHUTTLE_H__

#include "DriverInput.h"
#include "Hardware.h"
#include "RAGearToothSensor.h"
#include "PIDController.h"
#include "MotionProfile.h"
#include "Timer.h"

class Shuttle
{

	static int ShutPos;
public:
	//Actual position modes
	static void HomingSequence();
	static void ManualMode();
	
	//Stuff
	static void Init();
	static void Process();
	
	static float GetCommandedPosition()
	{
		if (m_pid != NULL) { return m_pid->GetSetpoint(); }
		else { return m_motor->Get(); }
	}
	
	static float GetPosition()
	{
		if (m_gt != NULL) { return m_gt->GetDistance(); }
		else { return 0; }
	}
	
	static int GetState()
	{
		return m_state;
	}
	
	static bool DoneWithMove() { return GetState() == 4; }
	static bool IsHoming();
	
	static void Reset() {
		m_pid->Disable();
		m_state = 4;
		m_motion_timer->Stop();
		m_motion_timer->Reset();
		/*
		if (m_motion_profile != NULL) {
			delete m_motion_profile;
		} */
	}
		
private:
	static CANJaguar * m_motor;
	static RAGearToothSensor * m_gt;
	static DigitalInput * m_home_sensor;
	static PIDController * m_pid;
	
	static Timer * m_motion_timer;
	static MotionProfile * m_motion_profile;
	
	static float m_start_position;
	static float m_end_position;
	static float m_time_to_move;
	static int m_state;
	static bool m_manual_override_last;
	
	static void ScheduleMovement(float end_position, float time_to_move);
	
	
	Shuttle() {} ;
};

#endif