ControlElevator.cpp

#include "ControlElevator.h"
#include <iostream>


ControlElevator::ControlElevator()
{
	std::cout << "[ControlElevator] Constructed" << std::endl;

	if ( CommandBase::pElevator != nullptr )
	{
		Requires(CommandBase::pElevator);
	}
	else
	{
		std::cout << "[ControlElevator] elevator is NULL!" << std::endl;
	}

	return;
}


void ControlElevator::Initialize()
{
	std::cout << "[ControlElevator] Constructed" << std::endl;
	this->setElevatorPos = 0;
	this->lastElevatorPos = 1;

	if(CommandBase::pElevator->GetBottomSwitchState())
		this->lastElevatorPos = 1;
	if(CommandBase::pElevator->GetMidSwitchState())
		this->lastElevatorPos = 2;
	if(CommandBase::pElevator->GetTopSwitchState())
		this->lastElevatorPos = 3;
	return;
}


void ControlElevator::Execute()
{
	//Get Controls
	frc::XboxController* pJoyOperator = CommandBase::pOI->GetJoystickOperator();
	double rightOpTriggerAxis = pJoyOperator->GetTriggerAxis(frc::XboxController::kRightHand);
	double leftOpTriggerAxis = pJoyOperator->GetTriggerAxis(frc::XboxController::kLeftHand);
	double motorSpeed = 0.0;
	//Operator controller overrides auto mode
	if(rightOpTriggerAxis > 0.1 || leftOpTriggerAxis > 0.1)
		this->setElevatorPos = 0;

	/**
	 * Here is how the lovely 3 positions of the elevator
	 * auto mode are organized
	 * | Position 3
	 * |
	 * | Position 2
	 * |
	 * |____________ Position 1 (base)
	 *
	 */

	//Set the elevator last seen location according to the hall effect sensors
	if(CommandBase::pElevator->GetTopSwitchState())
		this->lastElevatorPos = 3;
	if(CommandBase::pElevator->GetMidSwitchState())
		this->lastElevatorPos = 2;
	if(CommandBase::pElevator->GetBottomSwitchState())
		this->lastElevatorPos = 1;


	/**
	 * Controller mapping is shown below for setting positions
	 * 			+++++++
     * 	   	   	+:::::+   <---- Set to position 3
     * 	   	   	+:::::+
 	 *	  +++++++:::::+++++++
 	 *	  +:::::::::::::::::+ <---- Set to position 2
 	 *	  +:::::::::::::::::+
 	 *	  +++++++:::::+++++++
     * 	   	    +:::::+
     * 	   	^   +:::::+   <---- Set to position 1
     * 	   	|   +++++++
     * 	   	\-------------------- Stop movement and enable override (position 0)
     *
	 */
	//POV is measured in degrees
	double opPOV = pJoyOperator->GetPOV();

	if(opPOV == 0)
		this->setElevatorPos = 3;
	if(opPOV == 90)
		this->setElevatorPos = 2;
	if(opPOV == 180)
		this->setElevatorPos = 1;
	if(opPOV == 270)
		this->setElevatorPos = 0;




	/**
	 * This code moves the elevator into the requested position
	 * For example if last seen was 3 and you want to go to 1 it will go down
	 *
	 * | Position 3 | Go down two positions
	 * |			|
	 * | Position 2 V
	 * |
	 * |____________ Position 1 (base)
	 *
	 * Position 0 (DO NOT MOVE OPERATOR OVERRIDE)
	 *
	 */

	//Ensure operator override isn't in place
	if(this->setElevatorPos != 0)
	{
		if(this->setElevatorPos < this->lastElevatorPos)
			motorSpeed = 0.25;
		if(this->setElevatorPos > this->lastElevatorPos)
			motorSpeed = -0.25;
	} else {
		//Take in manual controller input
		motorSpeed = leftOpTriggerAxis -  rightOpTriggerAxis;
	}



	//std::cout << "[ControlElevator] Motorspeed: '" << motorSpeed << "'.\n";
	SmartDashboard::PutNumber( "Elevator Motorspeed", motorSpeed );
	SmartDashboard::PutNumber( "Last Elevator Position", this->lastElevatorPos);
	SmartDashboard::PutNumber( "Next Elevator Position", this->setElevatorPos);



	CommandBase::pElevator->SetMotorSpeed(motorSpeed);

	std::cout << "[ControlElevator] SET Switch PositPosion " << this->setElevatorPos << std::endl;
	std::cout << "[ControlElevator] LAST Switch Position " << this->setElevatorPos << std::endl;


	return;
}



bool ControlElevator::IsFinished()
{
	return false;
}



void ControlElevator::End()
{
	return;
}


void ControlElevator::Interrupted()
{
	return;
}

ControlElevator.h

#ifndef SRC_COMMANDS_CONTROLELEVATOR_H_
#define SRC_COMMANDS_CONTROLELEVATOR_H_

#include <iostream>
#include <WPILib.h>
#include "../CommandBase.h"

class ControlElevator : public CommandBase
{
public:
	ControlElevator();
	void Initialize() override;
	void Execute() override;
	bool IsFinished() override;
	void End() override;
	void Interrupted() override;
private:
	int setElevatorPos;
	int lastElevatorPos;
};

#endif /* SRC_COMMANDS_CONTROLELEVATOR_H_ */

Elevator.cpp

#include "Elevator.h"
#include <iostream>
#include "../Commands/ControlElevator.h"


Elevator::Elevator() : frc::Subsystem("Elevator")
{
	std::cout << "[Elevator] Constructed" << std::endl;

	this->pElevatorMotor = new can::WPI_TalonSRX(ELEVATOR_MOTOR_ID);
	this->pTopElevatorSwitch = new frc::DigitalInput(ELEVATOR_TOP_SWITCH_ID);
	this->pMidElevatorSwitch = new frc::DigitalInput(ELEVATOR_MID_SWITCH_ID);
	this->pBottomElevatorSwitch = new frc::DigitalInput(ELEVATOR_BOTTOM_SWITCH_ID);

	return;
}


Elevator::~Elevator()
{
	delete this->pElevatorMotor;
	delete this->pTopElevatorSwitch;
	delete this->pMidElevatorSwitch;
	delete this->pBottomElevatorSwitch;

	return;

}


void Elevator::SetMotorSpeed(double elevatorSpeed)
{
	this->pElevatorMotor->Set(elevatorSpeed);

	return;
}

bool Elevator::GetBottomSwitchState()
{
	//Sensors return true if there isn't any magnet
	//inverse for clarification
	return !this->pBottomElevatorSwitch->Get();
}

bool Elevator::GetMidSwitchState()
{
	//Sensors return true if there isn't any magnet
	//inverse for clarification
	return !this->pMidElevatorSwitch->Get();
}

bool Elevator::GetTopSwitchState()
{
	//Sensors return true if there isn't any magnet
	//inverse for clarification
	return !this->pTopElevatorSwitch->Get();
}

void Elevator::InitDefaultCommand()
{
	std::cout << "[Elevator] Initialized  Default Command" << std::endl;

	SetDefaultCommand( new ControlElevator() );

	return;
}


void Elevator::Reset()
{
	std::cout << "[Elevator] Resetting the motors" << std::endl;

	this->pElevatorMotor->Set(0.0);

	return;
}

Elevator.h

#ifndef SRC_SUBSYSTEMS_ELEVATOR_H_
#define SRC_SUBSYSTEMS_ELEVATOR_H_

#include <WPILib.h>
#include <Commands/Subsystem.h>
#include <ctre/Phoenix.h>
#include "../RobotMap.h"

#include <iostream>

class Elevator: public frc::Subsystem
{
public:
	Elevator();
	~Elevator();
	void InitDefaultCommand() override;
	void Reset();
	void SetMotorSpeed(double speed);
	bool GetBottomSwitchState();
	bool GetMidSwitchState();
	bool GetTopSwitchState();
private:
	can::WPI_TalonSRX* pElevatorMotor;

	frc::DigitalInput* pTopElevatorSwitch;
	frc::DigitalInput* pMidElevatorSwitch;
	frc::DigitalInput* pBottomElevatorSwitch;

	int setElevatorPos;
	int lastElevatorPos;
};



#endif /* SRC_SUBSYSTEMS_ELEVATOR_H_ */