Shuttle.cpp

#include "Shuttle.h"
#include "DriverInput.h"
#include "RAGearToothSensor.h"
#include "PIDController.h"
#include "WPILib.h"
#include "DebugUtil.h"
#include "RobotConfiguration.h"
#include "PoseControl.h"

#include <cmath>

CANJaguar * Shuttle::m_motor= NULL;
RAGearToothSensor * Shuttle::m_gt= NULL;
PIDController * Shuttle::m_pid= NULL;
Timer * Shuttle::m_motion_timer= NULL;
MotionProfile * Shuttle::m_motion_profile= NULL;
DigitalInput * Shuttle::m_home_sensor= NULL;
float Shuttle::m_start_position = 0;
float Shuttle::m_end_position = 0;
float Shuttle::m_time_to_move = 0;

int Shuttle::m_state = 0;

bool Shuttle::m_manual_override_last;

int Shuttle::ShutPos = 0; //This stores which position the shuttle is s'posed to go to.

#define SHUTTLE_STEP 0
#define SHUTTLE_AUTO 1

void Shuttle::Init() {
	//Stuff happens here
	ShutPos = 0;
	DPRINTF("Getting shuttle motor.\n", NULL);
	m_motor = Hardware::GetShuttleMotor();
	DPRINTF("Done.\n", NULL);
	DPRINTF("Getting tooth sensor.\n", NULL);
	m_gt = Hardware::GetShuttleGearToothSensor();
	DPRINTF("Done.\n", NULL);
	DPRINTF("Getting timer.\n", NULL);
	m_motion_timer = new Timer();
	DPRINTF("Done.\n", NULL);
	DPRINTF("Getting home sensor...", NULL);
	m_home_sensor = Hardware::GetShuttleHomeSensor();
	DPRINTF("Done.\n", NULL);

	m_pid = new PIDController(
			RobotConfiguration::GetShuttleP(),
			RobotConfiguration::GetShuttleI(),
			RobotConfiguration::GetShuttleD(),
			//4,.02,0, // PID for position controll
			m_gt, // PIDSource (position)
			m_motor, // PIDOutput (speed)
			0.005 // Period ??
	);

	//m_pid->SetInputRange(-18, 10); // Real limits are closer to [-11,11]
	m_pid->SetInputRange(RobotConfiguration::GetShuttleInputRangeMin(),RobotConfiguration::GetShuttleInputRangeMax()); // Real limits are closer to [-11,11]
	m_pid->SetOutputRange(RobotConfiguration::GetShuttleOutputMin(),
			RobotConfiguration::GetShuttleOutputMax());
	m_pid->SetTolerance(10); // If we're within 5%, we're "on target"
	//m_pid->SetTolerance(RobotConfiguration::GetShuttlePIDTolerance()); // If we're within 5%, we're "on target"
	m_pid->SetSetpoint(0);
	m_pid->Enable();

	m_manual_override_last = false;
	//m_pid->Disable();

#if SHUTTLE_STEP
	float eps = .01;
	m_motion_profile = new MotionProfile();

	m_motion_profile->AddPoint(0, 0); // 0
	m_motion_profile->AddPoint(1-eps, 0); // 1
	m_motion_profile->AddPoint(1, 1); // 2
	m_motion_profile->AddPoint(5-eps, 1); // 3
	m_motion_profile->AddPoint(5, 0); // 4
	m_motion_profile->AddPoint(9-eps, 0); // 5
	m_motion_profile->AddPoint(9, 1); // 6
	m_motion_profile->AddPoint(13-eps, 1); // 7
	m_motion_profile->AddPoint(13, 0); // 8
	m_motion_profile->AddPoint(14,0); // 9
	m_motion_profile->SetTimeScale(1);
	m_motion_profile->SetValueScale(8);

#endif
}

bool Shuttle::IsHoming() 
{
	if (m_state == 4) { 
		return false; 
	}
	
	return (m_state <= 8);
	
}

void Shuttle::Process() {

	bool should_override = DriverInput::GetManualOverride();

	if (m_manual_override_last != should_override) {
		if (should_override) {
			m_pid->Disable(); // Disable PID for now.
			m_state = 42;
		} else {
			m_state = 4;
			m_pid->Enable();
		}

}

	m_manual_override_last = should_override;

	float shuttle_up_speed = RobotConfiguration::GetShuttleUpSpeed();
	if (should_override) {
		m_motor->Set(DriverInput::GetShuttleSpeed());
	} else {

		switch (m_state) {
#if SHUTTLE_AUTO
		case 0: // We are initializing, start homing.
			m_pid->Disable(); // Disable control so we can run the shuttle
			// ourselves.
			m_state = 1;
			break;
		case 1: // Keep going until "home"
			m_motor->Set(shuttle_up_speed);

			if (m_home_sensor->Get()) {
				m_state = 2;
			} else if (!m_motor->GetForwardLimitOK()) {
				// Dang it, somebody forgot to put the shuttle below
				// home before trying to home it. Start the alternate
				// sequence to correct.
				m_state = 5;
			}
			break;
		case 2: // Running slower until "not home"
			m_motor->Set(shuttle_up_speed - .05);

			if (!m_home_sensor->Get()) {
				m_state = 3;
			}
			break;
		case 3: // We're home. Set this as zero.
			m_motor->Set(0);
			m_gt->Reset();
			/*m_pid->Enable();
			 m_pid->SetSetpoint(0); */
			m_state = 4;
			break;
		case 4:
			// NOP (waiting for scheduled move)
			if (DriverInput::GetThirdJoy()->GetTrigger()) {
				m_state = 0;
			} else if (DriverInput::GetShut1() || PoseControl::GetShut1()) {
				// Got to the top position
				ScheduleMovement(RobotConfiguration::GetShuttleHigh(),
						RobotConfiguration::GetShuttleMoveTime());
			} else if (DriverInput::GetShut2() || PoseControl::GetShut2()) {
				//Go to lower position
				ScheduleMovement(RobotConfiguration::GetShuttleLow(),
						RobotConfiguration::GetShuttleMoveTime());
			} else if (DriverInput::GetShut3() || PoseControl::GetShut3()) {
				//Go to middle position
				// TODO: This position is less important than the other two
				ScheduleMovement(RobotConfiguration::GetShuttleMid(),
						RobotConfiguration::GetShuttleMoveTime());
			} else {
				m_motor->Set(DriverInput::GetShuttleSpeed());
			}

			break;
		case 5:
			// We hit the limit switch in 1, reverse and 
			// go down until home switch.
			m_motor->Set(-.2);

			if (m_home_sensor->Get()) {
				m_state = 6;
			} else if (!m_motor->GetReverseLimitOK()) {
				// WHOA! We blew straight past the home sensor 
				// and hit the lower limit.
				// But now we're below home, so go back to state 1.

				// ... After a pause. :)
				m_motion_timer->Reset();
				m_motion_timer->Start();
				m_state = 8;
			}
			break;
		case 6:
			m_motor->Set(-.15);

			if (!m_home_sensor->Get()) { // We're past the home sensor.
				m_state = 7; // Keep going for a while.
				m_motion_timer->Start();
			}
			break;
		case 7:
			if (m_motion_timer->HasPeriodPassed(.2)) { // A while has passed. We're below the home sensor. Now start
				// the whole darn thing over as if we had been correctly
				// positioned in the first place.

				// After a pause.

				m_state = 8;
				m_motion_timer->Reset();
				//m_motion_timer->Reset();
			}
			m_motor->Set(-.15);
			break;
		case 8:
			m_motor->Set(0);
			if (m_motion_timer->HasPeriodPassed(.5)) {
				m_state = 1;
				m_motion_timer->Reset();
				m_motion_timer->Stop();
			}
			break;
		case 9:
			// We have been ordered to move. Set up everything.
			m_motion_timer->Reset();
			m_motion_timer->Start();

			m_pid->Enable();
			m_pid->SetSetpoint(GetPosition());

			m_state = 10;
			break;
		case 10:
			// We are still moving. Calculate next setpoint, give
			// order, and check to see if we're there.

			// TODO: Handle the case where we have reached a limit.
			float time = m_motion_timer->Get();
			if ((time >= m_time_to_move) && (m_pid->OnTarget())) {
				m_state = 11;
			/*} else if ( ! (m_motor->GetForwardLimitOK()
					&& m_motor->GetReverseLimitOK())) {
				m_state = 11; */
			} else {
				// We're still moving.
				// Get an updated waypoint.
				//float s = m_start_position + m_motion_profile->GetInterpolatedValue(time);
				float s = m_start_position
						+ m_motion_profile->GetCosineInterpolatedValue(time);
				m_pid->SetSetpoint(s);

			}

			break;

		case 11:
			// We are on target. Everything should be shut down.
			// TODO: Does anything need to happen here?
			// No, it doesn't.
			m_pid->Disable();
			m_motion_timer->Reset();
			m_motion_timer->Stop();
			m_state = 4;

			break;
		default:
			DPRINTF("Unexpected state: %d\n", m_state);
#elif SHUTTLE_STEP
			case 0:
			if (DriverInput::GetThirdJoy()->GetTrigger()) {
				m_motion_timer->Reset();
				m_motion_timer->Start();
				m_state = 1;
			}

			break;
			case 1:
			float time = m_motion_timer->Get();
			float setpoint = m_motion_profile->GetInterpolatedValue(time);

			m_pid->SetSetpoint(setpoint);

			printf("Point: (%2.2f, %2.2f)\tPosition: %2.2f Error: %2.2f\n",time, GetCommandedPosition(),
					m_gt->PIDGet(), m_pid->GetError());

			if (m_motion_timer->HasPeriodPassed(15))
			{
				m_state = 2;
			}
			break;
			case 2:
			m_pid->SetSetpoint(0);
			m_state = 0;
			break;
#else
			default:
			ManualMode();
#endif

		}
	}
	//DPRINTF("Shuttle State: %d\n", m_state);
	//DPRINTF("ST: %d\tPOS: %2.2f inches\n", m_state, GetPosition());
	/*
	 if (DriverInput::GetPIDTesterButton()) {
	 m_state = 0;
	 m_motion_timer->Reset();
	 m_motion_timer->Stop();
	 }
	 */
}

void Shuttle::HomingSequence() {
	//Used in Autonomous. Begins traveling the shuttle upwards.
	//When the shuttle trips a home sensor, the encoder count should be reset on the 
	//trailing edge of the home sensor. What does that mean?


}

void Shuttle::ManualMode() {
	//Used for testing'n'crap. Two buttons on the joysticks will be used, one for manual
	//up, and another for manual down. Limit switches will be honored.
	//m_motor->Set(DriverInput::GetShuttleSpeed());

	// Update: This will drive the shuttle up and down based on joystick coordinates.
	// Limit switches should still be honored.
	//m_pid->SetSetpoint( DriverInput::GetShuttleSpeed() );
	m_motor->Set(DriverInput::GetShuttleSpeed());
}

void Shuttle::ScheduleMovement(float end_position, float time_to_move) {
	if (m_state != 4) {
		DPRINTF("Invalid! State = %d, not 4 as required.\n", m_state);
	} else {

		// Set up everything.
		m_end_position = end_position;
		m_start_position = GetPosition();
		m_time_to_move = time_to_move;

		if (m_motion_profile != NULL) {
			delete m_motion_profile;
			m_motion_profile = NULL;
		}
		DPRINTF("Moving from %2.2f to %2.2f over %2.2 seconds.\n", m_start_position, m_end_position, m_time_to_move);
		m_motion_profile = new MotionProfile();

		// Might have better shape later.
		// For now, schedule a linear move.
		m_motion_profile->AddPoint(0, 0);

		if (m_end_position < m_start_position) {
			m_motion_profile->AddPoint(1, -1);
		} else {
			m_motion_profile->AddPoint(1, 1);
		}

		m_motion_profile->SetTimeScale(time_to_move);

		float distance =:: fabs(m_end_position - m_start_position);
		m_motion_profile->SetValueScale(distance);

		m_state = 9; // Move to set up state.
			}
		}