Autonomous.cpp

#include "CodeLevel.h"
#include "Autonomous.h"
#include <cstdlib>

//Yes, I know this code sucks.
//You can call me an idiot later.

// Implement the code level once we have a better idea on how we want to go about doing that.

Autonomous::Autonomous()
	: AutoKicker(NULL), AutoDrive(NULL) {
	AutoState = 0;
	iteration = 1; 
	distance = 1; // Find the value to stick in here
	heading = 0;
	wantedTurn = 0;
	 			   
}

void Process() {
	int level = GetLevel();
		
	switch (level) {
	case 0:
		break;
	case 1:
		break;
	case 2:
		break;
	case 3:
		break;
	case 4:
		break;
	case 5:
		break;
	default:
		break;
	}
}

void Autonomous::SetKicker(AutonomousKicker *k) {
	AutoKicker = k;
}

void Autonomous::SetDriveTrain(DriveTrain *dt) {
	AutoDrive = dt;
}

void Autonomous::AutonomousStateMachine() {
	
	static bool has_a_ball = false;
	switch (AutoState) {
	case 0:
		//Start
		AutoState = 10;
		AutoDrive->ResetTurn();
		AutoDrive->ResetHeading();
		break;
	case 10:
		//Drives forward a distance that was set by the current iteration of the forward/kick that is being run
		AutoDrive->Forward(distance);
		
		AutoState = 15;
		break;
	case 15:
		//Checks to see if it has reached its destination.
		//We'll need some form of "GetPosition" code. Which doesn't exist. Or anything similar to that.
		//For testing purpose, assume it has.
		AutoDrive->Forward(distance);
		if (AutoKicker->BallCaptured()) {
			has_a_ball = true;
			AutoState = 20;
		}
		if (AutoDrive->DoneMoving()) {
			AutoDrive->Forward(0); // Reset drive
			AutoState = 20;
		}
		break;
	case 17:
		AutoDrive->StraightForward(-.3);
		break;
	case 20:
		//Check to see if ball is in possesion
		//bool possession = AutoVacuum->checkPosession();
		//bool possession =
		AutoDrive->StraightForward(-.3);
//		if (has_a_ball == true || AutoKicker->BallCaptured() ){
		if (AutoKicker->GetNumBallsKicked() == iteration){
			AutoDrive->StraightForward(0);
			AutoState = 30;
			has_a_ball = false;
		}
		break;		
	case 30:
		//Kick
		//Run this through the kicker state and force a kick w/o a trigger pull
		//Assume this was kicked for time being
		
		//Someone said something about the code needing to manually call the kick states...? I'm not entirely sure.
		
		//This may need a few more states. Until I have more information for how to kick this autonomously, it stays
		
		AutoState = 40;
		break;
	case 40:
		//Check iteration and tells the code to execute the drive forward and kick again
		/*Each of these send the code to a different state depending on how many times the robot has driven forward
		 *and kicked.*/
		AutoDrive->ResetEncoders();
		switch(iteration){
		case 1:
			//Setup for iteration 2
			AutoState = 50;
			break;
		case 2:
			//Setup for iteration 3
			AutoState = 60;
			break;
		case 3:
			//Exits the "drive forward + kick" and sends the code to the turn left phase
			AutoState = 70;
			break;
		default:
			//This does nothing!
			break;
		}
		break;
	case 50:
		//This sets the variables for distance and iteration for the second drive forward and kick
		iteration = 2; // Don't touch this. It's important.
		distance  = 3; // Change this if the 2nd ball is a different distance away. In the mean time, leave it.
		AutoState = 10; // Jump back up to the "Drive forward" phase
		break;
	case 60:
		//This sets the variables for distance and iteration for the third drive forward and kick
		iteration = 3; // Don't touch this either. It's also important.
		distance  = 3; // Change this if the 3rd ball is a different distance away. In the mean time, leave it.
		AutoState = 10; // Jump back up to the "Drive forward" phase
		break;
	case 70:
		/*AutoState = 73;*/
		// This is really 180, but the gyro is not calibrated
		AutoDrive->RotateTo(180);
		AutoState = 71;
		
		break;
	case 71:
		// This is really 180, but the gyro is not calibrated
		AutoDrive->RotateTo(160);
		if (AutoDrive->DoneTurning())
		{
			AutoDrive->ResetTurn();
			AutoState = 90;
		}
		break;
	case 73:
		//This drives the robot forward. Not a part of the "Drive forward & kick" phase, but a part of the turn phase.
		AutoDrive->Forward(OVER_BUMP_DISTANCE, -1);
		AutoState = 75;
		break;
	case 75:
		//Checks to see if drive forward is complete.
		//Like state 15, this code needs to be located. There seems to be no "Get Position" code.
		AutoDrive->Forward(OVER_BUMP_DISTANCE, -1);
		if (AutoDrive->DoneMoving()) {
			AutoDrive->Forward(0);
			AutoState = 10; // Start Over.
		}
		
		break;
	case 80:
		//This makes it turn. I believe it's supposed to turn left?
		/*AutoDrive->TurnLeft(wantedTurn);
		AutoState = 85; */
		AutoState = 10;
		break;
	case 85:
		//Check to see if turn is complete
		heading = AutoDrive->GetHeading();
		if(heading == wantedTurn){
			AutoState = 90;
		}
		break;
	case 90:
		//Wait for teleop to engage
		//This case does nothing! Yay!
		//Untill next time, meatbag
		break;
	default:
		break;
	}
}