Hardware.cpp

#include "WPILib.h"
#include "CANWrapper.h"
#include "CANJaguar.h"
#include "Hardware.h"
#include "RobotConfiguration.h"
#include "DebugUtil.h"
#include "LimitSwitch.h"
#include "Potentiometer.h"

CANJaguar * Hardware::m_left_jag1 = NULL;
CANJaguar * Hardware::m_right_jag1 = NULL;
CANJaguar * Hardware::m_left_jag2 = NULL;
CANJaguar * Hardware::m_right_jag2 = NULL;

Accelerometer * Hardware::accelX = NULL;
Accelerometer * Hardware::accelY = NULL;
Accelerometer * Hardware::accelZ = NULL;

LimitSwitch * Hardware::m_gripper_limit = NULL;

Encoder * Hardware::left_encoder = NULL;
Encoder * Hardware::right_encoder = NULL;
RAGearToothSensor * Hardware::m_shuttle_geartooth_encoder = NULL;
DigitalInput * Hardware::center_line_sensor;
DigitalInput * Hardware::right_line_sensor;
DigitalInput * Hardware::left_line_sensor;
Solenoid * Hardware::m_sol = NULL;
Gyro * Hardware::gyro = NULL;

GearTooth * Hardware::ShuttleTooth = NULL;

CANJaguar * Hardware::m_motors[12] = { NULL };

DigitalInput * Hardware::m_shuttle_home = NULL;

Potentiometer * Hardware::m_arm_pot = NULL;

DigitalOutput * Hardware::m_signal_do1 = NULL;
DigitalOutput * Hardware::m_signal_do2 = NULL;
DigitalOutput * Hardware::m_signal_do3 = NULL;

DigitalInput * Hardware::m_deployer_optical = NULL;

SonarSensor * Hardware::m_sonar = NULL;

//DeployerSolenoid * Hardware::m_deploy_solenoid = NULL;

Solenoid * Hardware::m_deploy_solenoids[4] = { NULL };

DigitalInput * Hardware::GetDeployerOpticalSensor()
{
	if (m_deployer_optical == NULL) {
		m_deployer_optical = new DigitalInput(6,1);
	}
	return m_deployer_optical;
}

DigitalOutput * Hardware::GetSignalDO1()
{
	if (m_signal_do1 == NULL) {
		m_signal_do1 = new DigitalOutput(6, 12);
	}
	
	return m_signal_do1;
}

DigitalOutput * Hardware::GetSignalDO2()
{
	if (m_signal_do2 == NULL) {
		m_signal_do2 = new DigitalOutput(6, 13);
	}
	
	return m_signal_do2;
}

DigitalOutput * Hardware::GetSignalDO3()
{
	if (m_signal_do3 == NULL) {
		m_signal_do3 = new DigitalOutput(6, 14);
	}
	
	return m_signal_do3;
}

Solenoid * Hardware::GetLineSensorPower()
{
	if (m_sol == NULL) {
		m_sol = new Solenoid(8,1);
	}
	
	return m_sol;		
	
}

LimitSwitch * Hardware::GetGripperLimitSwitch()
{
	if (m_gripper_limit == NULL) {
		m_gripper_limit = new LimitSwitch(4, 7);
	}
	return m_gripper_limit;
}

void Hardware::ReConfigureDriveMotors()
{
	ConfigureForSpeedControl(m_left_jag1);
	ConfigureForSpeedControl(m_left_jag2);
	ConfigureForSpeedControl(m_right_jag1);
	ConfigureForSpeedControl(m_right_jag2);
}
CANJaguar * Hardware::AllocateDriveMotor(int number)
{
	// Set up for simple percentage of voltage (-1 to 1, 
	// no closed loop)
	return new CANJaguar(number, CANJaguar::kPercentVbus);
	//return new CANJaguar(number, CANJaguar::kSpeed);
}
void Hardware::ConfigureForSpeedControl(CANJaguar * motor)
{
	
	if (motor != NULL) {
		/*
		int num_inits = 10;
		for (int counter = 0; counter < num_inits; ++counter) {
			printf("Attempt %d of %d\n", counter, num_inits);
			// Uncomment if you aren't using voltage mode on the motors
			
			//motor->ChangeControlMode(CANJaguar::kSpeed);
			motor->SetPID(RobotConfiguration::GetDriveTrainP(),
					  RobotConfiguration::GetDriveTrainI(),
					  RobotConfiguration::GetDriveTrainD());
			DPRINTF("P: %2.2f\tI: %2.2f\tD: %2.2f\n", motor->GetP(), motor->GetI(), motor->GetD());
			motor->ConfigEncoderCodesPerRev(100);
		//motor->ConfigMaxOutputVoltage(9.0);
			motor->ConfigNeutralMode(CANJaguar::kNeutralMode_Coast);
			motor->SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
			motor->EnableControl(0);
			DPRINTF("Alive? %s\n", motor->IsAlive() ? "Yar" : "Nar");
			
			if (motor->GetSpeedReference() == CANJaguar::kSpeedRef_QuadEncoder)
			{
				printf("Attempt %d succeeded.\n", counter);
				break;
			}
		}
		*/
	}
}

CANJaguar * Hardware::GetLeftMotorJag()
{
	if (m_left_jag1 == NULL)
	{
		m_left_jag1 = AllocateDriveMotor(3);
		ConfigureForSpeedControl(m_left_jag1);
		
	}
	return m_left_jag1;
}

CANJaguar * Hardware::GetRightMotorJag()
{
	if (m_right_jag1 == NULL)
	{
		m_right_jag1 = AllocateDriveMotor(5);
		ConfigureForSpeedControl(m_right_jag1);
	}
	return m_right_jag1;
}

CANJaguar * Hardware::GetLeftMotorJag2()
{
	if (m_left_jag2 == NULL)
	{
		m_left_jag2 = AllocateDriveMotor(4);
		ConfigureForSpeedControl(m_left_jag2);
	}
	return m_left_jag2;
}

CANJaguar * Hardware::GetRightMotorJag2()
{
	if (m_right_jag2 == NULL)
	{
		m_right_jag2 = AllocateDriveMotor(6);
		ConfigureForSpeedControl(m_right_jag2);
	}
	return m_right_jag2;
}

DigitalInput * Hardware::GetLeftLineSensor()
{
	if (left_line_sensor == NULL)
	{
		left_line_sensor = new DigitalInput(8);
	}
	return left_line_sensor;
}

DigitalInput * Hardware::GetCenterLineSensor()
{
	if (center_line_sensor == NULL)
	{
		center_line_sensor = new DigitalInput(9);
	}
	return center_line_sensor;
}

DigitalInput * Hardware::GetRightLineSensor()
{
	if (right_line_sensor == NULL)
	{
		right_line_sensor = new DigitalInput(10);
	}
	return right_line_sensor;
}

DigitalInput * Hardware::GetShuttleHomeSensor()
{
	if (m_shuttle_home == NULL)
	{
		m_shuttle_home = new DigitalInput(6);
	}
	return m_shuttle_home;
}

CANJaguar * Hardware::GetUpperGrabberMotor()
{
	CANJaguar * motor = GetMotor(7);
	
	if (motor == NULL) {
		motor = AllocateGrabberMotor(7);
	}
	
	return motor;
}

CANJaguar * Hardware::GetLowerGrabberMotor()
{
	CANJaguar * motor = GetMotor(8);
	
	if (motor == NULL) {
		motor = AllocateGrabberMotor(8);
	}
	
	return motor;
}

CANJaguar * Hardware::GetArmMotor()
{
	int number = 10;
	//int number = 5;
	
	CANJaguar * arm_motor = GetMotor(number);
	if (arm_motor == NULL) {
		//arm_motor = AllocatePositionMotor(number);
		arm_motor = new CANJaguar(number, CANJaguar::kPercentVbus);
		/*arm_motor->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
		arm_motor->ConfigPotentiometerTurns(3);
		arm_motor->SetPID(1,.01,0);
		arm_motor->EnableControl();
		*/
		m_motors[number-1] = arm_motor;
	}
	return arm_motor;
}

CANJaguar * Hardware::AllocateDeployerMotor(int number)
{
	// Set up for simple percentage of voltage (-1 to 1, 
	// no closed loop)
	CANJaguar * motor = new CANJaguar(number, CANJaguar::kPercentVbus);
	m_motors[10] = motor;
	return motor;
	//return new CANJaguar(number, CANJaguar::kSpeed);
}

CANJaguar * Hardware::GetDeployerMotor()
{
	/*
	CANJaguar * motor = GetMotor(11);
		
	if (motor == NULL) {
		motor = AllocateDeployerMotor(11);	
	} 
	return motor; */
	return NULL;
}

//TODO: This needs to work. And is a bookmark
Servo * Hardware::GetAlignmentReleaser()
{
	
	return NULL;
}

CANJaguar * Hardware::GetMotor(int i) {
	if (i < 3 || i > 11) {
		return NULL;
	} else {
		return m_motors[i-1];
	}
}

CANJaguar * Hardware::AllocateGrabberMotor(int number)
{
	
	/*
	if (m_motors[number-1] == NULL) {
		m_motors[number-1] = new CANJaguar(number, CANJaguar::kSpeed);
	}
	
	if (m_motors[number-1] == NULL) {
		CANJaguar * m = m_motors[number-1];
		m->SetPID(1,.2,0);
		
	}
	*/
	
	
	if (m_motors[number-1] == NULL) {
		/*
		m_motors[number-1] = new CANJaguar(number, CANJaguar::kSpeed);
		m_motors[number-1]->SetPID(1,0,0);
		m_motors[number-1]->SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
		m_motors[number-1]->ConfigEncoderCodesPerRev(250);
		m_motors[number-1]->EnableControl(0);
		*/
		m_motors[number-1] = new CANJaguar(number, CANJaguar::kPercentVbus);
		
	}
	
	 
	return m_motors[number-1];
}

// TODO: There should probably be a better way to combine 
// the above and below methods.
CANJaguar * Hardware::AllocatePositionMotor(int number)
{
	if (m_motors[number-1] == NULL) {
		m_motors[number-1] = new CANJaguar(number, CANJaguar::kPosition);
	}
	
	return m_motors[number-1];
}

Encoder * Hardware::GetLeftEncoder()
{
	if(left_encoder == NULL){
		left_encoder = new Encoder(1,2);
		//left_encoder->SetDistancePerPulse(0.07540);  // 250 Count Encoders
		left_encoder->SetDistancePerPulse(0.1884);    // 100 Count Encoders
		left_encoder->Start();
	}
	return left_encoder;
}


Encoder * Hardware::GetRightEncoder()
{
	if(right_encoder == NULL){
		right_encoder = new Encoder(3,4);
		// TODO: This might be a good candidate for the config file
		// assumes 6-inch wheels
		//right_encoder->SetDistancePerPulse(0.07540);  // 250 Count Encoders
		right_encoder->SetDistancePerPulse(0.1884);  // 100 Count Encoders
		right_encoder->Start();
	}
	return right_encoder;
}

RAGearToothSensor * Hardware::GetShuttleGearToothSensor()
{
	if(m_shuttle_geartooth_encoder == NULL){
#warning "If you need to fix the direction for the gear tooth sensor, do so here."
		// Changed to 40 teeth because of sprocket change (from 19)
		m_shuttle_geartooth_encoder = new RAGearToothSensor(5,true,40, 0.5);
		m_shuttle_geartooth_encoder->Start();
	}
	return m_shuttle_geartooth_encoder;
}

CANJaguar * Hardware::GetShuttleMotor()
{
	CANJaguar * motor = GetMotor(9);
	if (motor == NULL) {
		//motor = AllocateDriveMotor(5);
		//motor = AllocatePositifonMotor(5);
		//motor->SetPID(120,.001,.01);
		motor = new CANJaguar(9, CANJaguar::kPercentVbus);
		motor->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		
		m_motors[8] = motor;
		//motor->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		//motor->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);

		//motor->ConfigEncoderCodesPerRev(250);
		//motor->EnableControl(0);
	}
	
	return motor;
}

Potentiometer * Hardware::GetArmPotentiometer()
{
	if (m_arm_pot == NULL) {
		m_arm_pot = new Potentiometer(6, 3);
		m_arm_pot->ConfigureVoltageLimits(2.18, 280);
	}
	return m_arm_pot;
}

Gyro * Hardware::GetGyro()
{
	if(gyro==NULL){
		gyro = new Gyro(1,1);
		gyro->SetSensitivity(.0068);
		gyro->Reset();
	}
	return gyro;
}

SonarSensor * Hardware::GetSonarSensor()
{
	if (m_sonar == NULL) {
		m_sonar = new SonarSensor(1,7);
	}
	
	return m_sonar;
}

Accelerometer * Hardware::GetAccelX()
{
	if(accelX==NULL){
		accelX = new Accelerometer(1,3);
		InitAccel(accelX);
	}
	return accelX;
}
Accelerometer * Hardware::GetAccelY()
{
	if(accelY==NULL){
		accelY = new Accelerometer(1,4);
		InitAccel(accelY);
	}
	return accelY;
}
Accelerometer * Hardware::GetAccelZ()
{
	if(accelZ==NULL){
		accelZ = new Accelerometer(1,5);
		InitAccel(accelZ);
	}
	return accelZ;
}

void Hardware::InitAccel(Accelerometer * a)
{
	//a->SetZero(2.5);
	a->SetZero(1.5);
	a->SetSensitivity(.36);
	
}
#if 0
DeployerSolenoid *Hardware::GetDeployerSolenoid()
{
	if(m_deploy_solenoid==NULL)
	{
		m_deploy_solenoid = new DeployerSolenoid();
	}
	return m_deploy_solenoid;
}
#endif


Solenoid * Hardware::GetDeployerSolenoid(int num)
{
	if (num < 0  || num >= 4) {
		return NULL;
	}
	
	if (m_deploy_solenoids[num] == NULL) {
		m_deploy_solenoids[num] = new Solenoid(8, 2 + num);
	}
	return m_deploy_solenoids[num];
	
}