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.