Spinner Speed Control
Now that we have the drive motors controllable by speed, we need to do the same for the spinner motors. In order to make consistent shots, we need to be able to spin the wheels at a consistent speed.
Let’s start by adding the encoders you will need to your SpinnerSubsystem class. Note, however, that the encoders attached to the spinner motors are controlled by the IREncoder class. I would like you to take a crack at modifying your SpinnerSubsystem class to include these encoders. Note that the left encoder is attached to port 1 and the right encoder is attached to port 0. When you are done, you can compare your result to the solution below.
.
.
.
.
.
Once you have added the encoders, your SpinnerSubsystem.java file should look something like:
package subsystem;
import robotCore.I2CPWMMotor;
import robotCore.IREncoder;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class SpinnerSubsystem extends Subsystem
{
private static final double k_intakeSpeed = -0.8;
private static final int k_rightSpinnerPin1 = 6;
private static final int k_rightSpinnerPin2 = 7;
private static final int k_leftSpinnerPin1 = 10;
private static final int k_leftSpinnerPin2 = 11;
private static final int k_leftEncoderPort = 1;
private static final int k_rightEncoderPort = 0;
private I2CPWMMotor m_rightSpinner = null;
private I2CPWMMotor m_leftSpinner = null;
private IREncoder m_leftEncoder = null;
private IREncoder m_rightEncoder = null;
public void initDefaultCommand()
{
Logger.Log("SpinnerSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
m_rightSpinner = new I2CPWMMotor(k_rightSpinnerPin1, k_rightSpinnerPin2);
m_leftSpinner = new I2CPWMMotor(k_leftSpinnerPin1, k_leftSpinnerPin2);
m_leftEncoder = new IREncoder(k_leftEncoderPort);
m_rightEncoder = new IREncoder(k_rightEncoderPort);
Logger.Log("SpinnerSubsystem", 2, "Init()");
}
public void SpinUp()
{
m_rightSpinner.set(1.0);
m_leftSpinner.set(1.0);
}
public void Stop()
{
m_rightSpinner.set(0);
m_leftSpinner.set(0);
}
public void Intake()
{
m_rightSpinner.set(k_intakeSpeed);
m_leftSpinner.set(k_intakeSpeed);
}
public IREncoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public IREncoder GetRightEncoder()
{
return(m_rightEncoder);
}
}
Now create a new test command (let’s call it SpinnerSpeedTestCommand). This time, we really only care about controlling the spinners when they are spinning at about maximum power. So set up your test so that it runs the motors at full power and logs the speed vs time. I would like you to take a crack at implementing this new class. When you are finished, you can compare your solution to the one below.
.
.
.
.
.
You SpinnerSpeedTestCommand.java file should look something like:
package commands;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class SpinnerSpeedTestCommand extends Command
{
public SpinnerSpeedTestCommand()
{
Logger.Log("SpinnerSpeedTestCommand", 3, "SpinnerSpeedTestCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_spinnerSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("SpinnerSpeedTestCommand", 2, "initialize()");
Robot.m_spinnerSubsystem.SpinUp();
Logger.SetLogFile("SpinnerSpeedTest", "SpinnerSpeedTest");
Logger.Log("SpinnerSpeedTest", 3, "Time,Left,Right");
Logger.ResetElapsedTime();
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("SpinnerSpeedTestCommand", -1, "execute()");
Logger.Log("SpinnerSpeedTest", 3, String.format("%d,%d,%d",
Logger.GetElapsedTime(), Robot.m_spinnerSubsystem.GetLeftEncoder().getRate(),
Robot.m_spinnerSubsystem.GetRightEncoder().getRate()));
Robot.Sleep(35);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("SpinnerSpeedTestCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("SpinnerSpeedTestCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("SpinnerSpeedTestCommand", 2, "interrupted()");
Logger.CloseLogFile("SpinnerSpeedTest");
Robot.m_spinnerSubsystem.Stop();
}
}
Finally we will want change the OI class so that button 12 now runs our new command SpinnerSpeedTestCommand. After you have made the change, compare you result to the solution below:
.
.
.
.
.
Your OI.java file should now look like:
package robot;
import commands.DriveSpeedTestCommand;
import commands.IntakeCommand;
import commands.ShootCommand;
import commands.SpinnerSpeedTestCommand;
import commands.SpinupCommand;
import robotCore.Joystick;
import wpilib.Button;
import wpilib.JoystickButton;
/**
* This class is the glue that binds the controls on the physical operator
* interface to the commands and command groups that allow control of the robot.
*/
public class OI {
//// CREATING BUTTONS
// One type of button is a joystick button which is any button on a joystick.
// You create one by telling it which joystick it's on and which button
// number it is.
Joystick m_stick = new Joystick(0);
Button m_trigger = new JoystickButton(m_stick, 1);
Button m_spinup = new JoystickButton(m_stick, 2);
Button m_intake = new JoystickButton(m_stick, 8);
Button m_test = new JoystickButton(m_stick, 12);
// There are a few additional built in buttons you can use. Additionally,
// by subclassing Button you can create custom triggers and bind those to
// commands the same as any other Button.
//// TRIGGERING COMMANDS WITH BUTTONS
// Once you have a button, it's trivial to bind it to a button in one of
// three ways:
// Start the command when the button is pressed and let it run the command
// until it is finished as determined by it's isFinished method.
public OI()
{
// m_trigger.whenPressed(new ExampleCommand());
m_spinup.whileHeld(new SpinupCommand());
m_trigger.whileHeld(new ShootCommand());
m_intake.whileHeld(new IntakeCommand());
m_test.whileHeld(new SpinnerSpeedTestCommand());
}
}
Now run your program, get the logged data, and graph it. Your result should look something like. Note that in this case, since we know that we get one count for each rotation, the units for the speed are simply rotations/sec.
As we can see from the graph, the maximum speed of the spinner motors is around 90 rotations/sec (or about 5400 rpm). In this case, however, we are not going to want to try and run the motors at full power. It is more important that we can hit a consistent target speed than running at max speed. If we try and shoot for the max, we will not be able to control the speed as well since the PIDController can never increase the power beyond full power. So to be a little conservative, lets set our max speed to 75 rotations/sec.
Now you need to add the MotorSpeedController class to your SpinnerSubsystem, and set it up to control the motor by speed instead of power. When you set up the P, I, D, and F parameters for the MotorSpeedController constructor, you will want to start with only an F term. Since the max speed possible is about 90, a good first guess would be 1/90 or about 0.01. Also, let’s not set a Scale Factor, instead choosing to set the speed in rotations/sec, rather than from 0 to 1.0. After you have made your changes, you can compare your result to the one below.
.
.
.
.
.
Your SpinnerSubsystem.java file should now look like:
package subsystem;
import robotCore.I2CPWMMotor;
import robotCore.IREncoder;
import robotCore.Logger;
import robotCore.MotorSpeedController;
import wpilib.Subsystem;
/**
*
*/
public class SpinnerSubsystem extends Subsystem
{
private static final double k_intakeSpeed = -0.8;
private static final int k_rightSpinnerPin1 = 6;
private static final int k_rightSpinnerPin2 = 7;
private static final int k_leftSpinnerPin1 = 10;
private static final int k_leftSpinnerPin2 = 11;
private static final int k_leftEncoderPort = 1;
private static final int k_rightEncoderPort = 0;
private I2CPWMMotor m_rightSpinner = null;
private I2CPWMMotor m_leftSpinner = null;
private IREncoder m_leftEncoder = null;
private IREncoder m_rightEncoder = null;
private MotorSpeedController m_leftController = null;
private MotorSpeedController m_rightController = null;
public void initDefaultCommand()
{
Logger.Log("SpinnerSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
m_rightSpinner = new I2CPWMMotor(k_rightSpinnerPin1, k_rightSpinnerPin2);
m_leftSpinner = new I2CPWMMotor(k_leftSpinnerPin1, k_leftSpinnerPin2);
m_leftEncoder = new IREncoder(k_leftEncoderPort);
m_rightEncoder = new IREncoder(k_rightEncoderPort);
m_leftController = new MotorSpeedController(m_leftSpinner, m_leftEncoder, 0.0, 0.0, 0, 0.011);
m_rightController = new MotorSpeedController(m_rightSpinner, m_rightEncoder, 0.0, 0.0, 0, 0.011);
Logger.Log("SpinnerSubsystem", 2, "Init()");
}
public void SpinUp()
{
m_rightController.set(75);
m_leftController.set(75);
}
public void Stop()
{
m_rightController.set(0);
m_leftController.set(0);
}
public void Intake()
{
m_rightSpinner.set(k_intakeSpeed);
m_leftSpinner.set(k_intakeSpeed);
}
public IREncoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public IREncoder GetRightEncoder()
{
return(m_rightEncoder);
}
}
If you run the program and graph the logged data you should see something like:
Here we can see that the right motor is slightly weaker than the left, but both are close enough to our target of 75 rps that we can now set the rest of the terms. You should try and see if you can come up with values for the P and I terms, as well as the SetIRange value that works best. Once you have your best numbers, scroll down and see what our best set was.
.
.
.
.
.
We chose a value of 0.02 for P, 0.0025 for I, and 10 for the SetIRange. The following was our result:
How does yours compare?