CurveStraightDrive
Take a look at this code below. Then, do your best to answer these questions:
- What is this command supposed to do?
- What are some issues with this code? Be specific and clear!
- If we don’t change anything and just run this code, what do you think will happen? Does this match our goal?
- What should we change in order to accomplish the goal? You don’t need to implement it or make the changes. Just mention what you’d do.
package frc.team670.robot.commands.drive;
import com.pi4j.io.gpio.RaspiPin;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.team670.pi.sensors.Encoder;
import frc.team670.robot.Robot;
import frc.team670.robot.subsystems.DriveBase;
import frc.team670.robot.utils.Logger;
public class CurveStraightDrive extends Command {
private double speedL, speedR, dist, time;
private Encoder lEncoder, rEncoder;
private DriveBase drivebase = Robot.driveBase;
public CurveStraightDrive(double dist, double lspeed, double rspeed, double secs) {
this.speedL = lspeed;
this.speedR = rspeed;
this.time = dist;
this.dist = secs;
lEncoder = new Encoder(RaspiPin.GPIO_07, RaspiPin.GPIO_01);
rEncoder = new Encoder(RaspiPin.GPIO_22, RaspiPin.GPIO_21);
requires(drivebase);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
setTimeout(dist);
Logger.consoleLog();
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
drivebase.tankDrive(speedL, speedR);
}
// Called once after isFinished returns true
@Override
protected void end() {
stop();
Logger.consoleLog("LeftSpeed: %s Right Speed: %s DistanceT: %s", speedL, speedR, getDistance());
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
end();
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
if(isTimedOut()) {
end();
return isTimedOut();
}
else {
return false;
}
// return (this.error <= 1);
}
public double getDistance() {
double distance = Robot.driveBase.getLeftEncoder().getDistance();
return Math.abs(distance);
}
public void tankDrive() {
while (RobotState.isEnabled()) {
Robot.driveBase.tankDrive(speedL, speedL);
}
}
public void stop() {
speedL = 0;
}
}