Tank Drive Can Jaguar PID Speed Control

[coreyjon]

Let me preface this with: I am not normally a programmer. However- I am trying to learn. I've been building a demo bot to use as a teaching aid to not only help teach students on our team, but also a team that I am helping to start. I would say for someone who is only 10 hours in a 24 hour book in Java and having never used netbeans before this I have gotten pretty far on reading the forums, FIRST documentation, and experimenting and having patience.

I'm posting my code so that I may ask a few questions, and also perhaps it may help others in the future.

The demo bot has 4 CIM motors, 2 per side, each with it's own independent gearbox and Jaguar. The Jags are connected via CAN bus to the serial port on the cRIO. There are 4 quadrature encoders, one each on the output shafts of each gearbox. they are wired directly to the Jags.

For control, I'm using a simple logitech gamepad. I have mapped the left joystick for the left side, and right joystick for the right side (basic tank drive).

Using PID capability built in to the Jags, I've set the code up so that it takes the gamepad joystick inputs and makes them a target speed for each side (in order to do this I've had to amplify the joystick input (the number was figured out by trial and error). The jags set the output speed to match the target speed.

This works, to my shock. First time attempting this and while it's certainly simple compared to most robot code I've seen, its easy to explain and use to get started, It's far from perfect though.

One thing I've noticed: when there is no joystick input (well non that I am inputing) the output shafts will kind of have a mind of their own - 1-5 rpms here and there - little twitch here. I'm sure there is a way to simply add in a dead spot - just haven't figured out an easy way to do it yet.

Question: Whats the best way to implement a dead spot - can you do this when dealing with a speed (both positive and negative speed)?

Other than that I'm really happy with how this code behaves - happy robot besides the twitching - I would consider that pretty minor though.

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.CANJaguar.SpeedReference;
import edu.wpi.first.wpilibj.CANJaguar.ControlMode;
import edu.wpi.first.wpilibj.CANJaguar.PositionReference;
import edu.wpi.first.wpilibj.CANJaguar.NeutralMode;

/**
* 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 RobotTemplate extends IterativeRobot {

/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
//Name Jags
Joystick gp;// Name Joystick 'gp'
Relay light; //sets up a relay named 'light'
CANJaguar leftFront;
CANJaguar leftRear;
CANJaguar rightFront;
CANJaguar rightRear;



public void robotInit() {
try {
gp = new Joystick(1); //defines gp as Joystick port 1
light = new Relay(1); //defines new relay port
//

//Sets up CAN Jags PID Below
leftFront = new CANJaguar(10);//JAG CAN ID 10
leftFront.configNeutralMode(CANJaguar.NeutralMode. kCoast);
leftFront.changeControlMode(CANJaguar.ControlMode. kSpeed);
leftFront.setSpeedReference(CANJaguar.SpeedReferen ce.kQuadEncoder);
leftFront.configEncoderCodesPerRev(360);//sets encoder count
leftFront.setPID(.350, .004, .000);//PID GAINS
leftFront.enableControl();

leftRear = new CANJaguar(11);
leftRear.configNeutralMode(CANJaguar.NeutralMode.k Coast);
leftRear.changeControlMode(CANJaguar.ControlMode.k Speed);
leftRear.setSpeedReference(CANJaguar.SpeedReferenc e.kQuadEncoder);
leftRear.configEncoderCodesPerRev(360);
leftRear.setPID(.350, .004, .000);
leftRear.enableControl();

rightFront = new CANJaguar(12);
rightFront.configNeutralMode(CANJaguar.NeutralMode .kCoast);
rightFront.changeControlMode(CANJaguar.ControlMode .kSpeed);
rightFront.setSpeedReference(CANJaguar.SpeedRefere nce.kQuadEncoder);
rightFront.configEncoderCodesPerRev(360);
rightFront.setPID(.350, .004, .000);
rightFront.enableControl();

rightRear = new CANJaguar(13);
rightRear.configNeutralMode(CANJaguar.NeutralMode. kCoast);
rightRear.changeControlMode(CANJaguar.ControlMode. kSpeed);
rightRear.setSpeedReference(CANJaguar.SpeedReferen ce.kQuadEncoder);
rightRear.configEncoderCodesPerRev(360);
rightRear.setPID(.350, .004, .000);
rightRear.enableControl();

} catch (Exception e) {
System.out.println("Exception Thrown");
e.printStackTrace();
}
}

/**
* This function is called periodically during autonomous
*/
public void autonomousPeriodic() {
light.set(Relay.Value.kReverse);

}

/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() {
//Relay code section
if (gp.getRawButton(1))
{
light.set(Relay.Value.kForward);
}
else if (gp.getRawButton(2))
{
light.set(Relay.Value.kReverse);
}
else
{
light.set(Relay.Value.kOff);
}
//end relay control code
try{

//tank drive, gamepad - CAN Jags withs PID loop

try{//joystick sets speed per side, and PID encoder loop in jags adjust output to match
leftFront.setX(gp.getRawAxis(2)* -1 *400);//left stick in, inverts, then amplifies
leftRear.setX(gp.getRawAxis(2)* -1 *400);//left stick in, inverts,then amplifies
rightFront.setX(gp.getRawAxis(5) *400);//right stick in, amplifies
rightRear.setX(gp.getRawAxis(5) *400);//right stick in, amplifies
System.out.println("LF=" + leftFront.getSpeed() + " -LR=" + leftRear.getSpeed() + " -RF=" + rightFront.getSpeed() + " -RR=" + rightRear.getSpeed());
} catch(Exception e){
e.printStackTrace();
}

} catch(edu.wpi.first.wpilibj.can.CANNotInitializedE xception cnie) {
cnie.printStackTrace();

}
}
}

Like I said, I'm happy how this works. To insure it is trying to keep the target speed I had it print out output current instead of speed, and then put a load on each shaft and watched the current increase to keep the rpms the same.

[Ether]

Originally Posted by coreyjon

...

First let me say: Wonderfully detailed and articulate post. Kudos.

For the dead spot (aka "deadband" or "neutral zone"), try the following test: Add something like this to your code:

if ((joystick<threshold) && (joystick>-threshold)) joystick=0;

... if that doesn't work, I think you may be at the mercy of the limited control you have over the Jag's built-in PID.

If you ran the encoders to the cRIO, and did the speed control there instead, you could easily bypass the PID and send a zero voltage or %Vbus command to the Jag whenever the joystick was in the neutral zone.

[coreyjon]

Thank you for the kind words. Using your idea here is what I came up with for setting the dead band.

if ((gp.getRawAxis(2) < .001) && (gp.getRawAxis(2) > -.001)) leftFront.disableControl(); else leftFront.enableControl();

I have 4 of those statements, one for each drive motor. works like a dream! Hopefully this helps others out as well! I have placed them in the same try catch block as the "setX" inputs.