Getting Gyro Working w/ Mecanum Drive

[WillNess]

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:

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]

Originally Posted by WillNess

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:

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

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]

Originally Posted by Ether

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:

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

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]

Originally Posted by WillNess

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?

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

Originally Posted by Ether

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

* @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:

Originally Posted by Ether

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]

Originally Posted by Ether

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]

Originally Posted by WillNess

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

No.

Check out some of these links.

[WillNess]

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]

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

[WillNess]

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]

Originally Posted by WillNess

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:

Originally Posted by Ether

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

[WillNess]

Originally Posted by Ether

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]

Originally Posted by WillNess

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

Why? What part is not clear?

[WillNess]

Originally Posted by Ether

Why? What part is not clear?

The actual code I would write.

[GeeTwo]

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]

How do I set the robotdrive to the MecanumDrive method?

Originally Posted by GeeTwo

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.