Getting Gyro Working w/ Mecanum Drive

[GeeTwo]

Originally Posted by WillNess

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]

Originally Posted by GeeTwo

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.

So in my teleopPeriodic would I put:

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]

Originally Posted by WillNess

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.

Originally Posted by Ether

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

Originally Posted by GeeTwo

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:

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

[dawonn]

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]

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

Originally Posted by Ether

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]

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

Originally Posted by dawonn

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.

[Ether]

Originally Posted by dawonn

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]

Originally Posted by WillNess

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():

    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():

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

Thank you Ether, that helps tons.

Originally Posted by Ether

Look at the WPILib code for mecanumDrive_Cartesian():

    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():

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

Do you think this will work? (I only included the drive 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]

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:

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