/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45 degrees.
* When looking at the wheels from the top, the roller axles should form an X across the robot.
*
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The direction the robot should drive in degrees. The direction and maginitute are
* independent of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely independent of
* the magnitute or direction. [-1.0..1.0]
*/
void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation)
{
static bool reported = false;
if (!reported)
{
nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_MecanumPolar);
reported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = Limit(magnitude) * sqrt(2.0);
// The rollers are at 45 degree angles.
double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
double cosD = cos(dirInRad);
double sinD = sin(dirInRad);
double wheelSpeeds[kMaxNumberOfMotors];
wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
Normalize(wheelSpeeds);
uint8_t syncGroup = 0x80;
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
CANJaguar::UpdateSyncGroup(syncGroup);
m_safetyHelper->Feed();
}
/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45 degrees.
* When looking at the wheels from the top, the roller axles should form an X across the robot.
*
* This is designed to be directly driven by joystick axes.
*
* @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction.
* This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely independent of
* the translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented controls.
*/
void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle)
{
static bool reported = false;
if (!reported)
{
nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_MecanumCartesian);
reported = true;
}
double xIn = x;
double yIn = y;
// Negate y for the joystick.
yIn = -yIn;
// Compenstate for gyro angle.
RotateVector(xIn, yIn, gyroAngle);
double wheelSpeeds[kMaxNumberOfMotors];
wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
Normalize(wheelSpeeds);
uint8_t syncGroup = 0x80;
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
CANJaguar::UpdateSyncGroup(syncGroup);
m_safetyHelper->Feed();
}
/**
* Provide tank steering using the stored robot configuration.
* This function lets you directly provide joystick values from any source.
* @param leftValue The value of the left stick.
* @param rightValue The value of the right stick.
*/
void RobotDrive::TankDrive(float leftValue, float rightValue, bool squaredInputs)
{
static bool reported = false;
if (!reported)
{
nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_Tank);
reported = true;
}
// square the inputs (while preserving the sign) to increase fine control while permitting full power
leftValue = Limit(leftValue);
rightValue = Limit(rightValue);
if(squaredInputs)
{
if (leftValue >= 0.0)
{
leftValue = (leftValue * leftValue);
}
else
{
leftValue = -(leftValue * leftValue);
}
if (rightValue >= 0.0)
{
rightValue = (rightValue * rightValue);
}
else
{
rightValue = -(rightValue * rightValue);
}
}
SetLeftRightMotorOutputs(leftValue, rightValue);
}
/**
* Drive the motors at "speed" and "curve".
*
* The speed and curve are -1.0 to +1.0 values where 0.0 represents stopped and
* not turning. The algorithm for adding in the direction attempts to provide a constant
* turn radius for differing speeds.
*
* This function will most likely be used in an autonomous routine.
*
* @param outputMagnitude The forward component of the output magnitude to send to the motors.
* @param curve The rate of turn, constant for different forward speeds.
*/
void RobotDrive::Drive(float outputMagnitude, float curve)
{
float leftOutput, rightOutput;
static bool reported = false;
if (!reported)
{
nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_ArcadeRatioCurve);
reported = true;
}
if (curve < 0)
{
float value = log(-curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
}
else if (curve > 0)
{
float value = log(curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
}
else
{
leftOutput = outputMagnitude;
rightOutput = outputMagnitude;
}
SetLeftRightMotorOutputs(leftOutput, rightOutput);
}
/**
* Arcade drive implements single stick driving.
* This function lets you directly provide joystick values from any source.
* @param moveValue The value to use for fowards/backwards
* @param rotateValue The value to use for the rotate right/left
* @param squaredInputs If set, increases the sensitivity at low speeds
*/
void RobotDrive::ArcadeDrive(float moveValue, float rotateValue,
bool squaredInputs) {
static bool reported = false;
if (!reported) {
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
HALUsageReporting::kRobotDrive_ArcadeStandard);
reported = true;
}
// local variables to hold the computed PWM values for the motors
float leftMotorOutput;
float rightMotorOutput;
moveValue = Limit(moveValue);
rotateValue = Limit(rotateValue);
if (squaredInputs) {
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (moveValue >= 0.0) {
moveValue = (moveValue * moveValue);
} else {
moveValue = -(moveValue * moveValue);
}
if (rotateValue >= 0.0) {
rotateValue = (rotateValue * rotateValue);
} else {
rotateValue = -(rotateValue * rotateValue);
}
}
if (moveValue > 0.0) {
if (rotateValue > 0.0) {
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = std::max(moveValue, rotateValue);
} else {
leftMotorOutput = std::max(moveValue, -rotateValue);
rightMotorOutput = moveValue + rotateValue;
}
} else {
if (rotateValue > 0.0) {
leftMotorOutput = -std::max(-moveValue, rotateValue);
rightMotorOutput = moveValue + rotateValue;
} else {
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = -std::max(-moveValue, -rotateValue);
}
}
SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
}
#include "lift.h"
void LiftSystem::InitLiftSystem() {
m_frontLeftMotor = NULL;
m_frontRightMotor = NULL;
m_rearRightMotor = NULL;
m_rearLeftMotor = NULL;
m_maxOutput = 127;
for (int i=0; i < maxMotors; i++)
{
m_invertedMotors[i] = 0;
}
}
LiftSystem::LiftSystem(int leftMotorChannel, int rightMotorChannel)
{
InitLiftSystem();
m_frontLeftMotor = new Motor(leftMotorChannel);
m_frontRightMotor = new Motor(rightMotorChannel);
setMotorOutput(0.0);
}
LiftSystem::LiftSystem(Motor leftMotor, Motor rightMotor)
{
InitLiftSystem();
m_frontLeftMotor = leftMotor;
m_frontRightMotor = rightMotor;
setMotorOutput(0.0);
}
LiftSystem::LiftSystem(int leftFrontMotorChannel, int leftBackMotorChannel, int rightFrontMotorChannel, int rightBackMotorChannel)
{
InitLiftSystem();
m_frontLeftMotor = new Motor(leftFrontMotorChannel);
m_frontRightMotor = new Motor(rightFrontMotorChannel);
m_rearLeftMotor = new Motor(leftBackMotorChannel);
m_rearRightMotor = new Motor(rightBackMotorChannel);
setMotorOutput(0.0);
}
LiftSystem::LiftSystem(Motor leftFrontMotor, Motor leftBackMotor, Motor rightFrontMotor, Motor rightBackMotor)
{
InitLiftSystem();
//?// xccxxcdsxxzsdc();
m_frontLeftMotor = leftFrontMotor;
m_frontRightMotor = rightFrontMotor;
m_rearLeftMotor = leftBackMotor;
m_rearRightMotor = rightBackMotor;
setMotorOutput(0.0);
}
void LiftSystem::controlLift(JoystickButton up, JoystickButton down, int speed = 127, int deadspeed = 0)
{
control(getDigital(up), getDigital(down), speed, deadspeed);
} /* DONE */
void LiftSystem::control(bool up, bool down, int speed = 127, int deadspeed = 0)
{
if(up)
{
setMotorOutput(speed);
}
else if(down)
{
setMotorOutput(speed * -1);
}
else
{
setMotorOutput(deadspeed);
}
} /* DONE */
void LiftSystem::setMotorOutput(float output)
{
if(GetNumMotors() == 4)
{
m_frontLeftMotor->setPulse(leftOutput * reversedMotors[0]);
m_frontRightMotor->setPulse(rightOutput * reversedMotors[1]);
m_rearLeftMotor->setPulse(leftOutput * reversedMotors[2]);
m_rearRightMotor->setPulse(rightOutput * reversedMotors[3]);
}
else
{
m_frontLeftMotor->setPulse(leftOutput * reversedMotors[0]);
m_frontRightMotor->setPulse(rightOutput * reversedMotors[1]);
}
}
