/** * 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]); } }