I can enable, drive, then use the pid to setpoint my drivetrain, drive somemore but if i reenable the pid it breaks the drivetrain.
/*
* To change this license header, choose License Headers in Project Properties.
* To change this template file, choose Tools | Templates
* and open the template in the editor.
*/
package us.mn.k12.fairmont.robotics.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import us.mn.k12.fairmont.robotics.RobotMap;
import us.mn.k12.fairmont.robotics.commands.CommandBase;
import us.mn.k12.fairmont.robotics.commands.DriveWithJoysticks;
/**
*
* @author Teacher
*/
public class DriveTrain extends PIDSubsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
RobotDrive drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
Encoder enc = new Encoder(RobotMap.frontRightEncA, RobotMap.frontRightEncB);
// Timer timer = new Timer();
public DriveTrain() {
super("Drivetrain", 6, 0, 0);
setAbsoluteTolerance(10);
enc.start();
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
setDefaultCommand(new DriveWithJoysticks());
}
public void driveMechanum_Cartesian(double rightJoyY, double rightJoyX, double rotation) {
drive.mecanumDrive_Cartesian((rightJoyY * -1), rotation, rightJoyX * -1, 0); // Mechanam wheel rollers should form an x when looking directaly down from the top
}
public void putPID() {
CommandBase.table.putNumber("p", 8.0); // change it to smartdashboard getnumber tomorrow when you arent bored of deploying
CommandBase.table.putNumber("i", 0.0); // HOLY shoot YOU FIXED THE MOMORY LIEEK
CommandBase.table.putNumber("d", 0.0);
}
public double updateStatus() {
SmartDashboard.putNumber("Enc", enc.getRaw());
return enc.getRaw();
}
public void reset() {
enc.reset();
reset();
}
public double returnPIDInput() {
return enc.getRaw();
}
public void setSetpoint(int setpoint) {
setSetpoint(setpoint);
}
public void getSetpoint(int setpoint) {
getSetpoint();
}
public void getPosition(int position) {
getPosition();
}
public void updatePID() {
RobotMap.p = CommandBase.table.getNumber("p", 8);
RobotMap.i = CommandBase.table.getNumber("i", 0);
RobotMap.d = CommandBase.table.getNumber("d", 0);
getPIDController().setPID(RobotMap.p, RobotMap.i, RobotMap.d);
}
public void usePIDOutput(double output) {
driveMechanum_Cartesian(output, output, 0);
}
}
package us.mn.k12.fairmont.robotics.commands;
public class DriveToDistance extends CommandBase {
boolean go;
double setpoint;
/**
* Require the drive train and store the desired setpoint.
*
* @param setpoint The desired setpoint for the drive train.
*/
public DriveToDistance(boolean go, double setpoint) {
requires(drivetrain);
this.go = go;
this.setpoint = setpoint;
}
// Called just before this Command runs the first time
/**
* Set the setpoint to the stored value and enable PID on the drivetrain.
*/
protected void initialize() {
System.out.println(this);
if (go) {
drivetrain.enable();
drivetrain.setSetpoint(setpoint);
}
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
/**
* @return true when it's close enough to the setpoint
*/
protected boolean isFinished() {
return drivetrain.onTarget();
}
// Called once after isFinished returns true
/**
* When this command ends, disable the drivetrain's PID
*/
protected void end() {
System.out.println("done");
go = false;
drivetrain.disable();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
/**
* When this command exits, disable the drivetrain's PID
*/
protected void interrupted() {
end();
}
}
/*
* To change this license header, choose License Headers in Project Properties.
* To change this template file, choose Tools | Templates
* and open the template in the editor.
*/
package us.mn.k12.fairmont.robotics.commands;
/**
*
* @author Teacher
*/
public class DriveWithJoysticks extends CommandBase {
public DriveWithJoysticks() {
// Use requires() here to declare subsystem dependencies
requires(drivetrain);
}
// Called just before this Command runs the first time
protected void initialize() {
System.out.println(this);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
drivetrain.driveMechanum_Cartesian(oi.getRightJoyY(), oi.getRightJoyX(), oi.getRightJoyR());
// drivetrain.drivetest(oi.getRightJoyY(), oi.getLeftJoyY());
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return false;
}
// Called once after isFinished returns true
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
}
Does anybody see what is wrong with the code?
