Chief Delphi - Getting Gyro Working w/ Mecanum Drive

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)

- - Getting Gyro Working w/ Mecanum Drive (http://www.chiefdelphi.com/forums/showthread.php?t=136764)

WillNess

19-04-2015 13:53

Getting Gyro Working w/ Mecanum Drive

Although our season is over, the programming team is still tasked with completing several things before the start of the next season. One of the more difficult things (and something I struggled with implementing) during the build season was using a gyro to help the drivers. This is the code I've come up with so far, but I only included the drive code, nothing else:

Code:

teleopPeriodic(){

   driveCode();

}

    public static void driveCode(){

            RobotMap.calculateTurnSpeedsAndAngles();

        if(OI.stick.getZ() < .1){

                double subtractValue = 0; 

                if(Robot.turnSpeedForGyroCorrection > .3){

                        subtractValue = Robot.turnSpeedForGyroCorrection - .3;

                }else{

                         //I do not want to robot traveling past 30%, so this subtractValue will never let it go past 50%

                        subtractValue = 0;

                }

                RobotMap.setVelocityGyro(OI.stick.getX(), OI.stick.getY(), OI.stick.getZ());

        }else{

                RobotMap.setVelocity(OI.stick.getX(), OI.stick.getY(), OI.stick.getZ());

                Robot.setAngle = RobotMap.driveGyro.getAngle();

        }

    }



public static void setVelocity(double x,double y,double z){

                double fMax = Math.sin(-0*Math.PI/4+1*Math.PI/4);

                double fR = Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;

                double rL = -Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;

                double fL = -Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;

                double rR = Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;

            if(Math.abs(z)<.1){

                    drivefrontLeft.set(fL);

                    drivefrontRight.set(fR);

                    driverearLeft.set(rL);

                    driverearRight.set(rR);

            }else{

                    drivefrontLeft.set(fL+z/2);

                    drivefrontRight.set(fR+z/2);

                    driverearLeft.set(rL+z/2);

                    driverearRight.set(rR+z/2);

            }

    }

    public static void setVelocityGyro(double x,double y,double z){

                    RobotMap.calculateTurnSpeedsAndAngles();

                double fMax = Math.sin(-0*Math.PI/4+1*Math.PI/4);

                double fR = Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;

                double rL = -Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;

                double fL = -Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;

                double rR = Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;

                double errorOfAngles = Robot.actualAngle - Robot.setAngle;

                double turnSpeedRight = 0;

                double turnSpeedLeft = 0;

                if(errorOfAngles > 0){

                        if(fL > 0){

                                turnSpeedLeft = Math.abs(Robot.turnSpeedForGyroCorrection);

                        }else{

                                turnSpeedLeft = -Math.abs(Robot.turnSpeedForGyroCorrection);

                        }

                        turnSpeedRight = 0;

                }else if(errorOfAngles < 0){

                        turnSpeedLeft = 0;

                        if(fR > 0){

                                turnSpeedRight = Math.abs(Robot.turnSpeedForGyroCorrection);

                        }else{

                                turnSpeedRight = -Math.abs(Robot.turnSpeedForGyroCorrection);

                        }

                }else{

                        turnSpeedLeft = 0;

                        turnSpeedRight = 0;

                }

            drivefrontLeft.set(fL - turnSpeedLeft);

            drivefrontRight.set(fR - turnSpeedRight);

            driverearLeft.set(rL - turnSpeedLeft);

            driverearRight.set(rR - turnSpeedRight);

    } 

    public static void calculateTurnSpeedsAndAngles(){

            double turnValue = 0.02;

            Robot.actualAngle = RobotMap.driveGyro.getAngle();

            double angleError = Robot.actualAngle - Robot.setAngle;

            Robot.turnSpeedForGyroCorrection = angleError * turnValue;

    }

Sorry for the weird spacing, that's just how the copy+paste did it. Any help would be appreciated! Also: This is untested

Ether

19-04-2015 14:30

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1473486)

One of the more difficult things (and something I struggled with implementing) during the build season was using a gyro to help the drivers.

What do you mean by help the drivers ?

If your goal is to use the gyro to provide a field-centric driver interface, that's already part of the Java WPILib:

Code:





    /**

     * 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;

        }

... Or if your goal is to make the bot drive straight when the driver is commanding zero rotation, you could do something like this:

http://www.chiefdelphi.com/forums/at...4&d=1420598437

...Or here's an interesting optional (button-press selectable) driver interface mode for mec that your drivers might like:

Quote:

Halo Auto-Rotate (Halo-AR) Driver interface

A single 2-axis joystick does field-centric control of forward/reverse
and strafe right/left motion, while the robot automatically and
simultaneously rotates to face the direction it is commanded.

http://www.chiefdelphi.com/media/papers/download/3721

WillNess

19-04-2015 14:49

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by Ether (Post 1473495)

What do you mean by help the drivers ?

If your goal is to use the gyro to provide a field-centric driver interface, that's already part of the Java WPILib:

Code:





    /**

     * 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;

        }

Wait so I can just do
RobotMap.robotDrive.mecanumDrive_Cartesian(OI.joys tick0.getX(), OI.joystick0.getY(), OI.joystick0.getZ(), RobotMap.driveGyro.getAngle());
in my teleopPeriodic and it'll do corrections for me? So if I'm driving w/o rotating it'll auto correct for me?

Ether

19-04-2015 15:52

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1473502)

What you described above will make the driver interface field-centric:

Quote:

Originally Posted by Ether (Post 1473495)

If your goal is to use the gyro to provide a field-centric driver interface, that's already part of the Java WPILib:

Code:

* @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented controls.

If you want the vehicle not to rotate when the driver rotate command is zero, you could do something like this:

Quote:

Originally Posted by Ether (Post 1473495)

if your goal is to make the bot drive straight when the driver is commanding zero rotation, you could do something like this:

http://www.chiefdelphi.com/forums/at...4&d=1420598437

WillNess

19-04-2015 16:09

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by Ether (Post 1473530)

What you described above will make the driver interface field-centric:

If you want the vehicle not to rotate when the driver rotate command is zero, you could do something like this:

So by field-centric, the robot will ALWAYS face one way? How will the robot act when in this mode?

Ether

19-04-2015 17:05

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1473534)

So by field-centric, the robot will ALWAYS face one way?

No.

Check out some of these links.

WillNess

19-04-2015 20:12

Re: Getting Gyro Working w/ Mecanum Drive

I think I'm confused about what field-centric really is. To my understanding it's positioning based on the layout of the field. Could someone explain this to me?

Ether

19-04-2015 20:46

Re: Getting Gyro Working w/ Mecanum Drive

1 Attachment(s)

See the green arrow? Download that PDF and read Page 18.

WillNess

19-04-2015 21:02

Re: Getting Gyro Working w/ Mecanum Drive

Oh I see. That is not what I'm trying to accomplish. I am trying to get the robot not to rotate when the driver rotate command is zero. How do you accomplish this in Java, not in Labview?

Ether

19-04-2015 21:23

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1473710)

I am trying to get the robot not to rotate when the driver rotate command is zero. How do you accomplish this in Java

Like this:

Quote:

Originally Posted by Ether (Post 1473530)

If you want the vehicle not to rotate when the driver rotate command is zero, you could do something like this:

Quote:

Originally Posted by Ether (Post 1473495)

if your goal is to make the bot drive straight when the driver is commanding zero rotation, you could do something like this:

http://www.chiefdelphi.com/forums/at...4&d=1420598437

WillNess

19-04-2015 23:01

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by Ether (Post 1473725)

Like this:

Sorry but that doesn't really help me. Like I understand how it works but I don't know how I would implement this in Java.

Ether

19-04-2015 23:18

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1473819)

I don't know how I would implement this in Java.

Why? What part is not clear?

WillNess

19-04-2015 23:30

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by Ether (Post 1473829)

Why? What part is not clear?

The actual code I would write.

GeeTwo

21-04-2015 07:51

Re: Getting Gyro Working w/ Mecanum Drive

I recommend using one of the WPIlib MecanumDrive methods, and just make corrections to the "twist" input when you want to go straight based on the error. Do not use the fourth "gyroscope" input to MecanumDrive_cartesian; this is only used for "field coordinates".

The correction could be a full PID, but because of the amount of friction in a mecanum drive system, you should be able to get away with a simple proportional correction; D will be mechanical, and I is usually not needed when seeking a position.

To do this, you will need to create a numeric variable to hold the desired heading, say azi.
In your joystick-reading loop:

When the joystick values indicate that you do not wish to hold a constant facing, set azi to NaN, or some number outside of the valid range.
When the joystick values indicate that you do wish to hold a constant facing:
- If isnan(azi), set azi =gyro.getAngle()
- otherwise, add a correction of the form prop * (azi - gyro.getAngle()) to the "twist" value. You will have to tune the value of prop.

Debugging tip: If the robot starts spinning faster and faster, do a quick shut down, change the sign of "prop" and try again.

WillNess

21-04-2015 19:05

Re: Getting Gyro Working w/ Mecanum Drive

How do I set the robotdrive to the MecanumDrive method?

Quote:

Originally Posted by GeeTwo (Post 1474463)

When the joystick values indicate that you do not wish to hold a constant facing, set azi to NaN, or some number outside of the valid range.
When the joystick values indicate that you do wish to hold a constant facing:
- If isnan(azi), set azi =gyro.getAngle()
- otherwise, add a correction of the form prop * (azi - gyro.getAngle()) to the "twist" value. You will have to tune the value of prop.

Debugging tip: If the robot starts spinning faster and faster, do a quick shut down, change the sign of "prop" and try again.

GeeTwo

22-04-2015 03:00

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1474733)

How do I set the robotdrive to the MecanumDrive method?

mecanumDrive_cartesian() is a method within the standard RobotDrive class in both C++ and java WPIlibs. For what are hopefully obvious reasons, you must use one of the four-controller forms of the new RobotDrive() constructor in order to use the mecanum methods.

WillNess

26-04-2015 00:20

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by GeeTwo (Post 1474843)

So in my teleopPeriodic would I put:

Code:

RobotMap.driveRobotDrive41.mecanumDrive_Cartesian(OI.joystick0.getX(), OI.joystick0.getY(), OI.joystick0.getZ(), 0);

or where should I do that? Also, should I set the fourth variable to my Gyro Angle or to 0?

Ether

26-04-2015 08:48

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1476164)

should I set the fourth variable to my Gyro Angle or to 0?

As explained in earlier posts in this thread, passing the gyro angle to the fourth parameter gives you field-centric control. You said that's not what you want.

Quote:

Originally Posted by Ether (Post 1473530)

What you described above will make the driver interface field-centric

Quote:

Originally Posted by GeeTwo (Post 1474463)

Do not use the fourth "gyroscope" input to MecanumDrive_cartesian; this is only used for "field coordinates".

See the note "Optional Input for field-centric control of XY motion" in the code diagram linked in this post:

Quote:

Originally Posted by Ether (Post 1473495)

if your goal is to make the bot drive straight when the driver is commanding zero rotation, you could do something like this:

http://www.chiefdelphi.com/forums/at...4&d=1420598437

dawonn

26-04-2015 11:04

Re: Getting Gyro Working w/ Mecanum Drive

The code is not particularly well documented, but here's the code we used this year:

https://github.com/CopperBots/Copper...obot.java#L632

We pass in the rotation joystick value that has already been deadband'ed.
If the joystick is at zero rotation, we use a proportional controller to keep the robot straight.
If the joystick is not at zero, we update the current heading of the robot and pass the joystick value back to the drive system unchanged.

We also have the option to disable gyro mode at any time in case something goes wrong like a bad calibration, or a loose cable.
There is also an option to update the kP value from the smart dashboard and some diagnostic information sent back to help troubleshoot when things go wrong.

WillNess

26-04-2015 12:31

Re: Getting Gyro Working w/ Mecanum Drive

Well what I'm asking is what do I put in for the fourth parameter, because I have to put something in it.

Quote:

Originally Posted by Ether (Post 1476227)

As explained in earlier posts in this thread, passing the gyro angle to the fourth parameter gives you field-centric control. You said that's not what you want.

See the note "Optional Input for field-centric control of XY motion" in the code diagram linked in this post:

WillNess

26-04-2015 13:01

Re: Getting Gyro Working w/ Mecanum Drive

Thanks, this helps a BUNCH! The problem is mixing mecanum drive with your code

Quote:

Originally Posted by dawonn (Post 1476292)

Ether

26-04-2015 13:16

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by dawonn (Post 1476292)

If the joystick is at zero rotation, we use a proportional controller to keep the robot straight.
If the joystick is not at zero, we update the current heading of the robot and pass the joystick value back to the drive system unchanged

Sounds just like the note in the lower left corner of this code diagram :)

Ether

26-04-2015 13:28

Re: Getting Gyro Working w/ Mecanum Drive

Quote:

Originally Posted by WillNess (Post 1476319)

Well what I'm asking is what do I put in for the fourth parameter, because I have to put something in it.

Look at the WPILib code for mecanumDrive_Cartesian():

Code:

    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);

Now look at the WPILib code for rotateVector():

Code:

    /**

     * 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;

    }

The cosine of zero is 1. The sign of zero is 0. So passing zero as the fourth parameter to mecanumDrive_Cartesian() will result in no rotation of the vector, which is what you said you want.

WillNess

26-04-2015 13:35

Re: Getting Gyro Working w/ Mecanum Drive

Thank you Ether, that helps tons.

Quote:

Originally Posted by Ether (Post 1476343)

Look at the WPILib code for mecanumDrive_Cartesian():

Code:

    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);

Now look at the WPILib code for rotateVector():

Code:

    /**

     * 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;

    }

The cosine of zero is 1. The sign of zero is 0. So passing zero as the fourth parameter to mecanumDrive_Cartesian() will result in no rotation of the vector, which is what you said you want.

WillNess

26-04-2015 13:40

Re: Getting Gyro Working w/ Mecanum Drive

Do you think this will work? (I only included the drive code)

Code:

public void teleopInit(){

        gyroMode = true;

}



public void teleopPeriodic() {

        Jx = OI.joystick0.getX(); //Drive Joystick X

        Jy = OI.joystick0.getY(); //Drive Joystick Y

        Jz = OI.joystick0.getZ(); //Drive Joystick Z

        rotationSpeedForError = RobotMap.GyroMod(Jz);

        RobotMap.driveRobotDrive41.mecanumDrive_Cartesian(Jx, Jy, Jz + rotationSpeedForError, 0);

}



public double GyroMod(double rotation){

        if (gyroMode == true) {

                double error = gyro.getAngle() - gyroHeading;

                double kP = .05;

                if (rotation == 0) {

                        rotation = rotation + kP * error;

                } else {

                        gyroHeading = gyro.getAngle();

                }

        }

        return rotation;

        }

}

WillNess

26-04-2015 23:37

Re: Getting Gyro Working w/ Mecanum Drive

I believe I have gotten gyro code to work! It is a little tricky while strafing, but for every other use it works great, and it really does help limit the turning while strafing according to our driver. So here you go, FYI: I only included code related to the driving, don't replace whole sections with this code:

Code:



public void teleopInit() {

        RobotMap.gyro.reset();

        gyroMode = true;

}



public void teleopPeriodic() {

        Jx = OI.joystick0.getX(); //Drive Joystick X

        Jy = OI.joystick0.getY(); //Drive Joystick Y

        Jz = OI.joystick0.getZ(); //Drive Joystick Z

        //gyroHeading = 0;

        if(OI.joystick0.getTrigger()){

                rotationSpeedForError = RobotMap.GyroMod(0);

                RobotMap.setVelocity(Jx, 0, 0); //This is strafe mode

        }else{

                rotationSpeedForError = RobotMap.GyroMod(Jz);

            RobotMap.setVelocity(Jx, Jy, Jz);

        }

}



public static void setVelocity(double x,double y,double z){

        double fMax = Math.sin(-0*Math.PI/4+1*Math.PI/4);

        double fR = Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;

        double rL = -Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;

        double fL = -Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;

        double rR = Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;

          if(Math.abs(z)<.1){

                  drivefrontLeft.set(fL- rotationSpeedForError);

                drivefrontRight.set(fR - rotationSpeedForError);

                   driverearLeft.set(rL - rotationSpeedForError);

                    driverearRight.set(rR - rotationSpeedForError);

                // The reason that all of them are subtracting the error is that on our robot, the motors on the right side are flipped upside down, so to make them go the opposite way of the left, you have to flip the signs. These signs will change from bot to bot, so be prepared to change these.

            }else{

                    drivefrontLeft.set(fL+z/2);

                    drivefrontRight.set(fR+z/2);

                    driverearLeft.set(rL+z/2);

                    driverearRight.set(rR+z/2);

                // The reason that all of them are adding the Z is that on our robot, the motors on the right side are flipped upside down, so to make them go the opposite way of the left, you have to flip the signs. These signs will change from bot to bot, so be prepared to change these.

                    gyroHeading = gyro.getAngle();

            }

}

public static double GyroMod(double rotation){

        if (gyroMode) {

                double error = gyro.getAngle() - gyroHeading;

                double kP = .03;

                if (Math.abs(rotation) < .1) { 

                        rotation = rotation + kP * error;

                }else {

                        

                }

        }

        return rotation;

}

This code is also very useful because in autonomous instead of turning your robot for X amount of seconds, you just add X amount of degrees to the gyroHeading value and it will be very accurate, or in teleop you can have various buttons to fine-adjust the angle or flip the robot around, or etc.. There's many applications to this.
One thing I might try to do in the future is make the correction only read [-180,180] because when we set our heading to 0, we could rotate around 3-4 times and instead of going to the nearest "0" it would rotate back the exact amount. The reason this could help is that if the game is in a year with a goal, you could press a button and the robot would rotate to face the goal exactly, but this wouldn't work if maybe you had spun around several times before.

Thanks to: GeeTwo, Ether, and dawonn

All times are GMT -5. The time now is 11:13.