We can’t figure out what we’re doing wrong here. The goal here is to take the x value from the Limelight-generated network table, convert it to an angle, and create a PID turn-to-angle using the imu. We’re getting several syntax errors that we can’t seem to resolve.
-
Because we don’t start with any previous error for our D term, we want the first time the loop runs to calculate the D_gain to be zero, so we want “last_error” to start at the same angle we’re at. So we put it in “initialize” and used it in “execute”…but this makes the double in initialize unused, and the same double used in execute and IsFinished a syntax error.
-
We don’t want to get a new targetAngle from the Limelight every time the loop runs, we want to get it once at the beginning of the command, convert it from pixels to degrees, and then use it in math. Same attempt as above, same errors created. (Hey, at least it’s consistent…)
-
Every time the loop runs, we want I_gain to update itself as I_gain = I_gain + error, but it doesn’t seem to like this at all, and if I comment out the other syntax errors, this one appears.
package frc.robot.commands;
import java.util.function.DoubleSupplier;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants;
import frc.robot.subsystems.DriveTrain;
public class Turn_Test extends CommandBase {
private final DriveTrain m_drivetrain;
private static final double turnkP = 0.005; // proportionality constant
private static final double turnkI = 0;
private static final double turnkD = 0;
private static final double turnkF = 0;
private static final double DeadBand = 1; //deadband range
private static final double OffSet = 0; //Aim Offset for Limelight
private static final double BaseSpeed = 0; // speed before PID modification
public Turn_Test(DoubleSupplier leftspeed, DoubleSupplier rightspeed, DriveTrain driveTrain) {
m_drivetrain = driveTrain;
addRequirements(m_drivetrain);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
last_error = DriveTrain.imu.getAngle();
float tx = (float) NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
double targetAngle = tx*Constants.Pixels_To_Degrees+OffSet;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double error = DriveTrain.imu.getAngle() - targetAngle;
double P_gain = error;
double I_gain = I_gain + error;
double D_gain = error-last_error;
double rightspeed = BaseSpeed + P_gain*turnkP + I_gain*turnkI + D_gain*turnkD;
double leftspeed = -rightspeed;
m_drivetrain.drive(leftspeed, rightspeed);
error = last_error;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return Math.abs(error) <= targetAngle - DeadBand;
}
}
For the record, we have no experience using the integral PID in the talons to set up a PID loop based on the IMU sensor–that would probably be ideal, but we have no idea what that code would look like, and the Phoenix Documentation example code looks like a bunch of code spaghetti to us for this purpose. We have never used motion profiling or motion magic, and don’t know where to even get started; at this point we’ll be happy with a home-brew PID that turns us to the angle we want to turn to.
We have other turn-to-target code that works reasonably well, but not quite well enough, that is modeled after the sample code on the Limelight website. It looks like this:
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants;
import frc.robot.subsystems.DriveTrain;
import frc.robot.subsystems.Limelight;
public class RotateToTarget extends CommandBase {
private final DriveTrain driveTrain;
private final Limelight limelight;
private static final double kP_limelight = 0.002; // Patrick Added
public RotateToTarget(DriveTrain driveTrain, Limelight limelight) {
this.driveTrain = driveTrain;
this.limelight = limelight;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
public double rotateSpeed = Constants.SEEK_SPEED;
public double deadband = Constants.DEADBAND_RANGE;
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double target = limelight.aimbot();
//rotate left if aimbot is negative
/* PATRICK COMMENTED THIS OUT TO TRY A kP + MinValue Adjust Instead
if(target < deadband * -1)
driveTrain.drive(rotateSpeed, -rotateSpeed);
else if (target > deadband)
driveTrain.drive(-rotateSpeed, rotateSpeed);
} */
//Patrick Added Stuff Starts Here
double leftspeed = 0.0;
double rightspeed = 0.0;
double MinVal_limelight = Constants.MinVal_limelight;
if (target < -deadband) {
leftspeed = (rotateSpeed - target)*kP_limelight;
rightspeed = (target - rotateSpeed)*kP_limelight;
if (leftspeed < MinVal_limelight) {
leftspeed = MinVal_limelight;
}
if (rightspeed > -MinVal_limelight) {
rightspeed = -MinVal_limelight;
}
driveTrain.drive(leftspeed, rightspeed);
}
else if (target > deadband) {
leftspeed = (target - rotateSpeed)*kP_limelight;
rightspeed = (rotateSpeed - target)*kP_limelight;
if (leftspeed > -MinVal_limelight) {
leftspeed = -MinVal_limelight;
}
if (rightspeed < MinVal_limelight) {
rightspeed = MinVal_limelight;
}
driveTrain.drive(leftspeed, rightspeed);
}
//Patrick Added Stuff Ends Here
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
driveTrain.drive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
double target = limelight.aimbot();
//if the target is negative, convert to a positive number.
if(target < 0)
target = target * -1;
//return true if target is less than the deadband
return target < deadband;
}
}
It’s just not quite good enough.
Any help you could give would be most appreciated–we’re 98% there to a really killer robot, and this turn consistency issue on our aiming is one of the last things we need to hunt down.
