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.SpeedReference.kQuadEncoder); leftFront.configEncoderCodesPerRev(360);//sets encoder count leftFront.setPID(.350, .004, .000);//PID GAINS leftFront.enableControl(); leftRear = new CANJaguar(11); leftRear.configNeutralMode(CANJaguar.NeutralMode.kCoast); leftRear.changeControlMode(CANJaguar.ControlMode.kSpeed); leftRear.setSpeedReference(CANJaguar.SpeedReference.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.SpeedReference.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.SpeedReference.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.CANNotInitializedException 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.