Autonomous.cpp

#include "Autonomous.h"

#include "LineManager.h"
/*#include "Tubehanger.h"*/
#include "Hardware.h"
#include "PoseControl.h"
#include "Everything.h"
#include "DriveControl.h"


//Stuff for the state machine goes here. Maybe.

int Autonomous::state = 0;
Timer * Autonomous::auto_timer = new Timer();
Timer * Autonomous::auto_eject_delay = new Timer();
float Autonomous::m_left_speed = 0.0;
float Autonomous::m_right_speed = 0.0;
Gyro * Autonomous::m_gyro = NULL;
float Autonomous::m_straight_heading = 0;

void Autonomous::Init()
{
	state = 0;
	m_gyro = Hardware::GetGyro();
	DriverInput::ForceManualOff();
}

void Autonomous::Process()
{
	printf("STATE = %d\n", state);
	Encoder * left_encoder = Hardware::GetLeftEncoder();
	Encoder * right_encoder = Hardware::GetRightEncoder();
	int pose = 0;
	MotionProfile autoSpeedProfile;
	autoSpeedProfile.AddPoint(0,0);
	autoSpeedProfile.AddPoint(.5,1);
	autoSpeedProfile.AddPoint(.9,0.2);
	autoSpeedProfile.AddPoint(1,0);
	// This didn't affect the code. :D
	switch(state)
	{
	case 0:
		state = 1;
		left_encoder->Reset();
		right_encoder->Reset();
		PoseControl::GoToPose(PoseControl::kScore);
		m_straight_heading = SensorBox::Heading();
		printf("Read heading, navigating fixed on %2.2f\n", m_straight_heading);
		break;
	case 1:
		state = 2;
		break;
	case 2: //This doesn't make sense. Shouldn't the arm jump to it's position before it drives forward?
		
		LineManager::Process();
		if (RobotConfiguration::GetEnableLineFollowing()) {
			m_left_speed = -1 * LineManager::GetLeftSpeed();
			m_right_speed = -1 * LineManager::GetRightSpeed();
			printf("LineManager = %d\n", LineManager::GetSensorStatus());
		} else {
			// We shouldn't use line following at all.
			
			printf("Heading = %2.2f degrees, off of %2.2f degrees\n", SensorBox::Heading(), m_straight_heading);
			float diff = (m_straight_heading - SensorBox::Heading());
			printf("Error is %2.2f degrees\n", diff);
			float correction =  diff * RobotConfiguration::GetDriveStraightP();
			printf("Correction is %2.2f%%\n", correction*100);
			/*
			DriveControl::ArcadeToTank(correction, 
					RobotConfiguration::GetNormalAutonomousSpeed(), 
					m_left_speed, m_right_speed);
			*/
			
			// We're just trying drive straight for now.
			m_left_speed = m_right_speed = RobotConfiguration::GetNormalAutonomousSpeed();
		}
		
		printf("Distance = %2.2f\n", right_encoder->GetDistance());
		if (right_encoder->GetDistance() >  RobotConfiguration::GetAutoDriveDistance()) {
		//if (right_encoder->GetDistance() >  RobotConfiguration::GetAutoDriveDistance() { //|| LineManager::AtTheT()) {
			m_left_speed = 0;
			m_right_speed = 0;
			state = 3;
		}
		break;
	case 3:
		if (Shuttle::DoneWithMove() && RobotArm::DoneWithMove()) {
			state = 8;
		}
		break;
	case 4:
		left_encoder->Reset();
		right_encoder->Reset();
		// We are now at the T. Begin to back up.
		m_left_speed = m_right_speed = RobotConfiguration::GetBackupSpeed();
		
		state = 5;
		break;
	case 5:
		if (left_encoder->GetDistance() >= RobotConfiguration::GetBackupDistance()) {
			// Backup complete.
			printf("Backed up. Distance = %2.2f inches\n", left_encoder->GetDistance());
			state = 6;
		}
		break;
	case 6:
		m_left_speed = m_right_speed = 0;
		// Round just in case.
		pose = (int)(RobotConfiguration::GetHangingPose() + .5);
		printf("Hanging Pose selected: %d\n", pose);
		printf("Ignoring, because pose is constant.\n");
		pose = PoseControl::kScore;
		PoseControl::GoToPose(pose);
		printf("Shuttle state: %d\n", Shuttle::GetState());
		// Force re-evaluation of pose.
		//PoseControl::Process();
		// Wait for the shuttle to come back to a valid state.
		state = 15;
	break;
	case 15:
		if (Shuttle::DoneWithMove()) {
			state = 7;
			// Also, start rotating
			//RobotGrabber::Rotate();
			auto_timer->Reset();
			auto_timer->Start();
		}
		break;
	case 7:
		bool rotate_done = (auto_timer->Get() > RobotConfiguration::GetRotateTime());
		if (rotate_done) {
			RobotGrabber::Stop();
			// Important: Leave the timer running or the below conditions
			// won't synchronize well.
		}
		if (Shuttle::DoneWithMove() && RobotArm::DoneWithMove() && rotate_done) {
			printf("Movement complete.\n");
			auto_timer->Stop();
			auto_timer->Reset();
			// Done with pose.
			PoseControl::GoToPose(0);
			state = 14;
			break;
		}
	break;
	case 14:
		m_left_speed = m_right_speed = -1 * RobotConfiguration::GetBackupSpeed();
		float dist = left_encoder->GetDistance();
		printf("Distance = %2.2f\n", dist);
		float score_dist = RobotConfiguration::GetScoreDistance(); 
		
		if (dist <= score_dist)
		{
			printf("Up to scoring position. (%2.2f <= %2.2f) Continuing.\n", 
					dist, score_dist);
			m_left_speed = m_right_speed = 0;
			state = 8;
		}
		
	break;
	case 8:
		m_left_speed = m_right_speed = 0;
		
		PoseControl::GoToPose(PoseControl::kScore2);
		auto_eject_delay->Reset();
		auto_eject_delay->Start();
		
		auto_timer->Reset();
		auto_timer->Start();
		state = 16;
	break;
	case 16:
		if (Shuttle::GetPosition() <= -2) {
			auto_timer->Reset();
			auto_timer->Start();
			RobotGrabber::Eject();
			state = 9;
		}
		break;
	case 9:
		if (auto_timer->HasPeriodPassed(RobotConfiguration::GetEjectTime())) {
			RobotGrabber::Stop();
			state = 10;
			PoseControl::GoToPose(0);
		}
		
		
	break;
	case 10:
		// Back up.
		/*
		Encoder * left_encoder = Hardware::GetLeftEncoder();
		Encoder * right_encoder = Hardware::GetRightEncoder();
		*/			
		left_encoder->Reset();
		right_encoder->Reset();
		// We are now at the T. Begin to back up.
		m_left_speed = m_right_speed = RobotConfiguration::GetBackupSpeed();
		
		state = 11;
	break;
	case 11:
		//Encoder * left_encoder = Hardware::GetLeftEncoder();
		printf("Entering state 11\n");
		printf("Distance = %2.2f inches\n", left_encoder->GetDistance());
		if (left_encoder->GetDistance() >= RobotConfiguration::GetFinalBackupDistance()) {
			printf("Backed up. Distance = %2.2f inches\n", left_encoder->GetDistance());
			// Backup complete.
			state = 12;
		}
		break;
	case 12:
		m_left_speed = m_right_speed = 0;
		state = 13; // There IS NO state 13. <Dramatic Music>
		break;
	
	default:
		break;
	}
	
	printf("AUTO STATE: %d\n", state);
	//DPRINTF("AUTO STATE: %d\n", state);
	
}

float Autonomous::GetLeftSpeed()
{
	return -1 * m_left_speed;
}

float Autonomous::GetRightSpeed()
{
	return -1 * m_right_speed;
}