JAVA code to drive straight using H-drive

[RobOTies]

We are a second year team and are using 6" omni wheels in an H-drive configuration for the first time. Our robot tends to veer left when we are trying to drive straight ahead during auto and teleop, and I was wondering if anyone would be willing to share any code ideas that would help with this issue. We have a gyro as well, but not sure how to effectively utilize it for this purpose. We have already tried making sure the gear boxes that are attached to CIM motors are as identical as possible.

Go robots! Thank you!!

[kinganu123]

FIRST gave an example on their docs as to how you can accomplish driving straight using a gyro. However, just be wary that gyros don't always work as well as you want it to.
http://wpilib.screenstepslive.com/s/4485/m/13809/l/241871-gyros-measuring-rotation-and-controlling-robot-driving-direction

[GeeTwo]

The usual way to ensure you're driving straight is to put encoders on your wheel gearboxes, and get both sides going at the same speed. As long as the wheels don't slip, you can make this work by creating a PID on each axle based on speed, and set the target speed from your joysticks rather than the voltage. If you have time to do this, this is the better solution.

However, you probably don't have time, and you already have a gyroscope. During teleop, you're probably better off letting the driver compensate, unless you really do have a "directly forward" control (which means, among other things, not arcade or tank controls). If you do have a "directly forward", do the same as for autonomous below.

Autonomous: First, calibrate the gyroscope to determine the value received when the robot is not rotating. Then, as you're driving forward (or reverse), periodically check the rotation rate. If you are rotating to the left, decrease the right voltage a bit (or increase the left). Likewise, if you are rotating right, decrease the left voltage speed a bit. As you drive, continue the decrease/increase until the rotation rate is the desired value of zero.

As noted above, a gyroscope won't pinpoint you in an exact spot, but it may get you close enough so that you can use other sensors (rangefinders, touch sensors, etc) to get exactly where you want to be.

BTW, we've lost three analog KoP gyroscopes in the past year, two during this year's build season. If you can't get things working with the gyroscope, try checking it with a voltmeter or reading the values from it directly to verify that the hardware is still working.

[RobOTies]

Thank you for the suggestions! We did put encoders on both gearboxes before we bagged up, but we weren't sure how to use them with PIDs. Any tips on setting the constant values if we try using velocity? Will that work in auto since the robot would not be getting input from a joystick?

I'm a little confused about how to control the voltages independently for each axis because we use arcade drive. Is there another drive class that would work better for this task?

We will check the gyro when we unbag - thank you for that tip!

[Ben Wolsieffer]

BTW, we've lost three analog KoP gyroscopes in the past year, two during this year's build season.

That seems kind of odd, given that we have used to exact same KOP gyro for two years without any problems.

[GeeTwo]

BTW, we've lost three analog KoP gyroscopes in the past year, two during this year's build season. If you can't get things working with the gyroscope, try checking it with a voltmeter or reading the values from it directly to verify that the hardware is still working.

That seems kind of odd, given that we have used to exact same KOP gyro for two years without any problems.

I agree that it's odd. Our 4 KoP gyroscopes were the literal KoPs from the previous 3 years, and one we bought our first year; all four were working in Jan 2014. I would have thought we were doing something wrong electrically, but one of them was working before lunch break, and did not come up after lunch break. No one was working on the robot during that time, apart from the battery change. I suppose the warranties are running out:(. I just wanted to throw this out to try to keep other teams from spending as many hours as we did trying to get a navigation system work because we were focused on software and ignored the hardware.

[GeeTwo]

We did put encoders on both gearboxes before we bagged up, but we weren't sure how to use them with PIDs. Any tips on setting the constant values if we try using velocity? Will that work in auto since the robot would not be getting input from a joystick?

I'm a little confused about how to control the voltages independently for each axis because we use arcade drive. Is there another drive class that would work better for this task?

From screenstepslive (http://wpilib.screenstepslive.com/s/3120/m/7912/l/95588-getting-your-robot-to-drive-with-the-robotdrive-class#!prettyPhoto):
You can use RobotDrive with other types of speed controllers as well. In this case you must create the speed controller objects manually and pass the references or pointers to the RobotDrive constructor.

The speed controller in this case would have both the actual speed controller and a PID "front end". This is described at this page (http://wpilib.screenstepslive.com/s/3120/m/7882/l/79335-writing-the-code-for-a-pidsubystem-in-java).

[Joe Derrick]

This is our auto drive straight command for our robot this year.
We are using command based programming and java.
We used the RobotBuilder "PIDCommand" as a template that we modified to what you see below.

It drives forward at a set speed and maintains an angle using values from encoders on each side of the drivetrain.
We have removed some parts of the code as they are specific to our robot.
Hopefully this is of use to you.


public class AutoDriveStraight extends PIDCommand {

double driveSpeed = 0;
public AutoDriveStraight(double speed) {

super("AutoDriveStraight", 0.25, 0.0, 0.0, 0.02);

//we set our P to be 0.25 but this will need tuning.

getPIDController().setContinuous(false);
getPIDController().setAbsoluteTolerance(1.0);
driveSpeed = speed;


requires(Robot.driveTrain);

}
protected double returnPIDInput() {

return Robot.driveTrain.getDegreesFromEncoderValues();

//This is a method that uses Encoders and a formula
//to calculate a degree value. You could use the
//difference between the drivetrain encoders.

}
protected void usePIDOutput(double output) {

Robot.driveTrain.arcadeDrive(driveSpeed, output);

//This is a method within our Drivetrain subsystem.
//The driveSpeed is the constant, and the
//PIDController maintains the Setpoint set below.

}

protected void initialize() {



this.setSetpoint(0);

// This maintains the previously calculated
//degrees from the PIDInput at 0. ie: "Straight ahead"

this.getPIDController().enable();

}

protected void execute() {
}

protected boolean isFinished() {

return //a boolean that tells when you want your command to finish

}

protected void end() {
}

protected void interrupted() {
}
}

[nickbrickmaster]

I agree that it's odd. Our 4 KoP gyroscopes were the literal KoPs from the previous 3 years, and one we bought our first year; all four were working in Jan 2014. I would have thought we were doing something wrong electrically, but one of them was working before lunch break, and did not come up after lunch break. No one was working on the robot during that time, apart from the battery change. I suppose the warranties are running out:(. I just wanted to throw this out to try to keep other teams from spending as many hours as we did trying to get a navigation system work because we were focused on software and ignored the hardware.

We lost a gyro during competition this weekend because the roboRIO decided to output 12v instead of 5v and melted the cable off the gyro. Another one just didn't work when we got it. I'm switching to I2C gyros or maybe the navX.

[Ether]

the roboRIO decided to output 12v instead of 5v

Was this intended as this a joke? Hard to tell.

[nickbrickmaster]

Was this intended as this a joke? Hard to tell.




Sorry, it was not. Our auto started acting up during testing and we decided to check the gyro. The circuit was extremely hot, and upon investigating, the male PWM pins had melted up to the header. I checked the analog input with a multimeter to discover that what should have been 5v with a 2.5v signal pin was getting 12v with a 2.5v signal pin. We got a new gyro from spare parts, switched the pin in the code, and have not run into the problem again.

It was most likely some kind of short, but we have a cover on the gyro as well as a mounting plate, and Cable Magics on all the 3-pin roboRIO ports.

[Techwiz]

@Joe can you share the getDegreesFromEncoderValues method? That is the part we are having trouble with.
Robot.driveTrain.getDegreesFromEncoderValues(); .

[Joe Derrick]

sure


public double getDegreesFromEncoderValues(){

double leftDistance = Robot.drivetrain.leftEncoder.getDistance();
double rightDistance = Robot.drivetrain.rightEncoder.getDistance();
double robotArcLength = (leftDistance-rightDistance)/2;
double robotDegrees = 360*(robotArcLength/(Math.PI*wheelbase);

return robotDegrees;
}

[Ether]

double robotArcLength = (leftDistance-rightDistance)/2;
double robotDegrees = 360*(robotArcLength/(Math.PI*wheelbase);

You want to use the trackwidth (distance between left and right wheels), not wheelbase (distance between front and back wheels).

[GeeTwo]

double robotArcLength = (leftDistance-rightDistance)/2;
double robotDegrees = 360*(robotArcLength/(Math.PI*wheelbase);

Also, that should be 180, not 360. If your left wheels have traveled an extra track width, you will have turned one radian (57 degrees), not two (114 deg). If you're just using this for a PID, it probably didn't matter, because you wound up dividing all your PID constants by 2 in the tuning. It should read:

double robotArcLength = (leftDistance-rightDistance)/2;
double robotDegrees = 180*(robotArcLength/(Math.PI*trackWidth);

[Ether]

Also, that should be 180, not 360.

No, 360 is correct.

arc_length=(Left-Right)/2

arc_radius = trackwidth/2

arc_angle = arc_length/arc_radius = arc_length/(trackwidth/2) = 2*arc_length/trackwith

convert to degrees:

2*arc_length/trackwith * (180/pi) = 360*arc_length/(trackwidth*pi)