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