AutonomousKicker.cpp

#include "AutonomousKicker.h"
#include "CodeLevel.h"
#include <assert.h>
#include <cstdlib>
#include <cmath>
#include <cstring>

#include "RobotCANIds.h"

/*Inputs
 * Trigger
 * Distance from cam
 * Distance from user
 * Outputs
 * Position
 * Solenoid for the gear shift
 */

//The init is WAY down. Like, below our statemachine down.

//Constructor
AutonomousKicker::AutonomousKicker() {
	pull = false; //Trigger pulled?
	position = 0; //What position SHOULD the kicker be set to?
	ActPosition = 0; //What position is the kicker ACTUALLY at?
	Captured = false; //Is there a ball?
	//releaser->Close();
	releaser = NULL;
	shaftEncoder =NULL;
	AutoKickerState = 0;
	counter = 0;
	kickCounter = 0;
	manualBackup = false; // IMPORTANT: Set to false if the encoder ever works


}

//Colin told me to add this, I think it has something to do w/ the IO
void AutonomousKicker::KickerInit(Input * in) {
	if (manualBackup) {
		ChamberWench = new CANJaguar(KICK_WENCH_MOTOR, CANJaguar::kPercentVoltage);
	} else {
		ChamberWench = new CANJaguar(KICK_WENCH_MOTOR, CANJaguar::kPosition); //Handles the arming lever (?)
	}

	/*ChamberWench = new RedAlertJaguar(24, CANJaguar::kPosition, 
	 GetLevel() == 66 ? RedAlertJaguar::kStub : RedAlertJaguar::kCANJaguar); */

	//ChamberWench = new CANJaguar(24, CANJaguar::kPercentVoltage);
	assert(ChamberWench != NULL);
	releaser = new KickerReleaser();
	releaser->Init(8, 1, 8, 2);
	releaser->Close();
	HomeSensor = new DigitalInput(14);
	BallCapturedSensor = new DigitalInput(9);
	NumBallsKicked = 0;
	//shaftEncoder = new Encoder(4,1,4,2);
	//shaftEncoder->Start();
	input = in;
	//ChamberWench->Set(800);
	//ChamberWench->ConfigPotentiometerTurns(1);
	setPosition = -1.0;
	if (!manualBackup) {

		ChamberWench->SetPositionReference(CANJaguar::kPosRef_Encoder);
		sleep(1);
		ChamberWench->ConfigEncoderCodesPerRev(360);
		sleep(1);
		//ChamberWench->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		ChamberWench->ConfigNeutralMode(CANJaguar::kNeutralMode_Coast);
		sleep(1);
		//ChamberWench->Set(setPosition);

		ChamberWench->SetPID(-450, -0.5, 0);
	} else {
		//kickArmTime = new Timer();
		this->kickArmCounter = 0;
	}
	//ChamberWench->Set(0);
	//ChamberWench->EnableControl(0);

	//ChamberWench->DisableControl();
	//period = p;
	// kickDelay = new Timer();

}

float AutonomousKicker::GetWenchBusVoltage() {
	return ChamberWench->GetBusVoltage();
}

float AutonomousKicker::GetWenchTemperature() {
	return ChamberWench->GetTemperature();
}

float AutonomousKicker::GetWenchOutputCurrent() {
	return ChamberWench->GetOutputCurrent();
}

void AutonomousKicker::ProcessKicker() {
	int level = GetLevel();
	//int level  = 4; // this is just for testing the state machine

	switch (level) {
	case 0: //IDLE
		break;
	case 1:
		ManualMode();
		break;
	case 2:
		break;
	case 3:
		break;
	case 4: //Start the state machine
	case 10: // Also in universal mode
		//SetKickerPower();
		//assert(false);
		//exit(-1);

/*		if (input->ShouldResetKicker()) {
			ResetState();
		}
*/
		AutonomousKickerStateMachine();
		break;
	case 5:
		break;
	default:
		break;
	}
}

void AutonomousKicker::ResetState() {
	if (manualBackup) {
		ChamberWench->Set(0);

	} else {
		ChamberWench->DisableControl();
	}
	releaser->Open();
	NumBallsKicked = 0;
	setPosition = -1.0;
	AutoKickerState = 51;
}

void AutonomousKicker::AutonomousKickerStateMachine() {
	//printf("Kicker Encoder: %.2f", ChamberWench->GetPosition());
	switch (AutoKickerState) {
	case 0:
		// idle
		//ChamberWench->EnableControl(0);
		//ChamberWench->Set(0);
		/*if (input->ShouldArmKicker()) {
		 KickerSM.Bits.Bit0 = 1;
		 this->SetKickerPower();
		 }*/

		//First let's move to a home position where 
		//we can accept a ball and also drive over bumps
		releaser->Close();
		if (!manualBackup) {
			ChamberWench->EnableControl(0);
		} else {
			ChamberWench->Set(AutonomousKicker::kArmPower);
		}

		AutoKickerState = 2;
		break;
	case 1:
		// Command to arm kicker
		//releaser->Close();
		//ChamberWench->EnableControl(0);

		// Wait until we get to the home position
		if (!IsHome()) {
			if (manualBackup) {
				ChamberWench->Set(0);
			} else {
				ChamberWench->DisableControl();
			}
			AutoKickerState = 17;
		}

		break;
	case 2:
		ChamberWench->Set(-3);
		AutoKickerState = 1;
		break;
	case 8:
		// reset lauch 
		AutoKickerState = 0;
		break;
	case 16:
		// waiting for kicker to be armed
		//if (input->GetStick(1)->GetRawButton(7))

		if (manualBackup) {
			if (this->kickArmCounter <= 0) {
				AutoKickerSM.Bits.Bit2 = 1;
			}
		} else {
			if (::fabs(ChamberWench->GetPosition()) >= ::fabs(setPosition)- KICK_TOLERANCE)
			/*ChamberWench->GetPosition() > setPosition) */{
				// We got to the set position so lets disable the motor

				ChamberWench->DisableControl();
				AutoKickerSM.Bits.Bit2 = 1;
			}
		}
		break;
		case 17:
		// Command to arm kicker button down
		// Chamber(); // Doesnt do stuff. or it doesnt work. we forgot.
		//counter = static_cast<int>(::floor(1.0/period * Kicker::kArmTime)) /*change THIS one for changing the second count*/;
		counter = 30; //this is not needed anymore
		// yes it is, but using different field
		kickArmCounter = 2;
		// Set the kicker power


		// Wait for arming command
		//		if (input->ShouldArmKicker()) { don't want to wait for button input in autonomous mode
		SetKickerPower();
		
		if (!manualBackup) {
			ChamberWench->EnableControl(0);
		} else {
			// nop
		}
		AutoKickerState = 15;
		//		}

		break;
		case 15:
		if (!manualBackup) {
			
			ChamberWench->Set(setPosition);
			/*switch (NumBallsKicked){
				case 0:
					ChamberWench->Set(setPosition);
					break;
				case 1:
					ChamberWench->Set(setPosition * .8);
					break;
				case 2:
					ChamberWench->Set(setPosition);
					break;
			}*/
		}
		AutoKickerState = 16;
		break;
		case 18:
		//This is our counter
		if(counter<=0) {
			//ChamberWench->Set(0);
			AutoKickerState = 20;
		}
		else {
			counter --;
		}
		break;

		case 19:
		if (!IsHome()) {
			ChamberWench->EnableControl(0);
			ChamberWench->Set(setPosition);
			AutoKickerState = 16;
		}
		break;
		case 20:
		// kicker armed and ready
		if (!manualBackup) {
			ChamberWench->DisableControl();
		} else {
			ChamberWench->Set(0);
		}

		AutoKickerState = 36;
		break;
		case 32:
		// waiting for launch command
		//ChamberWench->DisableControl();

		if (BallCaptured()) {
			AutoKickerSM.Bits.Bit1 = 1;
		}
		break;
		case 34:
		// command to launch kicker		
		//	Kick(pull, Captured);
		AutoKickerState = 50;
		break;
		case 36:
		// reset kicker armed flag
		//AutoKickerSM.Bits.Bit2 = 0;
		AutoKickerState = 32;
		break;
		case 48:
		// Waiting for launch to complete
		if (/*input->GetStick(1)->GetRawButton(10)*/
		IsHome())/*This becomes IO14*/{
			AutoKickerSM.Bits.Bit3 = 1;
		}
		break;
		case 50:
		// command to launch kicker button down
		releaser->Open();
		NumBallsKicked++;
		// KickerState = 48; -- INSERTING DELAY
		AutoKickerState = 51;
		//kickDelay->Start();
		kickCounter = static_cast<int>(::floor(10.0 * KICK_DELAY_PERIOD) );
		break;

		case 51:

		//if (kickDelay->HasPeriodPassed(KICK_DELAY_PERIOD)) {
		--kickCounter;
		if (kickCounter <= 0) {
			//kickDelay->Reset();
			AutoKickerState = 48; // Delay finished, start watching for arm
		}
		break;
		case 56:
		// launch complete - reset launch flag complete
		// AutoKickerSM.Bits.Bit3 = 0;
		releaser->Close();
		AutoKickerState = 8;
		break;

		default:
		AutoKickerState = 0;
		break;
	}
}

int AutonomousKicker::GetCounter() {
	return counter;
}

//Kicks
void AutonomousKicker::Kick(bool trig, bool caught) {
	pull = trig;
	Captured = caught;
	if (pull == true /*&& CaughtOrNot == true && make it test to see if the 'boot' is actually cocked*/) {
		//To make the kicker, make the gear shifter for the arming device shift
		//into the freespin mode. Weeee!
	}
	/*else if(CaughtOrNot == false)
	 {
	 //Print a "NO BALL" failure
	 }
	 else if('BootTest' = failed)
	 {
	 //Boot is still arming
	 }*/
}

//Sets the firing position for the 'boot'. Think firing modes. DOES NOT ACTUALLY MOVE THE 'BOOT'.
void AutonomousKicker::SetPosition(float pos) {
	position = pos;
	//Sets the data for the position; does not actually send the 'boot' to said position.
}

//What is the position the 'boot' should be at?
float AutonomousKicker::GetSetPosition() {
	return position;
}

//Tells the robot to move the 'boot' to the SetPosition
void AutonomousKicker::Chamber() {
	//ChamberWench->SetPositionReference();
}

//Returns the current position of the 'boot'. NOT the SetPosition, but where the 'boot' is at in meatspace.
float AutonomousKicker::GetPosition() {
	//Returns the position of where the 'boot' currently is. 
	//IE Is it cocked and ready? Fired? Currently re-cocking? Etc.
	//Add more crap when I know how this stuff works.
	return ActPosition;
	//return shaftEncoder->Get(); 
}

//Returns whether or not the boot is actually armed. For use in other functions
bool AutonomousKicker::Armed() { // this isnt really doing anything
	return true;
}

bool AutonomousKicker::LaunchComplete() {
	// this needs to check if the kicker has completed a launch
	return true;
}

int AutonomousKicker::GetState() {
	return AutoKickerState;
}

void AutonomousKicker::DuctTape() {
	// Fixes EVERYTHING
	const bool FIXED = true;
	int everything;
	everything = FIXED;
}

bool AutonomousKicker::IsHome() {
	return HomeSensor->Get() != 0;
}

bool AutonomousKicker::BallCaptured() {
	return BallCapturedSensor->Get() != 0;
}

void AutonomousKicker::ManualMode() {

	/* if (input->GetStick(0)->GetRawButton(8)) {
	 ChamberWench->Set(-.25);
	 } else {
	 ChamberWench->Set(0);
	 }*/
}
double AutonomousKicker::GetEncoderPosition() {
	return ChamberWench->GetPosition();
}

void AutonomousKicker::SetKickerPower() {
	if (manualBackup) {
		this->kickArmCounter = input->GetKickPower() > 0.5 ? 2 : 1;
	} else {
		setPosition = input->GetKickPower() * kMaxKickPower;
	}
}

int AutonomousKicker::GetNumBallsKicked(){
	return NumBallsKicked;
}