Drive train PID control

curtis0gj · #1 26-01-2016, 09:29

Hello, my team this is looking at solutions for driving more accurately in autonomous. At this moment we are using this to drive a certain distance in autonomous.

Code:

if (encoderDistanceReading >= distance) {
				tankDrive.drive(0, 0);
				return;
			} else if (distance > encoderDistanceReading) {
				tankDrive.drive(-0.25, 0);
			} else {
				tankDrive.drive(0.25, 0);
			}

I am curious about using PID control to drive in autonomous with an encoder for input. I have looked at the Potentiometre PID example and read a bit about PID control (https://wpilib.screenstepslive.com/s...s-pid-control).

Our team is currently using the iterative robot template. The drive train is a four motored tank drive with 8 inch pneumatic tires. I have one encoder setup on the left small output shaft. We have a second encoder coming soon.

techkid86 · #2 26-01-2016, 15:51

The easiest way I can think to do this, is to initialize the motors separately without using the robotDrive class, as from the documentation, I can't find a way to integrate it with PID.

What kind of encoder are you using? continuous analog or digital quadrature?

curtis0gj · #3 26-01-2016, 16:28

Quote:

Originally Posted by techkid86

The easiest way I can think to do this, is to initialize the motors separately without using the robotDrive class, as from the documentation, I can't find a way to integrate it with PID.

What kind of encoder are you using? continuous analog or digital quadrature?

Digital Quadrature, similar to this one just a few years older. http://www.andymark.com/product-p/am-3132.htm.

techkid86 · #4 26-01-2016, 18:00

Be very careful with those encoders. My team has never successfully used these encoders in competition without hilariously disastrous consequences. Make sure you thoroughly test these in the shop and do plenty of research on how to mount them.

now, as to how to implement it. I did a little more research and found that you can infact use the robot drive class.

Code:

Encoder leftEncoder = new Encoder(1,2);
RobotDrive tank= new RobotDrive (/*numbers I dont know about*/)
PIDController leftControl = new PIDController(0.1,0,0, leftEncoder ,tank.m_frontLeftMotor);

This does not show how to tune PID, or set up encoder values, but it should be enough to get you started.

Code:

leftEncoder.setPIDSourceType(kDisplacement);
leftControl.enable();
leftControl.setSetpoint(distanceToTravel);

curtis0gj · #5 26-01-2016, 18:14

Quote:

Originally Posted by techkid86

Be very careful with those encoders. My team has never successfully used these encoders in competition without hilariously disastrous consequences. Make sure you thoroughly test these in the shop and do plenty of research on how to mount them.

now, as to how to implement it. I did a little more research and found that you can infact use the robot drive class.

Code:

Encoder leftEncoder = new Encoder(1,2);
RobotDrive tank= new RobotDrive (/*numbers I dont know about*/)
PIDController leftControl = new PIDController(0.1,0,0, leftEncoder ,tank.m_frontLeftMotor);

This does not show how to tune PID, or set up encoder values, but it should be enough to get you started.

Code:

leftEncoder.setPIDSourceType(kDisplacement);
leftControl.enable();
leftControl.setSetpoint(distanceToTravel);

Okay awesome, I will start working away at this. I am wondering if I will need to set up a right motor control and if so do I need a second encoder?

techkid86 · #6 26-01-2016, 18:22

It depends on what you're trying to keep track of in auto. If you're concerned only with distance, then one would work, but you would have to make sure all motors follow it. If you're using PWM, my team found it easiest to just splice the PWM cable into 2 end points so it can connect 2 Jags to 1 PWM output. This saves from having to set up sync/follower sets in code. ((obviously you would still need at least 2 PWM, one for each side))

if you're concerned about turning, (like when going over rough terrain) you may wish to use a gyro as well, as encoders will only track wheel spin, and not robot distance

curtis0gj · #7 26-01-2016, 20:53

Quote:

Originally Posted by techkid86

It depends on what you're trying to keep track of in auto. If you're concerned only with distance, then one would work, but you would have to make sure all motors follow it. If you're using PWM, my team found it easiest to just splice the PWM cable into 2 end points so it can connect 2 Jags to 1 PWM output. This saves from having to set up sync/follower sets in code. ((obviously you would still need at least 2 PWM, one for each side))

if you're concerned about turning, (like when going over rough terrain) you may wish to use a gyro as well, as encoders will only track wheel spin, and not robot distance

Driving straight for a desired distance is currently the main objective. However, it would be nice to be able to turn after going a set distance. My mentor did bring up the point about having wheels slip while going over rough terrain and we do have a gyro installed but I wasn't aware that it could be used to drive a distance.

Just starting adding some of the code but I am having difficulty with the leftControl. The error I am getting is: "The field RobotDrive.m_frontLeftMotor is not visible"

Code:

RobotDrive tankDrive = new RobotDrive(0, 1);
Encoder encoder = new Encoder(0 ,1 , true, EncodingType.k4X);
PIDController leftControl = new PIDController(0.1, 0, 0, encoder, tankDrive.m_frontLeftMotor);

I was looking at WPI docs and I realized that I may need to use the second constructor listed opposed to what I am doing now:
RobotDrive(int leftMotorChannel, int rightMotorChannel)
Constructor for RobotDrive with 2 motors specified with channel numbers.

RobotDrive(int frontLeftMotor, int rearLeftMotor, int frontRightMotor, int rearRightMotor)
Constructor for RobotDrive with 4 motors specified with channel numbers.

RobotDrive(SpeedController leftMotor, SpeedController rightMotor)
Constructor for RobotDrive with 2 motors specified as SpeedController objects.

RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor, SpeedController frontRightMotor, SpeedController rearRightMotor)

http://first.wpi.edu/FRC/roborio/rel...obotDrive.html

techkid86 · #8 26-01-2016, 22:16

That is a possibility, which you can fairly easily use if you split the PWM signal cable to both left drive jags. That way, you have one PWM signal, (one jag from the program's point of view) controlled by one PID/encoder.

Now, the gyro will help in keeping straight, but it will not help in going a set distance. for that, you would use an ultra sonic, (or similar range finder)

my team is trying to use vision to determine distance, but there is a myrid of other ways to do this. If your plan may involve drive slippage, PID distance control wont much help.

now, you might be able to combine that with an accelerometer, and a gyro to use PID after you get across the obstacle. That is, what for the accelerometer to say you are relatively level, use the gyro to straighten out, then use encoders to go forward a distance. IDK how well this will work in conjunction, since there is a lot of variability depending on your drive system.

techkid86 · #9 27-01-2016, 00:12

Sorry, after thought, it may be easier to just use basic the Jaguar class if you continue having issues with robotDrive. You'll have to set up your tank drive yourself, but it is as easy as linking a joystick to the jag's set function.

curtis0gj · #10 27-01-2016, 11:53

Quote:

Originally Posted by techkid86

Sorry, after thought, it may be easier to just use basic the Jaguar class if you continue having issues with robotDrive. You'll have to set up your tank drive yourself, but it is as easy as linking a joystick to the jag's set function.

kind of confused sorry.

Would I use:

Code:

RobotDrive tankDrive = new RobotDrive(SpeedController leftMotor, SpeedController rightMotor)

We are using Talons but I am guessing this will work the same?

grainne · #11 27-01-2016, 14:32

Quote:

Originally Posted by curtis0gj

Hello, my team this is looking at solutions for driving more accurately in autonomous. At this moment we are using this to drive a certain distance in autonomous.

Code:

if (encoderDistanceReading >= distance) {
				tankDrive.drive(0, 0);
				return;
			} else if (distance > encoderDistanceReading) {
				tankDrive.drive(-0.25, 0);
			} else {
				tankDrive.drive(0.25, 0);
			}

I am curious about using PID control to drive in autonomous with an encoder for input. I have looked at the Potentiometre PID example and read a bit about PID control (https://wpilib.screenstepslive.com/s...s-pid-control).

Our team is currently using the iterative robot template. The drive train is a four motored tank drive with 8 inch pneumatic tires. I have one encoder setup on the left small output shaft. We have a second encoder coming soon.

My team did this last year, and will probably do it again this year. I have attached the code for our drive train - we have a driveAuto method that would drive a number of inches passed as a parameter.

Our pid loop took a lot of time to tune, and we used mecanum drive, so please be aware of that.

Hope this helps

techkid86 · #12 27-01-2016, 14:39

More or less yes. So you're using talons. PWM I assume? if so, the following code should be enough to set up your drive.

using only basic classes:

Code:

Talon leftDrive=new Talon(1);
Talon rightDrive=new Talon(2);

//if using gamepad
Joystick gamepad=new Joystick(1);

//if using two individual joysticks
Joystick leftJoy=new Joystick(1);
Joystick leftJoy=new Joystick(2);

Encoder leftEncoder=new Encoder(1,2);
Encoder rightEncoder=new Encoder(1,2);

PIDController leftPID=new PIDController(0.1,0,0, leftEncoder, leftDrive);
PIDController rightPID=new PIDController(0.1,0,0, rightEncoder, rightDrive);

public void robotInit()
{
leftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
rightEncoder.setPIDSourceType(PIDSourceType.kDisplacement);

rightEncoder.setReverseDirection(true);
rightDrive.setInverted(true); //only one side needs inversion

leftEncoder.reset();
rightEncoder.reset();

leftPID.setOutputRange(-0.75,0.75); //max speed it can set to motors
rightPID.setOutputRange(-0.75,0.75); //max speed it can set to motors

leftPID.enable();
rightPID.enable();
}

public void 	autonomousPeriodic()
{
leftPID.setSetpoint(400);
rightPID.setSetpoint(400);
 //set distance. you can change distance per tick through the encoder
}

public void teleopInit()
{
leftPID.disable();
rightPID.disable();
}
public void teleopPeriodic()
{
//for two joysticks
leftDrive.set(leftJoy.getY());
rightDrive.set(rightJoy.getY());

//for one gamepad
leftDrive.set(gamepad.getRawAxis(1));
rightDrive.set(gamepad.getRawAxis(2));
//replace 1 and 2 axis with actual axis number. 
}

curtis0gj · #13 27-01-2016, 15:31

Quote:

Originally Posted by techkid86

More or less yes. So you're using talons. PWM I assume? if so, the following code should be enough to set up your drive.

using only basic classes:

Code:

Talon leftDrive=new Talon(1);
Talon rightDrive=new Talon(2);

//if using gamepad
Joystick gamepad=new Joystick(1);

//if using two individual joysticks
Joystick leftJoy=new Joystick(1);
Joystick leftJoy=new Joystick(2);

Encoder leftEncoder=new Encoder(1,2);
Encoder rightEncoder=new Encoder(1,2);

PIDController leftPID=new PIDController(0.1,0,0, leftEncoder, leftDrive);
PIDController rightPID=new PIDController(0.1,0,0, rightEncoder, rightDrive);

public void robotInit()
{
leftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
rightEncoder.setPIDSourceType(PIDSourceType.kDisplacement);

rightEncoder.setReverseDirection(true);
rightDrive.setInverted(true); //only one side needs inversion

leftEncoder.reset();
rightEncoder.reset();

leftPID.setOutputRange(-0.75,0.75); //max speed it can set to motors
rightPID.setOutputRange(-0.75,0.75); //max speed it can set to motors

leftPID.enable();
rightPID.enable();
}

public void 	autonomousPeriodic()
{
leftPID.setSetpoint(400);
rightPID.setSetpoint(400);
 //set distance. you can change distance per tick through the encoder
}

public void teleopInit()
{
leftPID.disable();
rightPID.disable();
}
public void teleopPeriodic()
{
//for two joysticks
leftDrive.set(leftJoy.getY());
rightDrive.set(rightJoy.getY());

//for one gamepad
leftDrive.set(gamepad.getRawAxis(1));
rightDrive.set(gamepad.getRawAxis(2));
//replace 1 and 2 axis with actual axis number. 
}

Is it complicated to set this up for arcade drive with one Logitech attack 3? We don't own two matching joysticks so tank drive would be funky.

This is what I thought of doing to have it work for arcade drive I have the PID setup stuff added on I just took it out for this.

Code:

public class Robot extends IterativeRobot {
	Talon leftDrive = new Talon(0);
	Talon rightDrive = new Talon(1);

	RobotDrive tankDrive = new RobotDrive(leftDrive, rightDrive);

	Joystick joystick = new Joystick(0);

	Encoder rightEncoder = new Encoder(0, 1, true,
			EncodingType.k4X);

	PIDController rightPID = new PIDController(0.1, 0, 0, rightEncoder, rightDrive);

	public void robotInit() {
	}

	public void autonomousInit() {
	}

	public void autonomousPeriodic() {
	}

	public void teleopPeriodic() {
		tankDrive.arcadeDrive(-joystick.getY(), -joystick.getX());
	}
}

techkid86 · #14 27-01-2016, 15:40

yes, just switch out the teleop init and periodic with the following.

Code:

RobotDrive drive;

public void teleopInit()
{
leftPID.disable();
rightPID.disable();

drive=new RobotDrive(leftDrive, rightDrive);

}

public void teleopPeriodic()
{
drive.arcadeDrive(oneJoy); //where Joystick oneJoy = new Joystick(1);
}

curtis0gj · #15 27-01-2016, 15:52

Quote:

Originally Posted by techkid86

yes, just switch out the teleop init and periodic with the following.

Code:

RobotDrive drive;

public void teleopInit()
{
leftPID.disable();
rightPID.disable();

drive=new RobotDrive(leftDrive, rightDrive);

}

public void teleopPeriodic()
{
drive.arcadeDrive(//however you setup the stick);
}

Alright I've got that setup nicely next step is to actually add in the PID jazz.

Code:

public class Robot extends IterativeRobot {
	RobotDrive tankDrive;

	Talon leftDrive = new Talon(Defines.DRIVE_CHANNEL_ZERO);
	Talon rightDrive = new Talon(Defines.DRIVE_CHANNEL_ONE);

	Joystick joystick = new Joystick(Defines.JOYSTICK_CHANNEL);

	Encoder rightEncoder = new Encoder(Defines.ENCODER_CHANNEL_ZERO, Defines.ENCODER_CHANNEL_ONE, true,
			EncodingType.k4X);

	PIDController rightPID = new PIDController(0.1, 0, 0, rightEncoder, rightDrive);

	public void robotInit() {
	}

	public void autonomousInit() {
		final double distancePerPulse = Math.PI * Defines.WHEEL_DIAMETER / Defines.PULSE_PER_REVOLUTION
				/ Defines.ENCODER_GEAR_RATIO / Defines.GEAR_RATIO * Defines.FUDGE_FACTOR;

		rightEncoder.setDistancePerPulse(distancePerPulse);
		rightEncoder.get();
	}

	public void autonomousPeriodic() {
	}

	public void teleopInit() {
		rightPID.disable();
		tankDrive = new RobotDrive(leftDrive, rightDrive);
	}

	public void teleopPeriodic() {
		tankDrive.arcadeDrive(joystick.getY(), -joystick.getX());

	}
}

Thread Tools
Show Printable Version Email this Page
Display Modes	Rate This Thread
Linear Mode Switch to Hybrid Mode Switch to Threaded Mode	Rate This Thread: