Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)
-   -   Drive forward to x inches (http://www.chiefdelphi.com/forums/showthread.php?t=145779)

acrilex 03-15-2016 06:46 PM
Drive forward to x inches
 
Hello,

I have a problem. We are trying to achieve a function to drive to a specific distance, for exemple 10 inches. What I would like to be able to do is call driveToDistance(leftDistance, rightDistance).

We are using encoders, and they are well calibrated. For exemple, if we manually push the robot during 10 feets, RobotMap.driveTrainEncoderLeft.getDistance(); really return 120 inches for the left encoder.

How could we make a function so the robot goes forward to 10 feets using information returned by the encoders? I'm trying to understand PID, but a code exemple would help greatly.

For your information, in our code,
Code:
Left encoder is RobotMap.driveTrainEncoderLeft ;
Right encoder is RobotMap.driveTrainEncodeRight ;

Left motor is RobotMap.driveTrainLeftController ;
Right motor is RobotMap.driveTrainRightController ;

RobotDrive is RobotMap.driveTrainRobotDrive ;
(for tankDrive and arcadeDrive)

Thanks,
Acrilex

TimTheGreat 03-15-2016 06:55 PM
Re: Drive forward to x inches
 
So this is written in python but you can make it work for java.

Code:
def encoder_drive(self, target_position, max_speed):
                target_offset = target_position - self.return_drive_encoder_position() #Figure out how far off you are
               
                if abs(target_offset)> 1000: #If you're less than 1000 'ticks' away (maybe change to 1 inch for you) then drive forward more
                        self.y = target_offset * self.drive_constant.value # speed = offset * low constant (decreases speed as you approach target)
                        self.y = max(min(max_speed, self.y), -max_speed) #limit that speed to some speed x)
                        return False #you aren't at the position yet
                return True # if you're within an inch, you're there
later on in the code you want to tell robotdrive to drive with the y variable. Take a look at 1418's 2015 code here

acrilex 03-15-2016 06:58 PM
Re: Drive forward to x inches
 
Thanks,

I do understand the logic within this, we had a code with this kind of calculations, even slowing down when approaching at less than 5 inches, but the real problem is to actually make it work with PID, as I see many other teams are using this and it works perfectly.

Acrilex

TimTheGreat 03-15-2016 07:01 PM
Re: Drive forward to x inches
 
Quote:
Originally Posted by acrilex (Post 1557889)
Thanks,

I do understand the logic within this, we had a code with this kind of calculations, even slowing down when approaching at less than 5 inches, but the real problem is to actually make it work with PID, as I see many other teams are using this and it works perfectly.

Acrilex
So this is basically PID? Or really just P. You probably don't need I and D for driving a distance. You probably can't get much more accurate than within an inch anyway (from what we have found. But every robot is different).

GeeTwo 03-15-2016 10:34 PM
Re: Drive forward to x inches
 
If you aren't interested in arriving at a spot with precision, you can simply drive forward and turn off the motors when you reach the desired number of clicks traveled.

That is, turn on the drive, then each pass of your loop, check the distance traveled and turn the motors off when you reach the desired count. You WILL overshoot if you do this, but probably not by much. If you figure out how far you coast after stopping the motors, just apply a bit of Kentucky windage and turn off the motors that much early.

TimTheGreat 03-15-2016 10:51 PM
Re: Drive forward to x inches
 
Quote:
Originally Posted by GeeTwo (Post 1558023)
If you aren't interested in arriving at a spot with precision
Kinda defeats the purpose of encoders though, doesn't it? Timed drive will do the same.

Pault 03-15-2016 11:29 PM
Re: Drive forward to x inches
 
You will want to use WPILib's built in PIDController class for this.

I am assuming you are doing command based programming. If not, the same lines of code still apply, just in a different structure.

One thing to keep in mind is that PIDController will run its calculations in a separate thread. That means that you will need to call driveToDistance() once, and then continuously be checking if the PID is done yet with a different method. This fits in nicely with the command based structure, where you can put driveToDistance() in the initialize method, and do the check in isFinished().

Code:
public class Drivetrain extends Subsytem {

  public PIDController distancePIDLeft;
  public PIDController distancePIDRight;

  public Drivetrain() {
      distancePIDLeft = new PIDController(kP, kI, kD, RobotMap.driveTrainEncoderLeft, RobotMap.driveTrainLeftController, 20);
      distancePIDLeft.setAbsoluteTolerance(2); // Sets the tolerance, in the same units as your encoders, for how close to the target the PID needs to get to be considered finished
      distancePIDRight = new PIDController(kP, kI, kD, RobotMap.driveTrainEncoderRight, RobotMap.driveTrainRightController, 20);
      distancePIDRight.setAbsoluteTolerance(2); // Sets the tolerance, in the same units as your encoders, for how close to the target the PID needs to get to be considered finished
  }

  //enables the distance PIDs and sets them to the given distances
  public void driveToDistance(leftDistance, rightDistance) {
      distancePIDLeft.enable();
      distancePIDLeft.setSetpoint(leftDistance);
      distancePIDRight.enable();
      distancePIDRight.setSetpoint(rightDistance);
  }

  //if the both distance PIDs have completed, returns true and disables the PIDs
  public boolean atDistanceTarget() {
      if(distancePIDLeft.onTarget() && distancePIDRight.onTarget()) {
        distancePIDLeft.disable();
        distancePIDRight.disable();
        return true;
      } else {
        return false;
      }
  }
}

mikets 03-16-2016 04:51 AM
Re: Drive forward to x inches
 
Quote:
Originally Posted by acrilex (Post 1557889)
Thanks,

I do understand the logic within this, we had a code with this kind of calculations, even slowing down when approaching at less than 5 inches, but the real problem is to actually make it work with PID, as I see many other teams are using this and it works perfectly.

Acrilex
If you want to do precision autonomous driving, you may want to look at our library class TrcPidDrive.
https://github.com/trc492/Frc2016Fir...cPidDrive.java
This class contains several overloads of the method setTarget. The mecanum version of it takes the parameters xTarget, yTarget and turnTarget among other things. It uses three PID controllers, one controlling the X direction using the encoder, one on Y direction also using the encoder and one controlling the turn (or keeping it straight) using the gyro.


All times are GMT -5. The time now is 07:55 AM.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi