Chief Delphi - Drive train PID control

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)

- - Drive train PID control (http://www.chiefdelphi.com/forums/showthread.php?t=142575)

curtis0gj

26-01-2016 09:29

Drive train PID control

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

26-01-2016 15:51

Re: Drive train PID control

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

26-01-2016 16:28

Re: Drive train PID control

Quote:

Originally Posted by techkid86 (Post 1530475)

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

techkid86

26-01-2016 18:00

Re: Drive train PID control

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

26-01-2016 18:14

Re: Drive train PID control

Quote:

Originally Posted by techkid86 (Post 1530545)

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

26-01-2016 18:22

Re: Drive train PID control

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

26-01-2016 20:53

Re: Drive train PID control

Quote:

Originally Posted by techkid86 (Post 1530560)

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

26-01-2016 22:16

Re: Drive train PID control

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

27-01-2016 00:12

Re: Drive train PID control

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

27-01-2016 11:53

Re: Drive train PID control

Quote:

Originally Posted by techkid86 (Post 1530772)

kind of confused sorry. :yikes: 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

27-01-2016 14:32

Re: Drive train PID control

1 Attachment(s)

Quote:

Originally Posted by curtis0gj (Post 1530259)

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);

                        }

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

27-01-2016 14:39

Re: Drive train PID control

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

27-01-2016 15:31

Re: Drive train PID control

Quote:

Originally Posted by techkid86 (Post 1531019)

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

27-01-2016 15:40

Re: Drive train PID control

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

27-01-2016 15:52

Re: Drive train PID control

Quote:

Originally Posted by techkid86 (Post 1531046)

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());



        }

}

techkid86

27-01-2016 16:11

Re: Drive train PID control

1: you don't need the .get() in auto init. you might want to make it a .reset().
2: you also probably want to set a defined Max and Min output.
In auto periodic, you need to set a distance for the PID to get to, but before all that, you have to tune your PID.

PID tries to get to it's target with as little error as possible, but it uses coefficients to multiply by this error to get to this point. Now, for your use, it's likely you can get away with PD control, and leave I at 0. First thing you're going to need to do, is set it up such that your bot goes back and forth on it's own. ((be VERY careful with this. you might want the robot on blocks first, as I've had bad experiences with a run away robot while tuning PID)) you do this by using 2 set points, say, 100 and -100. it will set this set point every, say 10 seconds. now, the bot will get to that point, and either over shoot it, or slow down too early and never reach it. you want to only set 1 PID variable at a time.

So I typically start with P, and small values like 0.1 should be close. increase this until it starts to over shoot, or decrease it until it no longer overshoots, (then go back up one tick)

now you increase D until it no longer overshoots.

curtis0gj

27-01-2016 16:17

Re: Drive train PID control

PROGRESS! Sorry for doing a double post.

So I just enable autonomous and the robot drove backwards very quickly right. I guess I need to change the direction the robot drives, so going forwards instead of backwards and then make the leftPID mask the rightPID so it drives straight.

Code:

public class Robot extends IterativeRobot {

        double xThrottle = 0;



        RobotDrive tankDrive;



        Talon leftDrive = new Talon(0);

        Talon rightDrive = new Talon(1);



        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() {

                rightEncoder.setPIDSourceType(PIDSourceType.kDisplacement);



                rightEncoder.setReverseDirection(true);

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



                rightEncoder.reset();



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



                rightPID.enable();

        }



        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.reset();

        }



        public void autonomousPeriodic() {

                rightPID.setSetpoint(20);

        }



        public void teleopInit() {

                rightPID.disable();

                tankDrive = new RobotDrive(leftDrive, rightDrive);

        }



        public void teleopPeriodic() {

                double throttle = joystick.getRawAxis(Defines.THROTTLE);

                xThrottle = (-throttle + 2) / 4;



                tankDrive.arcadeDrive(joystick.getX() * xThrottle, -joystick.getY() * xThrottle);

        }

}

techkid86

27-01-2016 16:20

Re: Drive train PID control

Splendid! Sounds like you're on the right track. Shoot me a msg if you have any more issues, either PM here, or the skype that is connected to this account.

All times are GMT -5. The time now is 11:54.