Create a gist now

Instantly share code, notes, and snippets.

Gyro Twist Correction
package org.usfirst.frc.team3238.robot;
/**
* Reduces unwanted rotation of mecanum chassis by using a gyro sensor and a PI
* loop
*
* @author Nick Papadakis
*/
public class GyroDrive
{
static double error;
static double cummulativeError = 0;
static double adjustedRotationValue;
static double oldX = 0;
static double oldY = 0;
static double time;
static double oldTime = 0;
static double timeDifference;
/**
* Takes the current movement commands for the chassis, PI constants, and
* current gyroValue
*
* @param x
* The current x power for the robot's movement
* @param y
* The current y power for the robot's movement
* @param rotation
* The current rotation power for the robot's movement
* @param pConstant
* The P constant for the PI loop
* @param iConstant
* The I constant for the PI loop
* @param spinThreshold
* The gyro value at which the PI loop will stop trying to
* correct the robot's spin
* @param gyroValue
* The current value of the gyro sensor
* @return The adjusted rotation value for correcting the robot's rotation
*/
static double getAdjustedRotationValue(double x, double y, double rotation,
double pConstant, double Iconstant, double spinThreshold,
double gyroValue)
{
error = Math.abs(gyroValue);
time = System.currentTimeMillis(); // Reads the system's time
// If this is the first loop, set timeDifference to 0
if(oldTime == 0)
{
timeDifference = 0;
}
else
{
timeDifference = time - oldTime;
}
// If the joystick input has changed, reset cummulativeError
if(Math.abs(oldX - x) > 0.01 || Math.abs(oldY - y) > 0.01)
{
cummulativeError = 0.0;
}
else
{
cummulativeError += error;
}
/*
* Check to see if the robot's spinning really fast due to a driver
* induced spin or if the driver is trying to rotate, and if so, don't
* change the current rotation value
*/
if((error > spinThreshold) || rotation <= -0.00001 || rotation >= 0.000001)
{
adjustedRotationValue = rotation;
}
/*
* Check the direction of the gyroValue to know which way the chassis
* needs to rotate in to compensate
*/
else if(gyroValue > 0)
{
adjustedRotationValue = error * pConstant + cummulativeError
* Iconstant * timeDifference;
}
else if(gyroValue < 0)
{
adjustedRotationValue = -(error * pConstant + cummulativeError
* Iconstant * timeDifference);
}
else
{
adjustedRotationValue = 0;
}
// Set up the "old" values for the next loop
oldX = x;
oldY = y;
oldTime = time;
return adjustedRotationValue;
}
/**
* Resets the cummulativeError and oldTime variables
*/
static void reinit()
{
cummulativeError = 0;
oldTime = 0;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment