DriveTrain TankDrive Control

[Kevin Zhang]

Hello,
Our team has been experimenting with controlling our robot drivetrain better for driver assist. We wanted to make a basic command to drive the robot for a certain distance using encoders. However we have been having a lot of trouble.

We can get very accurate encoder data from the drivetrain that is accurate to about 2 cm but unless we drive the drivetrain at around 0.2 percentvbus speed(basically unmoving) if we cut the motors off when the robot hits the desired encoder value, the momentum carries the robot forward another couple of feet

As a result I tried to control the robot using a PID control algorithm. I have a very good understanding of PID and as a project to help with understanding created a PID controlled beam balance to balance a ball. However, the PID Control objects for wpilib are very confusing. We are using CanTalon SRX to control a tank drive with 2 talons per side controlling a 2 cim vex gearbox with encoders connected to each canTalon

We've tried different approaches that all have issues that have stranded us because of the lack of documentation.

1) Instantiate a PIDController object with each of the talons assigned to each and control through that way.
Problems
-Only the front two talons have encoders attached because the gearbox only has one encoder each
-PIDController have a setSetpoint() method but I have no idea what how to select what the setpoint is( can i just change it to speed or position control and it'll do it for me)
-I realized that the Talons can use different controlModes which I assumed would resolve this problem but PID controllers don't work with Talons.

2) Just use the inherent PID objects inside the Talons
Problems
-When a setpoint is set through position, the drivetrain moves in the opposite direction that it should(If we set the talon to setpoint 256 ticks the drivetrain starts at 0 and goes backward so the encoder reads negative and never reads 256.
-When the setpoint is flipped(-256), the drivetrain also flips and starts reading positive.

3) Implementing our own PID
-We can get error and find the setpoint to set the motors but the motors are set in a -1 to 1 fashion so we don't know how to convert m/s or ft/s setpoint measurements to the motors.

Could anyone explain to me how exactly the PID control for the drivetrain works and how exactly we can accurately move a certain distance. This doesn't have to be encoders connected to Talons. Encoders can be separate objects. Code isn't necessary, I just need a bridge between the theory and the actual implementation from how to get the error to how exactly we can set the setpoint on the motors accurately so that it drives the right distance.

Thanks

[geniusadam69]

Hi,

We are currently trying to use PID control with navigation to turn to a certain angle, and find the same issue. The sampling period for the update is too slow for effective PID control.

With that said, I think you'll find 1 and 2 better than 3.

When using 1, 1 PID controller should be used for all 4 Talons. The setSetpoint should be the number of ticks on the encoder. It'll likely overshoot and have to count backwards. You'll probably have to write something that can do this.

For 2, look at the talon class and see if there's something that allows you to set the motors to reverse. Robotdrive class has this functionality, and I would guess talon has something similar.

Good luck!

[pblankenbaker]

The CANTalon speed controllers are very nice and offer a lot more features than most of the other speed controllers. So, there are at least two ways to approach this problem:

Ignore the enhanced features and wire the encoder to your roboRIO and use a standard PIDController.
Use the enhanced features of the CANTalons (onboard PID controller and wire your encoder directly to the "leader" of each side).

If you want to ignore the enhanced feature, and run your CANTalons in percent vbus mode:

• Construct a CANTalon as the "leader" for the left side of your drive train and change its control mode to percent vbus (or one of the other control modes that the CANTalon does not use it's own PIDController on).
• For each additional motor on the left side, change its control mode to follower and set it to follow the "leader" (mimic the leader's output).
• Connect your left side encoder cables directly to the roboRIO and create a standard Encoder object to manage them.
• Construct a PIDController that uses the "leader" CANTalon as the output and the Encoder as the input.
• When you want to move a specific distance, set the target and start your PIDController.
• When you want to get out of "postion mode" make sure you stop your PIDController (otherwise it will fight your driver).

If you want to use the enhanced features, you can run your CANTalons in position mode:

• Construct a CANTalon as the leader for the left side of your drive train and change its control mode to position and set the P, I and D values (there may be some additional set up here).
• For each additional motor on the left side, change its control mode to follower and set it to follow the "leader" (mimic the leader's output).
• Connect your left side encoder cables directly to the "leader" CANTalon (not the roboRIO).
• Set the position you want to move to on the "leader" and the CANTalons should do the rest.
• When you want to get out of "postion mode", you will need to change the control mode of the "leader" CANTalon (otherwise it will fight your driver).

Regardless of the approach you take, it will probably be useful to create a separate class to encapsulate your logic as the left side and right side will be almost identical.

[mikets]

Originally Posted by Kevin Zhang

Hello,
Our team has been experimenting with controlling our robot drivetrain better for driver assist. We wanted to make a basic command to drive the robot for a certain distance using encoders. However we have been having a lot of trouble.

We can get very accurate encoder data from the drivetrain that is accurate to about 2 cm but unless we drive the drivetrain at around 0.2 percentvbus speed(basically unmoving) if we cut the motors off when the robot hits the desired encoder value, the momentum carries the robot forward another couple of feet

As a result I tried to control the robot using a PID control algorithm. I have a very good understanding of PID and as a project to help with understanding created a PID controlled beam balance to balance a ball. However, the PID Control objects for wpilib are very confusing. We are using CanTalon SRX to control a tank drive with 2 talons per side controlling a 2 cim vex gearbox with encoders connected to each canTalon
We've tried different approaches that all have issues that have stranded us because of the lack of documentation.

1) Instantiate a PIDController object with each of the talons assigned to each and control through that way.
Problems
-Only the front two talons have encoders attached because the gearbox only has one encoder each
-PIDController have a setSetpoint() method but I have no idea what how to select what the setpoint is( can i just change it to speed or position control and it'll do it for me)
-I realized that the Talons can use different controlModes which I assumed would resolve this problem but PID controllers don't work with Talons.

2) Just use the inherent PID objects inside the Talons
Problems
-When a setpoint is set through position, the drivetrain moves in the opposite direction that it should(If we set the talon to setpoint 256 ticks the drivetrain starts at 0 and goes backward so the encoder reads negative and never reads 256.
-When the setpoint is flipped(-256), the drivetrain also flips and starts reading positive.

3) Implementing our own PID
-We can get error and find the setpoint to set the motors but the motors are set in a -1 to 1 fashion so we don't know how to convert m/s or ft/s setpoint measurements to the motors.

Could anyone explain to me how exactly the PID control for the drivetrain works and how exactly we can accurately move a certain distance. This doesn't have to be encoders connected to Talons. Encoders can be separate objects. Code isn't necessary, I just need a bridge between the theory and the actual implementation from how to get the error to how exactly we can set the setpoint on the motors accurately so that it drives the right distance.

Thanks

We are actually doing 3. The built-in PID control of CANTalon is nice but it is doing a per motor PID control which is fine if we are controlling a mechanism with only one motor (e.g. an arm or an elevator). In a drive base where you have at least 2, 4 or even 6 motors, individual motor PID control doesn't address the need for the drive base to run straight. For example, if you try to have the robot go forward 10 feet and say you have calculated 10 feet is equivalent to 10,000 encoder counts. If you command each motor to go 10000 encoder counts, what if the left motor is slightly faster than the right motor. The robot will curve to the right but the left motor will achieve the 10000 count earlier than the right motor, so it will stop while the right motor will try to catch up. This means the robot will now curve left. It will get there but the robot may be traveling in a slight 'S'. In a multi-motor PID control scenario, you need an overall PID control, not individual motor PID control. You need the ability to look at the overall drive base movement and be able to shift different power to different wheels in order to keep the robot on target whether it is the heading or the distance. So in our library, we implemented a PIDDrive class that takes two PID controllers. One PID controller is controlling the distance and the other PID controller is controlling the heading. My analogy to this is one is controlling the gas pedal and the other controlling the steering wheel. The PIDDrive class will combine all sensor data (e.g. 4 encoders and 1 gyro) to come up with "distance travelled" and "heading" data. Then it applies the distance data to the distance PID controller to calculate the base power for all four wheels. It also applies the heading data to the "Turn PID controller" to calculate the differential power to the left and the right side of the wheels. So it can add/subtract a delta power from the base power between the left and right side of the wheels (i.e. shift power from the left to the right and vice versa). This overall PID control is a lot better than individual motor PID control.
BTW, if you want to look at our library, you can find it here:
https://github.com/trc492/Frc2016Fir...cPidDrive.java

[mmaunu]

Originally Posted by Kevin Zhang

2) Just use the inherent PID objects inside the Talons
Problems
-When a setpoint is set through position, the drivetrain moves in the opposite direction that it should(If we set the talon to setpoint 256 ticks the drivetrain starts at 0 and goes backward so the encoder reads negative and never reads 256.
-When the setpoint is flipped(-256), the drivetrain also flips and starts reading positive.

It sounds like your encoder is "out of phase" with your motors. With a CANTalon object, you can use the reverseSensor() method, which will change the sign of the sensors output, but only if the sensor is plugged into the Talon itself. If it isn't plugged into the encoder, try switching the wiring; specifically, switch the A and B channels. If that doesn't help, you could write a wrapper class that reports the opposite of the encoder's actual value when pidGet() is called on it.

I would recommend going with pblankenbaker's suggestions. If you have an encoder that easily plugs directly into the Talon, go with the "enhanced" version; otherwise, go with the other.

[DGoldDragon28]

I actually advise writing your own PID. It's fairly simple. You just adjust your constants, which are arbitrary anyway, to account for the fact that the motors are -1 to 1. You may also want to limit the output to avoid faults.

private static final double KP = xx;
private static final double KI = xx;
private static final double KD = xx;
private static final double MAX_INTEGRAL_ERROR = xx;
private double integral = 0.0D;
private double pasterr;

public double motorPID(double encoderval, double desiredval) {
    double error = desiredval - encoderval
    if(error > MAX_INTEGRAL_ERROR) integral = 0.0D;
    else integral += error;
    double derivative = error - pasterr;
    pasterr = error;
    double out = KP * error + KI * integral + KD * derivative;
    out = Math.min(Math.max(out, -1.0D), 1.0D);
    return out;
}

You may want to put your constants on the SmartDashboard instead in order to tune the PID, or make other adjustments such as smoothing the data with an EWMA, but this is the basic idea. And I don't trust the wpilib PID, it doesn't act predictably in my experience.

[Tparbotmail]

Hi Kevin

We spent most of the weekend trying to get a feel for the closed loop CANTalon. We did quite a bit of testing. Here is what we found.

talonFL.setEncPosition(0);
talonFL.reverseOutput(false); // sets the "Reverse Closed-Loop Output" signal true reverses, false if default
talonFL.reverseSensor(true); // sets the "Reverse Feedback Sensor" signal, pass true to reverse closed loop math to reverse motor direction
this.talonFL.setInverted(true); // PercentVbus Mode
talonBL.reverseSensor(true);


We tested against these methods all weekend just trying to understand how they all work together. Primarily in autonomous. setInverted was necessary for teleo and non-closed loop work. That had an impact. What I could discover through research is the attachment I uploaded. I don't understand when I flipped the reverseSensor it did change the output to positive + in the "Selected Device 0 Quad Encoder" section. But below that in the "Quad Encoder 4x" Section it remained negative. Does someone understand why that is? Or what the relationship is? There are snippets in the software manual but it is not quite all in one place. Maybe it is an I missed it. But maybe some of the frustration is that this part is not well illuminated in the Software guide.

The picture is starting to form for our team but still a lot of unanswered questions. I'd say it took us over 8 hours just to figure out how to test phase alignment for for motor controllers. Robot has not been off the blocks all weekend just doing that. No significant testing of anything else either. Please let me know if you want to discuss in more detail.

[CyberTeam5713]

Down below is my code and we want the robot to drive with arcade with a xbox controller also plz help me to get it to turn. And we are using the standard wheels on the robot



package org.usfirst.frc.team5713.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Talon;



public class Robot extends IterativeRobot {
RobotDrive drive = new RobotDrive(1,2,3,4);
Joystick driveStick = new Joystick(0);
Joystick controlStick = new Joystick(1);
Talon frontLeft = new Talon(1);
Talon rearLeft = new Talon(2);
Talon frontRight = new Talon(3);
Talon rearRight = new Talon(4);

public void robotInit() {
drive = new RobotDrive(1,2,3,4);
driveStick = new Joystick(0);
controlStick = new Joystick(1);
frontLeft = new Talon(1);
frontLeft.enableDeadbandElimination(true);
frontLeft.set(+1.0);
rearLeft = new Talon(2);
rearLeft.enableDeadbandElimination(true);
rearLeft.set(-1.0);
frontRight = new Talon(3);
frontRight.enableDeadbandElimination(true);
frontRight.set(+1.0);
rearRight = new Talon(4);
rearRight.enableDeadbandElimination(true);
rearRight.set(-1.0);

public void teleopInit(){
drive = new RobotDrive(1,2,3,4);
driveStick = new Joystick(0);
controlStick = new Joystick(1);
frontLeft = new Talon(1);
frontLeft.enableDeadbandElimination(true);
frontLeft.set(-1.0);
rearLeft = new Talon(2);
rearLeft.enableDeadbandElimination(true);
rearLeft.set(+1.0);
frontRight = new Talon(3);
frontRight.enableDeadbandElimination(true);
frontRight.set(-1.0);
rearRight = new Talon(4);
rearRight.enableDeadbandElimination(true);
rearRight.set(+1.0);

}




public void teleopPeriodic() {
while (isOperatorControl() && isEnabled()) {
drive.setSafetyEnabled(true);


double joystickLeftY = driveStick.getRawAxis(2);
double joystickLeftX = driveStick.getRawAxis(1);
drive.arcadeDrive(joystickLeftY, joystickLeftX, true);
Timer.delay(0.01); } }

also our team is using standard wheels and we have the middle wheel and back wheel connected to drive train and the middle wheel has two motors and the same applies to the other middle wheel. for going forward and backward can someone plz help

[mikets]

Originally Posted by mikets

We are actually doing 3. The built-in PID control of CANTalon is nice but it is doing a per motor PID control which is fine if we are controlling a mechanism with only one motor (e.g. an arm or an elevator). In a drive base where you have at least 2, 4 or even 6 motors, individual motor PID control doesn't address the need for the drive base to run straight. For example, if you try to have the robot go forward 10 feet and say you have calculated 10 feet is equivalent to 10,000 encoder counts. If you command each motor to go 10000 encoder counts, what if the left motor is slightly faster than the right motor. The robot will curve to the right but the left motor will achieve the 10000 count earlier than the right motor, so it will stop while the right motor will try to catch up. This means the robot will now curve left. It will get there but the robot may be traveling in a slight 'S'. In a multi-motor PID control scenario, you need an overall PID control, not individual motor PID control. You need the ability to look at the overall drive base movement and be able to shift different power to different wheels in order to keep the robot on target whether it is the heading or the distance. So in our library, we implemented a PIDDrive class that takes two PID controllers. One PID controller is controlling the distance and the other PID controller is controlling the heading. My analogy to this is one is controlling the gas pedal and the other controlling the steering wheel. The PIDDrive class will combine all sensor data (e.g. 4 encoders and 1 gyro) to come up with "distance travelled" and "heading" data. Then it applies the distance data to the distance PID controller to calculate the base power for all four wheels. It also applies the heading data to the "Turn PID controller" to calculate the differential power to the left and the right side of the wheels. So it can add/subtract a delta power from the base power between the left and right side of the wheels (i.e. shift power from the left to the right and vice versa). This overall PID control is a lot better than individual motor PID control.
BTW, if you want to look at our library, you can find it here:
https://github.com/trc492/Frc2016Fir...cPidDrive.java

Just realize I have moved the file to a different package and so link is out-dated. Here is a new link:
https://github.com/trc492/Frc2016Fir...cPidDrive.java