RobotGrabber.cpp

#include "RobotGrabber.h"
#include "Hardware.h"
#include "DriverInput.h"
#include "LimitSwitch.h"
#include "DebugUtil.h"

CANJaguar * RobotGrabber::m_lower_motor = NULL;
CANJaguar * RobotGrabber::m_upper_motor = NULL;
LimitSwitch * RobotGrabber::m_grip_limit = NULL;
bool RobotGrabber::m_ejecting = false;
bool RobotGrabber::m_rotating = false;

const float RobotGrabber::kFasterSpeed = .8;
const float RobotGrabber::kSlowerSpeed = .6;

void RobotGrabber::Init()
{
	if (m_upper_motor == NULL) {
		m_upper_motor = Hardware::GetUpperGrabberMotor();
	}
	
	if (m_lower_motor == NULL) {
		m_lower_motor = Hardware::GetLowerGrabberMotor();
	}
	
	if (m_grip_limit == NULL) {
		m_grip_limit = Hardware::GetGripperLimitSwitch();
	}
}

void RobotGrabber::Process()
{
	//TODO: Get from configuration.
	float faster = RobotConfiguration::GetGrabberFasterSpeed();
	float slower = RobotConfiguration::GetGrabberSlowerSpeed();
	//float middle = RobotConfiguration::GetGrabberMiddleSpeed();  //-- This doesn't get used, I hate warnings
	// Do things.
	
	
	
	bool command_rotate_up = DriverInput::GetRotateUp() || m_rotating;
	bool command_rotate_down = DriverInput::GetRotateDown();
	bool command_capture = DriverInput::GetCapture();
	bool command_release = DriverInput::GetRelease() || m_ejecting;
	
	bool rotate_up = false;
	bool rotate_down = false;
	bool capture = false;
	bool release = false;
	
	bool not_tripped = (! m_grip_limit->IsTripped());
	
	// NOTE: This mapping has been changed to reflect the 
	// different wiring scheme used on 1747's claw.
	// UP -> IN 
	// DOWN -> OUT
	
#if USING_1747_CLAW
	rotate_up = command_capture;
	rotate_down = command_release;
	capture = command_rotate_up;
	release = command_rotate_down;
#else
	rotate_up = command_rotate_up;
	rotate_down = command_rotate_down;
	capture = command_capture;
	release = command_release;
#endif
	
	int count = 0;
	if (rotate_up) count++;
	if (rotate_down) count++;
	if (capture) count++;
	if (release) count++;
	
	bool only_one_anything = (count == 1);
	
	float upper_speed = 0;
	float lower_speed = 0;
	
	if (only_one_anything) {
		// Ok, now figure out which thing to do.
		
		if (rotate_up) {
			upper_speed = -faster;
			lower_speed = -slower;
		} else if (rotate_down) {
			upper_speed = slower;
			lower_speed = faster;
		} else if (capture) {
				// We're ok to keep going.
				upper_speed = -faster;
				lower_speed = faster;
		
		} else if (release) {
			upper_speed = slower;
			lower_speed = -slower;
		} else {
			DPRINTF("Problem, unexpected state.\n", NULL);
		}
	} else {
		// We're supposed to do multiple things. Bad. Do nothing.
	}
	
		
	m_upper_motor->Set(upper_speed);
	m_lower_motor->Set(lower_speed);
	
}