RobotArm.h

#ifndef ROBOT_ARM_H__
#define ROBOT_ARM_H__

#include "CANJaguar.h"
#include "Potentiometer.h"
#include "MotionProfile.h"

class RobotArm
{
	static int ArmState;
public:
	static void Init();
	static void Process();
	static void RobotArm::ScheduleMovement(float, float);
	static float GetPosition(){ return (m_pot == NULL) ? 2.5 : m_pot->PIDGet(); }
	static float GetCommandedPosition() { 
		return (m_pid == NULL ? 2.5 : m_pid->GetSetpoint()); 
	}
	
	static int GetState() { 
		return ArmState;
	}
	
	static bool DoneWithMove() { return m_pid->OnTarget(); } 

	

private:
	RobotArm() { } // This is a singleton.
	
	static CANJaguar * m_motor;
	static Potentiometer * m_pot;
	
	static PIDController * m_pid;
	static Timer * m_motion_timer;
	
	static float m_start_position;
	static float m_end_position;
	static float m_time_to_move;
	static bool m_manual_override_last;
	
	static MotionProfile * m_motion_profile;
	
	static float CalculateNeutralTorque(float angle);
	
	
};

#endif