Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)
-   -   Strafing Problem (http://www.chiefdelphi.com/forums/showthread.php?t=134661)

Tweety 15-02-2015 20:53
Strafing Problem
 
Hi all,
I'm having issues with the mecanum drive code. Problem is that strafing doesn't work. Robot can turn fine and go forward/back. I referred to how the wheels should be moving when it's strafing and it seems like they are just doing something else. Both sides are moving outwards so its just countering itself.

I read somewhere that I have to invert motors. How do I do it in java?


Code:

package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables2.type.NumberArray;


public class Robot extends IterativeRobot {
    //LEFT STICK = AXIS  RIGHT STICK = ROTATION
    private Joystick leftJoystick = new Joystick(1);
    private Joystick rightJoystick = new Joystick(2);
    private CANTalon frontLeft = new CANTalon(1);
    private CANTalon rearLeft = new CANTalon(2);
    private CANTalon frontRight = new CANTalon(3);
    private CANTalon rearRight = new CANTalon(4);
    private RobotDrive mecDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
    private Gyro gyroscope = new Gyro(1);
   
    //Pneumatics
    private Compressor compressor = new Compressor(1);
    private DoubleSolenoid Gripper = new DoubleSolenoid(1,2);
   
    //Nerd Stuff
    NetworkTable table;
   
 
    public void robotInit() {
        compressor.start();
        table = NetworkTable.getTable("");
    }
     
    public void autonomous() {
       
    }
   
    public void operatorControl() {
        double xValue = 0.0;
        double yValue = 0.0;
        double y = 0.0;
        double x = 0.0;
        double rotation = 0.0;
        double gyroAngle = 0.0;
        double throttle  = 0.0;
               
        while(isOperatorControl() && isEnabled()) {

            //EDC
            x = -leftJoystick.getX();
            y = -leftJoystick.getY();
            rotation = -rightJoystick.getX();
           
            throttle = leftJoystick.getThrottle();
            if (throttle < .5) {
                    yValue = y * .5;
                    xValue = x * .5;
            }

            gyroAngle = gyroscope.getAngle();
           
            mecDrive.mecanumDrive_Cartesian(xValue, yValue, rotation, gyroAngle);
                   
        }
   
    }
   
    public void test() {
   
    }
}

legts 15-02-2015 21:12
Re: Strafing Problem
 
I can't seem to dig up the code at this point in time, but my team did do strafing last year. All of our code is on GitHub, at https://github.com/frc2399/2014-code...libj/templates. The way we implemented strafing is a bit complicated. Check out the JoystickDrive and OI files for more information. I'd explain it further, but I think the code would do a better job explaining then I would. Good luck and let me know how it goes!

JacobD 15-02-2015 21:35
Re: Strafing Problem
 
Quote:
Originally Posted by Tweety (Post 1444708)
I read somewhere that I have to invert motors. How do I do it in java?
Code:
mecDrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
mecDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
Those are the motors that my team had to invert in order to strafe. But, we also had problems with driving forward and back at the time. So, if this messes up the forward and backward driving, ensure that the motor channels for your mecanum drive are like so:

0(frontLeft), 1(rearLeft), 2(frontRight), 3(rearRight)

This should fix any problems you might have. Otherwise, you might have to invert the motors when you get and X-input and then switch them back when you get Y. But, you should avoid this at all costs.

SuperBK 15-02-2015 22:55
Re: Strafing Problem
 
You might want to try specifying 0 for the gyro value to see if the robot can strafe relative to the way its currently pointing. Like JacobD said, make sure your pwn channels are in the correct order. If The robot can drive forward, reverse and turn, then you don't need to invert any motors. I would look on the driver's station usb tab and see what the X axis on the left joystick (your strafing one) is doing. You could also try printing values to stdout and viewing with the rioLog.

Tweety 15-02-2015 23:20
Re: Strafing Problem
 
Robot turns left/right and goes forward and backwards just fine. Just strafing is the problem. The motors are wired properly from frontLeft, rearLeft, frontRight, and rearRight as are the motors. But then again we swapped certain axis to try and fix the robot rotating when it should and such. It sounds confusing I know, I'll try and post an update tomorrow for others having similar issues.

The red arrows are the problem since one side should spin the other direction to strafe but it doesn't. So the solution would be to invert one set of motors since as of now, +1 in one direction is +1 in the other direction since the motors are flipped for mount.

http://prntscr.com/65ovz5

GeeTwo 16-02-2015 00:04
Re: Strafing Problem
 
Quote:
Originally Posted by JacobD (Post 1444724)
This should fix any problems you might have. Otherwise, you might have to invert the motors when you get and X-input and then switch them back when you get Y. But, you should avoid this at all costs.
Agreed. Inverting motors any time other than robot initialization sounds like a recipe for smoking and/or broken parts.

Ether 16-02-2015 08:10
Re: Strafing Problem
 

Guys, this is a mecanum drivetrain we're talking about. You're going to get X and Y inputs at the same time.

Ether 16-02-2015 10:29
Re: Strafing Problem
 
1 Attachment(s)
Quote:
Originally Posted by Tweety (Post 1444708)
I'm having issues with the mecanum drive code. Problem is that strafing doesn't work. Robot can turn fine and go forward/back. I referred to how the wheels should be moving when it's strafing and it seems like they are just doing something else. Both sides are moving outwards so its just countering itself.
Print the attachment.

Put your bot up on blocks and observe wheel directions for all six commands (forward, reverse, spin right, spin left, strafe right, strafe left) and carefully compare them to the printout. Mark up the printout to show any wheel directions which do not agree with the attachment.

Take a picture of the marked-up printout and post it, along with your code.

JacobD 16-02-2015 11:59
Re: Strafing Problem
 
Take a look at the RobotDrive Class. After reading through it, there are very few places you can go wrong.

Code:

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code  */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                              */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;

import edu.wpi.first.wpilibj.can.CANNotInitializedException;
import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tInstances;
import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
import edu.wpi.first.wpilibj.communication.UsageReporting;

/**
 * Utility class for handling Robot drive based on a definition of the motor configuration.
 * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and
 * mecanum drive trains are supported. In the future other drive types like swerve might
 * be implemented. Motor channel numbers are supplied on creation of the class. Those are
 * used for either the drive function (intended for hand created drive code, such as autonomous)
 * or with the Tank/Arcade functions intended to be used for Operator Control driving.
 */
public class RobotDrive implements MotorSafety {

    protected MotorSafetyHelper m_safetyHelper;

    /**
    * The location of a motor on the robot for the purpose of driving
    */
    public static class MotorType {

        /**
        * The integer value representing this enumeration
        */
        public final int value;
        static final int kFrontLeft_val = 0;
        static final int kFrontRight_val = 1;
        static final int kRearLeft_val = 2;
        static final int kRearRight_val = 3;
        /**
        * motortype: front left
        */
        public static final MotorType kFrontLeft = new MotorType(kFrontLeft_val);
        /**
        * motortype: front right
        */
        public static final MotorType kFrontRight = new MotorType(kFrontRight_val);
        /**
        * motortype: rear left
        */
        public static final MotorType kRearLeft = new MotorType(kRearLeft_val);
        /**
        * motortype: rear right
        */
        public static final MotorType kRearRight = new MotorType(kRearRight_val);

        private MotorType(int value) {
            this.value = value;
        }
    }
    public static final double kDefaultExpirationTime = 0.1;
    public static final double kDefaultSensitivity = 0.5;
    public static final double kDefaultMaxOutput = 1.0;
    protected static final int kMaxNumberOfMotors = 4;
    protected final int m_invertedMotors[] = new int[4];
    protected double m_sensitivity;
    protected double m_maxOutput;
    protected SpeedController m_frontLeftMotor;
    protected SpeedController m_frontRightMotor;
    protected SpeedController m_rearLeftMotor;
    protected SpeedController m_rearRightMotor;
    protected boolean m_allocatedSpeedControllers;
    protected boolean m_isCANInitialized = false;//TODO: fix can
    protected static boolean kArcadeRatioCurve_Reported = false;
    protected static boolean kTank_Reported = false;
    protected static boolean kArcadeStandard_Reported = false;
    protected static boolean kMecanumCartesian_Reported = false;
    protected static boolean kMecanumPolar_Reported = false;

    /** Constructor for RobotDrive with 2 motors specified with channel numbers.
    * Set up parameters for a two wheel drive system where the
    * left and right motor pwm channels are specified in the call.
    * This call assumes Talons for controlling the motors.
    * @param leftMotorChannel The PWM channel number that drives the left motor.
    * @param rightMotorChannel The PWM channel number that drives the right motor.
    */
    public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        m_frontLeftMotor = null;
        m_rearLeftMotor = new Talon(leftMotorChannel);
        m_frontRightMotor = null;
        m_rearRightMotor = new Talon(rightMotorChannel);
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = true;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * Constructor for RobotDrive with 4 motors specified with channel numbers.
    * Set up parameters for a four wheel drive system where all four motor
    * pwm channels are specified in the call.
    * This call assumes Talons for controlling the motors.
    * @param frontLeftMotor Front left motor channel number
    * @param rearLeftMotor Rear Left motor channel number
    * @param frontRightMotor Front right motor channel number
    * @param rearRightMotor Rear Right motor channel number
    */
    public RobotDrive(final int frontLeftMotor, final int rearLeftMotor,
                      final int frontRightMotor, final int rearRightMotor) {
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        m_rearLeftMotor = new Talon(rearLeftMotor);
        m_rearRightMotor = new Talon(rearRightMotor);
        m_frontLeftMotor = new Talon(frontLeftMotor);
        m_frontRightMotor = new Talon(frontRightMotor);
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = true;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * Constructor for RobotDrive with 2 motors specified as SpeedController objects.
    * The SpeedController version of the constructor enables programs to use the RobotDrive classes with
    * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
    * the curve to suit motor bias or dead-band elimination.
    * @param leftMotor The left SpeedController object used to drive the robot.
    * @param rightMotor the right SpeedController object used to drive the robot.
    */
    public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
        if (leftMotor == null || rightMotor == null) {
            m_rearLeftMotor = m_rearRightMotor = null;
            throw new NullPointerException("Null motor provided");
        }
        m_frontLeftMotor = null;
        m_rearLeftMotor = leftMotor;
        m_frontRightMotor = null;
        m_rearRightMotor = rightMotor;
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = false;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * Constructor for RobotDrive with 4 motors specified as SpeedController objects.
    * Speed controller input version of RobotDrive (see previous comments).
    * @param rearLeftMotor The back left SpeedController object used to drive the robot.
    * @param frontLeftMotor The front left SpeedController object used to drive the robot
    * @param rearRightMotor The back right SpeedController object used to drive the robot.
    * @param frontRightMotor The front right SpeedController object used to drive the robot.
    */
    public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
                      SpeedController frontRightMotor, SpeedController rearRightMotor) {
        if (frontLeftMotor == null || rearLeftMotor == null || frontRightMotor == null || rearRightMotor == null) {
            m_frontLeftMotor = m_rearLeftMotor = m_frontRightMotor = m_rearRightMotor = null;
            throw new NullPointerException("Null motor provided");
        }
        m_frontLeftMotor = frontLeftMotor;
        m_rearLeftMotor = rearLeftMotor;
        m_frontRightMotor = frontRightMotor;
        m_rearRightMotor = rearRightMotor;
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = false;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * 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.
    */
    public void drive(double outputMagnitude, double curve) {
        double leftOutput, rightOutput;

        if(!kArcadeRatioCurve_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeRatioCurve);
            kArcadeRatioCurve_Reported = true;
        }
        if (curve < 0) {
            double value = Math.log(-curve);
            double ratio = (value - m_sensitivity) / (value + m_sensitivity);
            if (ratio == 0) {
                ratio = .0000000001;
            }
            leftOutput = outputMagnitude / ratio;
            rightOutput = outputMagnitude;
        } else if (curve > 0) {
            double value = Math.log(curve);
            double 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);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * drive the robot using two joystick inputs. The Y-axis will be selected from
    * each Joystick object.
    * @param leftStick The joystick to control the left side of the robot.
    * @param rightStick The joystick to control the right side of the robot.
    */
    public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getY(), rightStick.getY(), true);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * drive the robot using two joystick inputs. The Y-axis will be selected from
    * each Joystick object.
    * @param leftStick The joystick to control the left side of the robot.
    * @param rightStick The joystick to control the right side of the robot.
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * This function lets you pick the axis to be used on each Joystick object for the left
    * and right sides of the robot.
    * @param leftStick The Joystick object to use for the left side of the robot.
    * @param leftAxis The axis to select on the left side Joystick object.
    * @param rightStick The Joystick object to use for the right side of the robot.
    * @param rightAxis The axis to select on the right side Joystick object.
    */
    public void tankDrive(GenericHID leftStick, final int leftAxis,
                          GenericHID rightStick, final int rightAxis) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * This function lets you pick the axis to be used on each Joystick object for the left
    * and right sides of the robot.
    * @param leftStick The Joystick object to use for the left side of the robot.
    * @param leftAxis The axis to select on the left side Joystick object.
    * @param rightStick The Joystick object to use for the right side of the robot.
    * @param rightAxis The axis to select on the right side Joystick object.
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void tankDrive(GenericHID leftStick, final int leftAxis,
                          GenericHID rightStick, final int rightAxis, boolean squaredInputs) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
    }

    /**
    * 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.
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {

        if(!kTank_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_Tank);
            kTank_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);
    }

    /**
    * 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.
    */
    public void tankDrive(double leftValue, double rightValue) {
        tankDrive(leftValue, rightValue, true);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
    * for the rotate value.
    * (Should add more information here regarding the way that arcade drive works.)
    * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
    * for forwards/backwards and the X-axis will be selected for rotation rate.
    * @param squaredInputs If true, the sensitivity will be decreased for small values
    */
    public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
        // simply call the full-featured arcadeDrive with the appropriate values
        arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
    * for the rotate value.
    * (Should add more information here regarding the way that arcade drive works.)
    * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
    * for forwards/backwards and the X-axis will be selected for rotation rate.
    */
    public void arcadeDrive(GenericHID stick) {
        this.arcadeDrive(stick, true);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given two joystick instances and two axis, compute the values to send to either two
    * or four motors.
    * @param moveStick The Joystick object that represents the forward/backward direction
    * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
    * @param rotateStick The Joystick object that represents the rotation value
    * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void arcadeDrive(GenericHID moveStick, final int moveAxis,
                            GenericHID rotateStick, final int rotateAxis,
                            boolean squaredInputs) {
        double moveValue = moveStick.getRawAxis(moveAxis);
        double rotateValue = rotateStick.getRawAxis(rotateAxis);

        arcadeDrive(moveValue, rotateValue, squaredInputs);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given two joystick instances and two axis, compute the values to send to either two
    * or four motors.
    * @param moveStick The Joystick object that represents the forward/backward direction
    * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
    * @param rotateStick The Joystick object that represents the rotation value
    * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
    */
    public void arcadeDrive(GenericHID moveStick, final int moveAxis,
                            GenericHID rotateStick, final int rotateAxis) {
        this.arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
    }

    /**
    * Arcade drive implements single stick driving.
    * This function lets you directly provide joystick values from any source.
    * @param moveValue The value to use for forwards/backwards
    * @param rotateValue The value to use for the rotate right/left
    * @param squaredInputs If set, decreases the sensitivity at low speeds
    */
    public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
        // local variables to hold the computed PWM values for the motors
        if(!kArcadeStandard_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeStandard);
            kArcadeStandard_Reported = true;
        }

        double leftMotorSpeed;
        double rightMotorSpeed;

        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) {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = Math.max(moveValue, rotateValue);
            } else {
                leftMotorSpeed = Math.max(moveValue, -rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            }
        } else {
            if (rotateValue > 0.0) {
                leftMotorSpeed = -Math.max(-moveValue, rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            } else {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
            }
        }

        setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
    }

    /**
    * 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
    */
    public void arcadeDrive(double moveValue, double rotateValue) {
        this.arcadeDrive(moveValue, rotateValue, true);
    }

    /**
    * 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.
    */
    public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
        if(!kMecanumCartesian_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumCartesian);
            kMecanumCartesian_Reported = true;
        }
        double xIn = x;
        double yIn = y;
        // Negate y for the joystick.
        yIn = -yIn;
        // Compenstate for gyro angle.
        double rotated[] = rotateVector(xIn, yIn, gyroAngle);
        xIn = rotated[0];
        yIn = rotated[1];

        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
        wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation;
        wheelSpeeds[MotorType.kFrontRight_val] = -xIn + yIn - rotation;
        wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation;
        wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation;

        normalize(wheelSpeeds);

        byte syncGroup = (byte)0x80;

        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);

        if (m_isCANInitialized) {
            try {
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (CANNotInitializedException e) {
                m_isCANInitialized = false;
            }
        }

        if (m_safetyHelper != null) 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.
    *
    * @param magnitude The speed that the robot should drive in a given direction.
    * @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]
    */
    public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
        if(!kMecanumPolar_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumPolar);
            kMecanumPolar_Reported = true;
        }
        double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed;
        // Normalized for full power along the Cartesian axes.
        magnitude = limit(magnitude) * Math.sqrt(2.0);
        // The rollers are at 45 degree angles.
        double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
        double cosD = Math.cos(dirInRad);
        double sinD = Math.sin(dirInRad);

        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
        wheelSpeeds[MotorType.kFrontLeft_val] = (sinD * magnitude + rotation);
        wheelSpeeds[MotorType.kFrontRight_val] = (cosD * magnitude - rotation);
        wheelSpeeds[MotorType.kRearLeft_val] = (cosD * magnitude + rotation);
        wheelSpeeds[MotorType.kRearRight_val] = (sinD * magnitude - rotation);

        normalize(wheelSpeeds);

        byte syncGroup = (byte)0x80;

        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);

        if (m_isCANInitialized) {
            try {
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (CANNotInitializedException e) {
                m_isCANInitialized = false;
            }
        }

        if (m_safetyHelper != null) m_safetyHelper.feed();
    }

    /**
    * Holonomic Drive method for Mecanum wheeled robots.
    *
    * This is an alias to mecanumDrive_Polar() for backward compatability
    *
    * @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. 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 holonomicDrive(float magnitude, float direction, float rotation) {
        mecanumDrive_Polar(magnitude, direction, rotation);
    }

    /** Set the speed of the right and left motors.
    * This is used once an appropriate drive setup function is called such as
    * twoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed"
    * and includes flipping the direction of one side for opposing motors.
    * @param leftOutput The speed to send to the left side of the robot.
    * @param rightOutput The speed to send to the right side of the robot.
    */
    public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
        if (m_rearLeftMotor == null || m_rearRightMotor == null) {
            throw new NullPointerException("Null motor provided");
        }

        byte syncGroup = (byte)0x80;

        if (m_frontLeftMotor != null) {
            m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
        }
        m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);

        if (m_frontRightMotor != null) {
            m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
        }
        m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);

        if (m_isCANInitialized) {
            try {
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (CANNotInitializedException e) {
                m_isCANInitialized = false;
            }
        }

        if (m_safetyHelper != null) m_safetyHelper.feed();
    }

    /**
    * Limit motor values to the -1.0 to +1.0 range.
    */
    protected static double limit(double num) {
        if (num > 1.0) {
            return 1.0;
        }
        if (num < -1.0) {
            return -1.0;
        }
        return num;
    }

    /**
    * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
    */
    protected static void normalize(double wheelSpeeds[]) {
        double maxMagnitude = Math.abs(wheelSpeeds[0]);
        int i;
        for (i=1; i<kMaxNumberOfMotors; i++) {
            double temp = Math.abs(wheelSpeeds[i]);
            if (maxMagnitude < temp) maxMagnitude = temp;
        }
        if (maxMagnitude > 1.0) {
            for (i=0; i<kMaxNumberOfMotors; i++) {
                wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
            }
        }
    }

    /**
    * Rotate a vector in Cartesian space.
    */
    protected static double[] rotateVector(double x, double y, double angle) {
        double cosA = Math.cos(angle * (3.14159 / 180.0));
        double sinA = Math.sin(angle * (3.14159 / 180.0));
        double out[] = new double[2];
        out[0] = x * cosA - y * sinA;
        out[1] = x * sinA + y * cosA;
        return out;
    }

    /**
    * Invert a motor direction.
    * This is used when a motor should run in the opposite direction as the drive
    * code would normally run it. Motors that are direct drive would be inverted, the
    * drive code assumes that the motors are geared with one reversal.
    * @param motor The motor index to invert.
    * @param isInverted True if the motor should be inverted when operated.
    */
    public void setInvertedMotor(MotorType motor, boolean isInverted) {
        m_invertedMotors[motor.value] = isInverted ? -1 : 1;
    }

    /**
    * Set the turning sensitivity.
    *
    * This only impacts the drive() entry-point.
    * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
    */
    public void setSensitivity(double sensitivity) {
        m_sensitivity = sensitivity;
    }

    /**
    * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.
    * @param maxOutput Multiplied with the output percentage computed by the drive functions.
    */
    public void setMaxOutput(double maxOutput) {
        m_maxOutput = maxOutput;
    }


    /**
    * Free the speed controllers if they were allocated locally
    */
    public void free() {
        if (m_allocatedSpeedControllers) {
            if (m_frontLeftMotor != null) {
                ((PWM) m_frontLeftMotor).free();
            }
            if (m_frontRightMotor != null) {
                ((PWM) m_frontRightMotor).free();
            }
            if (m_rearLeftMotor != null) {
                ((PWM) m_rearLeftMotor).free();
            }
            if (m_rearRightMotor != null) {
                ((PWM) m_rearRightMotor).free();
            }
        }
    }

    public void setExpiration(double timeout) {
        m_safetyHelper.setExpiration(timeout);
    }

    public double getExpiration() {
        return m_safetyHelper.getExpiration();
    }

    public boolean isAlive() {
        return m_safetyHelper.isAlive();
    }

    public boolean isSafetyEnabled() {
        return m_safetyHelper.isSafetyEnabled();
    }

    public void setSafetyEnabled(boolean enabled) {
        m_safetyHelper.setSafetyEnabled(enabled);
    }

    public String getDescription() {
        return "Robot Drive";
    }

    public void stopMotor() {
        if (m_frontLeftMotor != null) {
            m_frontLeftMotor.set(0.0);
        }
        if (m_frontRightMotor != null) {
            m_frontRightMotor.set(0.0);
        }
        if (m_rearLeftMotor != null) {
            m_rearLeftMotor.set(0.0);
        }
        if (m_rearRightMotor != null) {
            m_rearRightMotor.set(0.0);
        }
        if (m_safetyHelper != null) m_safetyHelper.feed();
    }

    private void setupMotorSafety() {
        m_safetyHelper = new MotorSafetyHelper(this);
        m_safetyHelper.setExpiration(kDefaultExpirationTime);
        m_safetyHelper.setSafetyEnabled(true);
    }

    protected int getNumMotors() {
        int motors = 0;
        if (m_frontLeftMotor != null) motors++;
        if (m_frontRightMotor != null) motors++;
        if (m_rearLeftMotor != null) motors++;
        if (m_rearRightMotor != null) motors++;
        return motors;
    }
}
Can you provide the pwm channels that you are using and the corresponding motors. (IE: 0=frontLeft)

It seems to me that the RobotDrive class inverts motors for you depending on the amount of motors you use. This is because if you have 4 motors (2 on one side -- 2 on the other). To drive straight, one side spins clockwise while the other spins counter-clockwise. So, the class will invert the last two motors.

So: motors(0-3) will invert motors 2 and 3
motors(1-4) will invert motors 3 and 4

Is it possible that you edited the RobotDrive code? If yes, then you should find an unadulterated version of that code.

Fletch1373 16-02-2015 14:41
Re: Strafing Problem
 
Quote:
Originally Posted by JacobD (Post 1445053)
Take a look at the RobotDrive Class. After reading through it, there are very few places you can go wrong.

Code:

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code  */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                              */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;

import edu.wpi.first.wpilibj.can.CANNotInitializedException;
import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tInstances;
import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
import edu.wpi.first.wpilibj.communication.UsageReporting;

/**
 * Utility class for handling Robot drive based on a definition of the motor configuration.
 * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and
 * mecanum drive trains are supported. In the future other drive types like swerve might
 * be implemented. Motor channel numbers are supplied on creation of the class. Those are
 * used for either the drive function (intended for hand created drive code, such as autonomous)
 * or with the Tank/Arcade functions intended to be used for Operator Control driving.
 */
public class RobotDrive implements MotorSafety {

    protected MotorSafetyHelper m_safetyHelper;

    /**
    * The location of a motor on the robot for the purpose of driving
    */
    public static class MotorType {

        /**
        * The integer value representing this enumeration
        */
        public final int value;
        static final int kFrontLeft_val = 0;
        static final int kFrontRight_val = 1;
        static final int kRearLeft_val = 2;
        static final int kRearRight_val = 3;
        /**
        * motortype: front left
        */
        public static final MotorType kFrontLeft = new MotorType(kFrontLeft_val);
        /**
        * motortype: front right
        */
        public static final MotorType kFrontRight = new MotorType(kFrontRight_val);
        /**
        * motortype: rear left
        */
        public static final MotorType kRearLeft = new MotorType(kRearLeft_val);
        /**
        * motortype: rear right
        */
        public static final MotorType kRearRight = new MotorType(kRearRight_val);

        private MotorType(int value) {
            this.value = value;
        }
    }
    public static final double kDefaultExpirationTime = 0.1;
    public static final double kDefaultSensitivity = 0.5;
    public static final double kDefaultMaxOutput = 1.0;
    protected static final int kMaxNumberOfMotors = 4;
    protected final int m_invertedMotors[] = new int[4];
    protected double m_sensitivity;
    protected double m_maxOutput;
    protected SpeedController m_frontLeftMotor;
    protected SpeedController m_frontRightMotor;
    protected SpeedController m_rearLeftMotor;
    protected SpeedController m_rearRightMotor;
    protected boolean m_allocatedSpeedControllers;
    protected boolean m_isCANInitialized = false;//TODO: fix can
    protected static boolean kArcadeRatioCurve_Reported = false;
    protected static boolean kTank_Reported = false;
    protected static boolean kArcadeStandard_Reported = false;
    protected static boolean kMecanumCartesian_Reported = false;
    protected static boolean kMecanumPolar_Reported = false;

    /** Constructor for RobotDrive with 2 motors specified with channel numbers.
    * Set up parameters for a two wheel drive system where the
    * left and right motor pwm channels are specified in the call.
    * This call assumes Talons for controlling the motors.
    * @param leftMotorChannel The PWM channel number that drives the left motor.
    * @param rightMotorChannel The PWM channel number that drives the right motor.
    */
    public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        m_frontLeftMotor = null;
        m_rearLeftMotor = new Talon(leftMotorChannel);
        m_frontRightMotor = null;
        m_rearRightMotor = new Talon(rightMotorChannel);
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = true;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * Constructor for RobotDrive with 4 motors specified with channel numbers.
    * Set up parameters for a four wheel drive system where all four motor
    * pwm channels are specified in the call.
    * This call assumes Talons for controlling the motors.
    * @param frontLeftMotor Front left motor channel number
    * @param rearLeftMotor Rear Left motor channel number
    * @param frontRightMotor Front right motor channel number
    * @param rearRightMotor Rear Right motor channel number
    */
    public RobotDrive(final int frontLeftMotor, final int rearLeftMotor,
                      final int frontRightMotor, final int rearRightMotor) {
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        m_rearLeftMotor = new Talon(rearLeftMotor);
        m_rearRightMotor = new Talon(rearRightMotor);
        m_frontLeftMotor = new Talon(frontLeftMotor);
        m_frontRightMotor = new Talon(frontRightMotor);
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = true;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * Constructor for RobotDrive with 2 motors specified as SpeedController objects.
    * The SpeedController version of the constructor enables programs to use the RobotDrive classes with
    * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
    * the curve to suit motor bias or dead-band elimination.
    * @param leftMotor The left SpeedController object used to drive the robot.
    * @param rightMotor the right SpeedController object used to drive the robot.
    */
    public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
        if (leftMotor == null || rightMotor == null) {
            m_rearLeftMotor = m_rearRightMotor = null;
            throw new NullPointerException("Null motor provided");
        }
        m_frontLeftMotor = null;
        m_rearLeftMotor = leftMotor;
        m_frontRightMotor = null;
        m_rearRightMotor = rightMotor;
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = false;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * Constructor for RobotDrive with 4 motors specified as SpeedController objects.
    * Speed controller input version of RobotDrive (see previous comments).
    * @param rearLeftMotor The back left SpeedController object used to drive the robot.
    * @param frontLeftMotor The front left SpeedController object used to drive the robot
    * @param rearRightMotor The back right SpeedController object used to drive the robot.
    * @param frontRightMotor The front right SpeedController object used to drive the robot.
    */
    public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
                      SpeedController frontRightMotor, SpeedController rearRightMotor) {
        if (frontLeftMotor == null || rearLeftMotor == null || frontRightMotor == null || rearRightMotor == null) {
            m_frontLeftMotor = m_rearLeftMotor = m_frontRightMotor = m_rearRightMotor = null;
            throw new NullPointerException("Null motor provided");
        }
        m_frontLeftMotor = frontLeftMotor;
        m_rearLeftMotor = rearLeftMotor;
        m_frontRightMotor = frontRightMotor;
        m_rearRightMotor = rearRightMotor;
        m_sensitivity = kDefaultSensitivity;
        m_maxOutput = kDefaultMaxOutput;
        for (int i = 0; i < kMaxNumberOfMotors; i++) {
            m_invertedMotors[i] = 1;
        }
        m_allocatedSpeedControllers = false;
        setupMotorSafety();
        drive(0, 0);
    }

    /**
    * 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.
    */
    public void drive(double outputMagnitude, double curve) {
        double leftOutput, rightOutput;

        if(!kArcadeRatioCurve_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeRatioCurve);
            kArcadeRatioCurve_Reported = true;
        }
        if (curve < 0) {
            double value = Math.log(-curve);
            double ratio = (value - m_sensitivity) / (value + m_sensitivity);
            if (ratio == 0) {
                ratio = .0000000001;
            }
            leftOutput = outputMagnitude / ratio;
            rightOutput = outputMagnitude;
        } else if (curve > 0) {
            double value = Math.log(curve);
            double 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);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * drive the robot using two joystick inputs. The Y-axis will be selected from
    * each Joystick object.
    * @param leftStick The joystick to control the left side of the robot.
    * @param rightStick The joystick to control the right side of the robot.
    */
    public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getY(), rightStick.getY(), true);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * drive the robot using two joystick inputs. The Y-axis will be selected from
    * each Joystick object.
    * @param leftStick The joystick to control the left side of the robot.
    * @param rightStick The joystick to control the right side of the robot.
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * This function lets you pick the axis to be used on each Joystick object for the left
    * and right sides of the robot.
    * @param leftStick The Joystick object to use for the left side of the robot.
    * @param leftAxis The axis to select on the left side Joystick object.
    * @param rightStick The Joystick object to use for the right side of the robot.
    * @param rightAxis The axis to select on the right side Joystick object.
    */
    public void tankDrive(GenericHID leftStick, final int leftAxis,
                          GenericHID rightStick, final int rightAxis) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
    }

    /**
    * Provide tank steering using the stored robot configuration.
    * This function lets you pick the axis to be used on each Joystick object for the left
    * and right sides of the robot.
    * @param leftStick The Joystick object to use for the left side of the robot.
    * @param leftAxis The axis to select on the left side Joystick object.
    * @param rightStick The Joystick object to use for the right side of the robot.
    * @param rightAxis The axis to select on the right side Joystick object.
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void tankDrive(GenericHID leftStick, final int leftAxis,
                          GenericHID rightStick, final int rightAxis, boolean squaredInputs) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
    }

    /**
    * 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.
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {

        if(!kTank_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_Tank);
            kTank_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);
    }

    /**
    * 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.
    */
    public void tankDrive(double leftValue, double rightValue) {
        tankDrive(leftValue, rightValue, true);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
    * for the rotate value.
    * (Should add more information here regarding the way that arcade drive works.)
    * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
    * for forwards/backwards and the X-axis will be selected for rotation rate.
    * @param squaredInputs If true, the sensitivity will be decreased for small values
    */
    public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
        // simply call the full-featured arcadeDrive with the appropriate values
        arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
    * for the rotate value.
    * (Should add more information here regarding the way that arcade drive works.)
    * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
    * for forwards/backwards and the X-axis will be selected for rotation rate.
    */
    public void arcadeDrive(GenericHID stick) {
        this.arcadeDrive(stick, true);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given two joystick instances and two axis, compute the values to send to either two
    * or four motors.
    * @param moveStick The Joystick object that represents the forward/backward direction
    * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
    * @param rotateStick The Joystick object that represents the rotation value
    * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
    * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
    */
    public void arcadeDrive(GenericHID moveStick, final int moveAxis,
                            GenericHID rotateStick, final int rotateAxis,
                            boolean squaredInputs) {
        double moveValue = moveStick.getRawAxis(moveAxis);
        double rotateValue = rotateStick.getRawAxis(rotateAxis);

        arcadeDrive(moveValue, rotateValue, squaredInputs);
    }

    /**
    * Arcade drive implements single stick driving.
    * Given two joystick instances and two axis, compute the values to send to either two
    * or four motors.
    * @param moveStick The Joystick object that represents the forward/backward direction
    * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
    * @param rotateStick The Joystick object that represents the rotation value
    * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
    */
    public void arcadeDrive(GenericHID moveStick, final int moveAxis,
                            GenericHID rotateStick, final int rotateAxis) {
        this.arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
    }

    /**
    * Arcade drive implements single stick driving.
    * This function lets you directly provide joystick values from any source.
    * @param moveValue The value to use for forwards/backwards
    * @param rotateValue The value to use for the rotate right/left
    * @param squaredInputs If set, decreases the sensitivity at low speeds
    */
    public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
        // local variables to hold the computed PWM values for the motors
        if(!kArcadeStandard_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeStandard);
            kArcadeStandard_Reported = true;
        }

        double leftMotorSpeed;
        double rightMotorSpeed;

        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) {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = Math.max(moveValue, rotateValue);
            } else {
                leftMotorSpeed = Math.max(moveValue, -rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            }
        } else {
            if (rotateValue > 0.0) {
                leftMotorSpeed = -Math.max(-moveValue, rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            } else {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
            }
        }

        setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
    }

    /**
    * 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
    */
    public void arcadeDrive(double moveValue, double rotateValue) {
        this.arcadeDrive(moveValue, rotateValue, true);
    }

    /**
    * 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.
    */
    public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
        if(!kMecanumCartesian_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumCartesian);
            kMecanumCartesian_Reported = true;
        }
        double xIn = x;
        double yIn = y;
        // Negate y for the joystick.
        yIn = -yIn;
        // Compenstate for gyro angle.
        double rotated[] = rotateVector(xIn, yIn, gyroAngle);
        xIn = rotated[0];
        yIn = rotated[1];

        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
        wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation;
        wheelSpeeds[MotorType.kFrontRight_val] = -xIn + yIn - rotation;
        wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation;
        wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation;

        normalize(wheelSpeeds);

        byte syncGroup = (byte)0x80;

        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);

        if (m_isCANInitialized) {
            try {
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (CANNotInitializedException e) {
                m_isCANInitialized = false;
            }
        }

        if (m_safetyHelper != null) 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.
    *
    * @param magnitude The speed that the robot should drive in a given direction.
    * @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]
    */
    public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
        if(!kMecanumPolar_Reported) {
            UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumPolar);
            kMecanumPolar_Reported = true;
        }
        double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed;
        // Normalized for full power along the Cartesian axes.
        magnitude = limit(magnitude) * Math.sqrt(2.0);
        // The rollers are at 45 degree angles.
        double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
        double cosD = Math.cos(dirInRad);
        double sinD = Math.sin(dirInRad);

        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
        wheelSpeeds[MotorType.kFrontLeft_val] = (sinD * magnitude + rotation);
        wheelSpeeds[MotorType.kFrontRight_val] = (cosD * magnitude - rotation);
        wheelSpeeds[MotorType.kRearLeft_val] = (cosD * magnitude + rotation);
        wheelSpeeds[MotorType.kRearRight_val] = (sinD * magnitude - rotation);

        normalize(wheelSpeeds);

        byte syncGroup = (byte)0x80;

        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);

        if (m_isCANInitialized) {
            try {
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (CANNotInitializedException e) {
                m_isCANInitialized = false;
            }
        }

        if (m_safetyHelper != null) m_safetyHelper.feed();
    }

    /**
    * Holonomic Drive method for Mecanum wheeled robots.
    *
    * This is an alias to mecanumDrive_Polar() for backward compatability
    *
    * @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. 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 holonomicDrive(float magnitude, float direction, float rotation) {
        mecanumDrive_Polar(magnitude, direction, rotation);
    }

    /** Set the speed of the right and left motors.
    * This is used once an appropriate drive setup function is called such as
    * twoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed"
    * and includes flipping the direction of one side for opposing motors.
    * @param leftOutput The speed to send to the left side of the robot.
    * @param rightOutput The speed to send to the right side of the robot.
    */
    public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
        if (m_rearLeftMotor == null || m_rearRightMotor == null) {
            throw new NullPointerException("Null motor provided");
        }

        byte syncGroup = (byte)0x80;

        if (m_frontLeftMotor != null) {
            m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
        }
        m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);

        if (m_frontRightMotor != null) {
            m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
        }
        m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);

        if (m_isCANInitialized) {
            try {
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (CANNotInitializedException e) {
                m_isCANInitialized = false;
            }
        }

        if (m_safetyHelper != null) m_safetyHelper.feed();
    }

    /**
    * Limit motor values to the -1.0 to +1.0 range.
    */
    protected static double limit(double num) {
        if (num > 1.0) {
            return 1.0;
        }
        if (num < -1.0) {
            return -1.0;
        }
        return num;
    }

    /**
    * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
    */
    protected static void normalize(double wheelSpeeds[]) {
        double maxMagnitude = Math.abs(wheelSpeeds[0]);
        int i;
        for (i=1; i<kMaxNumberOfMotors; i++) {
            double temp = Math.abs(wheelSpeeds[i]);
            if (maxMagnitude < temp) maxMagnitude = temp;
        }
        if (maxMagnitude > 1.0) {
            for (i=0; i<kMaxNumberOfMotors; i++) {
                wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
            }
        }
    }

    /**
    * Rotate a vector in Cartesian space.
    */
    protected static double[] rotateVector(double x, double y, double angle) {
        double cosA = Math.cos(angle * (3.14159 / 180.0));
        double sinA = Math.sin(angle * (3.14159 / 180.0));
        double out[] = new double[2];
        out[0] = x * cosA - y * sinA;
        out[1] = x * sinA + y * cosA;
        return out;
    }

    /**
    * Invert a motor direction.
    * This is used when a motor should run in the opposite direction as the drive
    * code would normally run it. Motors that are direct drive would be inverted, the
    * drive code assumes that the motors are geared with one reversal.
    * @param motor The motor index to invert.
    * @param isInverted True if the motor should be inverted when operated.
    */
    public void setInvertedMotor(MotorType motor, boolean isInverted) {
        m_invertedMotors[motor.value] = isInverted ? -1 : 1;
    }

    /**
    * Set the turning sensitivity.
    *
    * This only impacts the drive() entry-point.
    * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
    */
    public void setSensitivity(double sensitivity) {
        m_sensitivity = sensitivity;
    }

    /**
    * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.
    * @param maxOutput Multiplied with the output percentage computed by the drive functions.
    */
    public void setMaxOutput(double maxOutput) {
        m_maxOutput = maxOutput;
    }


    /**
    * Free the speed controllers if they were allocated locally
    */
    public void free() {
        if (m_allocatedSpeedControllers) {
            if (m_frontLeftMotor != null) {
                ((PWM) m_frontLeftMotor).free();
            }
            if (m_frontRightMotor != null) {
                ((PWM) m_frontRightMotor).free();
            }
            if (m_rearLeftMotor != null) {
                ((PWM) m_rearLeftMotor).free();
            }
            if (m_rearRightMotor != null) {
                ((PWM) m_rearRightMotor).free();
            }
        }
    }

    public void setExpiration(double timeout) {
        m_safetyHelper.setExpiration(timeout);
    }

    public double getExpiration() {
        return m_safetyHelper.getExpiration();
    }

    public boolean isAlive() {
        return m_safetyHelper.isAlive();
    }

    public boolean isSafetyEnabled() {
        return m_safetyHelper.isSafetyEnabled();
    }

    public void setSafetyEnabled(boolean enabled) {
        m_safetyHelper.setSafetyEnabled(enabled);
    }

    public String getDescription() {
        return "Robot Drive";
    }

    public void stopMotor() {
        if (m_frontLeftMotor != null) {
            m_frontLeftMotor.set(0.0);
        }
        if (m_frontRightMotor != null) {
            m_frontRightMotor.set(0.0);
        }
        if (m_rearLeftMotor != null) {
            m_rearLeftMotor.set(0.0);
        }
        if (m_rearRightMotor != null) {
            m_rearRightMotor.set(0.0);
        }
        if (m_safetyHelper != null) m_safetyHelper.feed();
    }

    private void setupMotorSafety() {
        m_safetyHelper = new MotorSafetyHelper(this);
        m_safetyHelper.setExpiration(kDefaultExpirationTime);
        m_safetyHelper.setSafetyEnabled(true);
    }

    protected int getNumMotors() {
        int motors = 0;
        if (m_frontLeftMotor != null) motors++;
        if (m_frontRightMotor != null) motors++;
        if (m_rearLeftMotor != null) motors++;
        if (m_rearRightMotor != null) motors++;
        return motors;
    }
}
Can you provide the pwm channels that you are using and the corresponding motors. (IE: 0=frontLeft)

It seems to me that the RobotDrive class inverts motors for you depending on the amount of motors you use. This is because if you have 4 motors (2 on one side -- 2 on the other). To drive straight, one side spins clockwise while the other spins counter-clockwise. So, the class will invert the last two motors.

So: motors(0-3) will invert motors 2 and 3
motors(1-4) will invert motors 3 and 4

Is it possible that you edited the RobotDrive code? If yes, then you should find an unadulterated version of that code.
Jacob, where in there do you see it inverting motors for you? I may blind, but I didn't see it. I see a function that's made available for you(setInvertedMotor) to manually invert specific ones.

JacobD 16-02-2015 14:49
Re: Strafing Problem
 
Quote:
Originally Posted by Fletch1373 (Post 1445149)
Jacob, where in there do you see it inverting motors for you? I may blind, but I didn't see it. I see a function that's made available for you(setInvertedMotor) to manually invert specific ones.
Not exactly inverting but taking the negative value for those motors.

cstelter 16-02-2015 20:01
Re: Strafing Problem
 
I'm kind of lost in all the help that is being given. Why are folks talking about PWM=0 channels and the like when he is instantiating CANTalons? His ports are clearly 1=LF, 2=LR, 3=RF, 4=R and that is what should be checked for on the robot. If the talons feeding the motors are not numbered that way, then it could explain the issue. Our team made that mistake too and it can behave just as described. Having X from above and Diamond from below wrong can also lead to this.

MecanumDrive_Cartesian is documented thus:

/**
* 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.
*/


so according to that, all these negations should not be necessary unless the joystick is something exotic that inverts everything from typical -1, -1 being in the upper left corner.

Code:
            x = -leftJoystick.getX();
            y = -leftJoystick.getY();
            rotation = -rightJoystick.getX();
If they were necessary to make it drive right, the problem is still likely in your mechanism and wiring as the code says inverting x and y should not be necessary. I know it is not necessary if you are using the Logitech 3D or the older black joystick.

Our geaboxes (CIM Mini's I think') drive in the forward direction when positive voltage is applied when mounted on the left side. Since the same boxes are rotated 180 degrees on the right they drive in reverse with positive voltage. So one must tell the RobotDrive to invert the motors on the right side for our bot. JacobD gave good advice on how to do that-- his robot's gearing must be opposite of ours as they had to invert the left side.

Not adding the gyro until you have the rest sorted out was good advice too. By sending 0, it drives relative to the robot and you can be sure things are right.

Ether's advice and chart is golden too.

I also agree with Fletch1373 that RobotDrive initializes all motors in the invertedMotors array to +1 which means no inversion. The only way to flip one to -1 is to call invertMotor.

But folks asking for code (I believe he posted it in the original post, no?) and all this talk about PWM channels and especially which channel goes with which motor is irrelevant as long as the motors are wired according to the code. One could use channels/ports 5,8,2,1 for LF,LR,RF,RR and as long as the bot is actually wired that way and as long as the SpeedControllers are passed to RobotDrive in the correct order that the constructor requires them, it should not matter what the numbers are. This has me hoping the OP is not more confused than when he came here.

Just trying to clarify things. Hopefully I didn't miss something and will only be further muddying the waters, but it seems pretty clear.

Oh-- one thing I did spot in the code is that the code is allocating Joystick(1) and (2), instead of the standard (0) and (1)--. That's not an issue if you have joysticks at #1 and #2, but it's not typical.

Ether 16-02-2015 21:56
Re: Strafing Problem
 
1 Attachment(s)

The OP has stopped posting so the following will be speculative.

IF his wheel directions look like the attachment, then -- and I really hate to suggest this -- he can fix the problem until he can clean up the code and make it right by making two small changes in the kinematic calculations in mecanumDrive_Cartesian() in RobotDrive.java:

Code:
                        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
        wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation;
        wheelSpeeds[MotorType.kFrontRight_val] = +xIn + yIn - rotation;
        wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation;
        wheelSpeeds[MotorType.kRearRight_val] = -xIn + yIn - rotation;


All times are GMT -5. The time now is 10:56.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi