Hey guys! I’m a senior from Salt Lake just getting into FIRST. I am new to java and I’m having some trouble specifically with my PID loop. As I understand it the PIDController function should take input from my encoder and send output to my motor (arm). The code builds with no errors but I am seeing no motion in the arm motor, I am using an encoder value of 250 as my setpoint just for testing purposes. If anyone can lend me some advice as to how I’m screwing this up that would be greatly appreciated! :deadhorse:
package org.usfirst.frc.team4598.robot;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.DigitalInput;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends IterativeRobot // w/ joysticks
{
CameraServer server;
Joystick controller = new Joystick(0);
Solenoid Punch = new Solenoid(1,3);
Victor leftDrive0 = new Victor(0);
Victor leftDrive1 = new Victor(1);
Victor rightDrive0 = new Victor(2);
Victor rightDrive1 = new Victor(3);
Victor launchWheel0 = new Victor(4);
Victor launchWheel1 = new Victor(5);
Victor arm = new Victor(6); //Shooter Arm
Victor pnuarm = new Victor(7);
Encoder encoder = new Encoder(1,2,true,EncodingType.k4X);
double Kp;
double Ki;
double Kd;
PIDController armPID = new PIDController(Kp,Ki,Kd,encoder,arm); //PID (P,I,D,INPUT,OUTPUT)
DigitalInput limitSwitchOut = new DigitalInput(3);
DigitalInput limitSwitchIn = new DigitalInput(4);
Compressor Compressy = new Compressor(1);
int autoLoopCounter;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.\
*/
public void robotInit()
{
Compressy.setClosedLoopControl(true);
CameraServer server = CameraServer.getInstance();
server.setQuality(50);
server.startAutomaticCapture("cam0");
encoder.startLiveWindowMode();
armPID.enable();
armPID.setInputRange(0, 550); armPID.setOutputRange(-1, 1);
armPID.setAbsoluteTolerance(5);
armPID.startLiveWindowMode();
}
/**
* This function is run once each time the robot enters autonomous mode
*/
public void autonomousInit()
{
}
/**
* This function is called periodically during autonomous
*/
public void autonomousPeriodic()
{
}
/**
* This function is called once each time the robot enters tele-operated mode
*/
public void teleopInit()
{
}
/**
* This function is called periodically during operator control
*/
public void teleopPeriodic()
{
armPID.setSetpoint(250);
SmartDashboard.putNumber("L Trigger Value", controller.getRawAxis(2));
SmartDashboard.putNumber("Encoder Value", encoder.getRaw());
SmartDashboard.putNumber("Kp", Kp);
SmartDashboard.putNumber("Ki", Ki);
SmartDashboard.putNumber("Kd", Kd);
SmartDashboard.putNumber("ARMPID", armPID.get());
SmartDashboard.putNumber("ARMPID6", armPID.getSetpoint());
SmartDashboard.putBoolean("ARMPID7", armPID.onTarget());
Kp = .1;
Ki = .1;
Kd = .1;
//arm.set(controller.getRawAxis(2));
if (controller.getRawButton(2))
{
pnuarm.set(.85);
}
else if (controller.getRawButton(3))
{
pnuarm.set(-.85);
}
else
{
pnuarm.set(0);
}
if (controller.getRawAxis(3) > .5)
{
launchWheel0.set(.5);
launchWheel1.set(-.5);
}
else if(controller.getRawButton(6))
{
launchWheel0.set(-1);
launchWheel1.set(1);
}
else
{
launchWheel0.set(0);
launchWheel1.set(0);
}
if (controller.getRawButton(1))
{
Punch.set(true);
}
else
{
Punch.set(false);
}
//Drive
leftDrive0.set(-controller.getRawAxis(1));
leftDrive1.set(-controller.getRawAxis(1));
rightDrive0.set(controller.getRawAxis(5));
rightDrive1.set(controller.getRawAxis(5));
arm.set(armPID.get());
//if (controller.getRawButton(4))
//{
//if (encoder.get() < 530)
//{
//arm.set(controller.getRawAxis(2)+.4);
//}
//else
//{
//arm.set(0.1);
//}
//}
}
/**
* This function is called periodically during test mode
*/
public void testPeriodic()
{
LiveWindow.run();
}
}
Here’s a video: