Accurately "Arcing" in Auton/Hybrid Mode

[kiettyyyy]

Hey all,

After competing at two regionals and watching numerous matches online, many teams are able to complete smooth turns based on some sensor such as a gyro or encoder while having some amount of momentum in the forward direction.

Does any body have sample code or could some what guide me in the right direction of performing a smooth turn, accurately(goes from point a to point b consistantly)?

Thanks in advance for the help!

[Uberbots]

to do our arc, we tell the robot to do a certain number of rpms on each side of the wheel base, using the following (pseudo) code:

VForward = 40;
omega = dist_from_wall / some_constant + desired_dist_from_wall / some_constant;
wheel_left = VForward + omega;
wheel_right = VForward - omega;

it has gotten us some pretty accurate turns, with some tweaking.

[kiettyyyy]

Originally Posted by Uberbots

to do our arc, we tell the robot to do a certain number of rpms on each side of the wheel base, using the following (pseudo) code:

VForward = 40;
omega = dist_from_wall / some_constant + desired_dist_from_wall / some_constant;
wheel_left = VForward + omega;
wheel_right = VForward - omega;

it has gotten us some pretty accurate turns, with some tweaking.

I just was thinking about that once you've posted!

That's one of the options, and it looks like it would make an accurate turn.

I'll play with this tomorrow and see if this would work out for us.

Thanks!

[billbo911]

Originally Posted by kiettyyyy

Hey all,

After competing at two regionals and watching numerous matches online, many teams are able to complete smooth turns based on some sensor such as a gyro or encoder while having some amount of momentum in the forward direction.

Does any body have sample code or could some what guide me in the right direction of performing a smooth turn, accurately(goes from point a to point b consistantly)?

Thanks in advance for the help!

2073 was able to make the U-turn at the end of the home stretch every time. Our robot was calibrated to make a U-turn with a diameter of 13.5 feet. If you'll notice, that is the width of the lane from the center divider to the outside wall. So, the robot would start out in one of three "lanes" in our home stretch and end up in the same lane on the opposing alliance side of the field.

Here is how we did it.
1) We used a trial and error method to determine exactly what fixed PWM values for the left and right drive wheels it would take to get the robot to make a U-Turn and end up 13.5 feet from where it started.
2) We figured out exactly how many counts our GTS would need to reach to go from the starting position until it reached the far end of the lane divider.
3) We commanded the robot to drive straight, again using fixed PWM values for right and left, until we reached the count value determined in step #2.
4) Once the count value was reached, the robot would then apply the "U-Turn" PWM values until our gyro output said we had turned 180 degrees.
5) Once we were facing 180 degrees from where we started, we then applied the straight driving values again.


All in all, it worked quite well. We even scored 3 lines and two balls with this simple process.

Honestly, we had planned to use our sensors better, but other issues prevented us from using them fully. Maybe by the time CalGames comes around we can have it working better.

[kiettyyyy]

Originally Posted by billbo911

2073 was able to make the U-turn at the end of the home stretch every time. Our robot was calibrated to make a U-turn with a diameter of 13.5 feet. If you'll notice, that is the width of the lane from the center divider to the outside wall. So, the robot would start out in one of three "lanes" in our home stretch and end up in the same lane on the opposing alliance side of the field.

Here is how we did it.
1) We used a trial and error method to determine exactly what fixed PWM values for the left and right drive wheels it would take to get the robot to make a U-Turn and end up 13.5 feet from where it started.
2) We figured out exactly how many counts our GTS would need to reach to go from the starting position until it reached the far end of the lane divider.
3) We commanded the robot to drive straight, again using fixed PWM values for right and left, until we reached the count value determined in step #2.
4) Once the count value was reached, the robot would then apply the "U-Turn" PWM values until our gyro output said we had turned 180 degrees.
5) Once we were facing 180 degrees from where we started, we then applied the straight driving values again.


All in all, it worked quite well. We even scored 3 lines and two balls with this simple process.

Honestly, we had planned to use our sensors better, but other issues prevented us from using them fully. Maybe by the time CalGames comes around we can have it working better.

In the case that your drivetrain has some type of inefficiency, ie: a motor going bad, etc. Couldn't your robot perform an arc with a wider or tighter turn radius than you commanded your robot to perform(without encoders or gear tooth sensors on both sides of your drivetrain)?

How do you go about countering this issue?

[AdamHeard]

Originally Posted by kiettyyyy

In the case that your drivetrain has some type of inefficiency, ie: a motor going bad, etc. Couldn't your robot perform an arc with a wider or tighter turn radius than you commanded your robot to perform(without encoders or gear tooth sensors on both sides of your drivetrain)?

How do you go about countering this issue?

If you know your robots approximate speed (via a commanded PWM and verifying with encoders or such) and you command a consistent angular turn rate by a gyro, wouldn't it have to turn in a decent arc? I've been out of the loop of programming for a while, but it seems feasible to me.

[kiettyyyy]

Originally Posted by AdamHeard

If you know your robots approximate speed (via a commanded PWM and verifying with encoders or such) and you command a consistent angular turn rate by a gyro, wouldn't it have to turn in a decent arc? I've been out of the loop of programming for a while, but it seems feasible to me.

That's one of my options at the moment.

[Jay Lundy]

Hey Kiet. I did some thinking about how to do this earlier but haven't talked to anyone on 254/968 about it, so I'll just tell you here.

Here's some of the math I came up with:

If:
V_L = Velocity of the left wheels
V_R = Velocity of the right wheels
w = Width of robot (distance from left wheels to right wheels)

Then:

omega = angular speed = (V_R - V_L) / w
r = turning radius = w/2 * (V_R + V_L) / (V_R - V_L)

A turning radius of 5 feet means you are turning around a point 5 feet to the left of the center of the robot.

So if you use Uberbots' code and do V_R = v + x, V_L =v - x, then you can solve for x for a desired r:

x = w/2 * v/r

Of course I've been talking about velocity all this time, not voltage. You can try to find what PWM outputs give you the right velocities, but like you said that can be a problem. The best thing is a feedback loop for each side, some sort of PID loop.

Using the gyro and encoders like Adam suggested will work too. In that case you can calculate the turning radius from just V_L = left velocity, and omega = gyro output:

r = V_L / omega + w/2

[Jay Lundy]

By the way, if you do use a PID loop to control the velocities I think you can improve stability (ie overshooting) and probably even response time by using both feedback and feed-forward control.

Instead of sending this to the motors:

pwm_out = PID(e) // PID is your PID function, e is the speed error

send this:

pwm_out = guess(target_velocity) + PID(e)

The guess function takes your PID setpoint, the target velocity, and guesses what PWM output you need to reach that velocity. It doesn't have to be exact, but as long as you're close you can improve the performance of your PID loop. The guess function ideally ends up providing most of the output and the PID loop just makes small corrections.

By the way your straight drive code uses the same idea (both sides are fed throttle and the PID loop just makes small adjustments to keep them at the same speed).

[billbo911]

Originally Posted by kiettyyyy

In the case that your drivetrain has some type of inefficiency, ie: a motor going bad, etc. Couldn't your robot perform an arc with a wider or tighter turn radius than you commanded your robot to perform(without encoders or gear tooth sensors on both sides of your drivetrain)?

Yes, that is absolutely correct. We were basically doing "DEAD RECKONING". Any variation in the drivetrain or an impact from another robot could cause this entire process to fail.

Originally Posted by kiettyyyy

How do you go about countering this issue?

That is what I was referring to in the last part of my post. One reliable way for you to maintain your intended path, taking into account bumps and additional drag, would be to use feedback via encoders in your drivetrain, and your heading with a gyro.
Then when deviations occur from your intended path your robot could automatically respond to them.

As Jay Lundy suggests, it is possible to automatically make a U-turn of a known diameter with a Gyro.

An alternative would also take a little trial and error, but is fairly simple.
Set the inside wheel to a fixed PWM value. Then use a PID loop to control the outside wheel. The feedback you would use it the RATE out of the gyro, not the angle. Give the PID loop the rate you desire, it will then modify the output until the rate is achieved. Then all you have to do is determine what PWM value given to the inside wheel will produce the diameter turn you desire.

[Bongle]

One way to look at it is that you're constantly trying to maintain a ratio between your left and right sides.

When you're going straight, the encoders/GTS's on your left and right sides should maintain a 1:1 ratio. If substantially less or more, you need to adjust.

When you're turning, you can determine the ratio based on the radius of the turn you want and the width of your robot, which can be dynamic and fed to the system by your robocoach.

Then you can use a PID feedback loop to determine the correct PID values to maintain the desired ratio.

One thing I found while coding 1281's auto mode though, is that motor glide is a problem. I coded a turn that was about the radius in the pits when run while the robot was stopped, but since the robot carried a lot of speed into the turn, the same corner voltages resulted in a barely perceptible left turn. I recommend putting the victors into BRAKE mode instead of GLIDE mode, that might help (don't know if it actually would, I didn't get to test that theory). Another issue we had was our gear tooth sensors seemed to only count about 60% of the gear teeth when the robot was running at high speed, but worked fine in the pits if we many turned the wheels slowly.

[Tom Line]

In our case, what looks a lot like a smooth turn isn't really programmed like one.

Our drive statement looks like this:

left_drive = 127 + drive_straight(total_distance) + turn(angle_desired);
right_drive = 127 + drive_straight(total_distance) - turn(angle_desired);

Here is how it works. Drive_straight is a pid loop that will drive to a "distance". Generally speaking we put something like 100 feet into it this year, because we don't really want to ever stop.

So, generally speaking, you could simplify our drive formulas to this:

left_drive = 254 + turn(angle_desired);
right_drive = 254 - turn(angle_desired);

We use a 300 degree/sec gyro in the turn function. It is a simple PID loop. It can output -254 to +254. We send it "turn 90 degrees" and it calculates the amount to modify the drive values.

This results in one side (the right side) always driving full forward. Because Kevin's gyro code measures any angle to the "left" of zero as a negative, our right drive ends up being right_drive = 254 - (-xxx) which results in right_drive = 254 + xxx. At the end of our loop, we have a sanity check that says
if right_drive > 254
right_drive = 254;
else if right_drive < 0
right_drive = 0;
We have the same for the left.

So our right_drive is pretty much ALWAYS 254. Our left drive is modified by the amount we want to turn.

All this results in a final "turn left 90 degrees" command resulting in a nice smooth curve for example:
left_drive = 200;
right_drive = 254;

or a very sharp "left":
left_drive = 127;
right_drive = 254;

depending on the gain that we're using at the time in the turn routine. The gain is determined by looking at where we think we are on the field.

Oh - and we also used a side sensor that looks for the end of the wall, OR robots, so we go past them before we start our turn. We also used a sensor in front (which I think a lot more robots out there this year should have used) that stops our robot if something is within a couple inches in front of us. That resulted in a lot of "saved" opponent robots when we came around the corner and there was someone that smacked into the center wall blocking our way. It also prevented us from backing head-on into the alliance walls or the center divider if someone managed to spin our robot around.

[Jared Russell]

Originally Posted by Jay Lundy

By the way, if you do use a PID loop to control the velocities I think you can improve stability (ie overshooting) and probably even response time by using both feedback and feed-forward control.

Instead of sending this to the motors:

pwm_out = PID(e) // PID is your PID function, e is the speed error

send this:

pwm_out = guess(target_velocity) + PID(e)

The guess function takes your PID setpoint, the target velocity, and guesses what PWM output you need to reach that velocity. It doesn't have to be exact, but as long as you're close you can improve the performance of your PID loop. The guess function ideally ends up providing most of the output and the PID loop just makes small corrections.

By the way your straight drive code uses the same idea (both sides are fed throttle and the PID loop just makes small adjustments to keep them at the same speed).

The feed forward option is almost always going to have better transient response in velocity control in terms of overshoot and oscillation, although it needs to be well tuned to improve rise time.

I would caution, though, that you probably want to code it like this:

pwm_out = feed_forward_gain*guess(target_velocity) + (1-feed_forward_gain)*PID(e)

Where "feed_forward_gain" is between 0 and 1 (functionally it will probably be .7+). You will want to do this using integer math, of course.

This clamps your output, and lets you use the same gains as in the pure feedback case.

[Terry Sherman]

We use both a gyro and a set of encoders for our navigation system. We are now consistently getting 4 lines in autonomous and sometimes getting a 5th. We're negotiating 180 degree turns on both ends of the field. Here a quick rundown of what we do.

Setup:
- We have 1 gyro mounted on the robot, mounted in the middle of the robot (centered on the width of the robot, not necessarily front/back center.)
- We use two encoders, one encoder for each side of the drive (left and right).

Modes:
We have 2 basic autonomous 'commands' or 'modes' for driving: cmd_drive_straight & cmd_turn. First we drive straight for the length of the center wall, then we do a turn, then we drive straight again to go back down the length of the field, then we turn again, then we drive straight… until the autonomous(hybrid) period ends.

Driving straight:
You'd think we could just put the motors at full speed and voila we'd go straight but that’s not the case for our robots. Each year we have a bit of an arc so we can't just drive straight without feedback. (This arc is caused by mechanical differences in the drive train, friction, and unbalanced motors, etc.) We could experiment and find a left and right pwm that would make us go more or less straight but we're not happy with that (As the robot gets broken in, the friction will change and what might have driven straight before might not be straight later. We want consistency so we decide to create some software to actively control that we drive straight so we don’t have to keep tweaking at the competitions). So how do we drive straight? We actually use the gyro from the Kit Of Parts. When we start driving straight we record what our current angle is and reset our distance. We then output pwm commands to the motors to get us moving. We look at the angle and see if we’re turning away from our original angle (heading). If we see we’re turning left we put less power (pwm) to the right side so we straighten out and vice versa.

Psuedo code:

int Cmd_drive(int InchesToDrive)
{
  static int heading;
  int retVal = WORKING;
  int leftMotor, rightMotor = 127; //by default stop driving motors
  int speed;
  int angleAdj;

  switch(drive_straight_state)
  {
    case INITIALIZE:
      Heading = getAngle(); //Determine what angle(heading) we start at
      ResetDistance();
      drive_straight_state = RUNNING;
      break;
    case RUNNING:
      if(getDistanceTraveled() >= InchesToDrive)
      {
        retVal = DONE; //tell caller we are done
        drive_straight_state = INITIALIZE;
      }
      else
      {
         Speed = 80; //how fast from 127 to drive straight, can be 127
         angleAdj = getAngle() – Heading; //determine how much angle error
         angleAdj *= 2; //Scale the error (proportional term). This needs   
                        //tuning to see how much you need to adjust by
                        //inorder to correct the error
         leftMotor  = speed + angleAdj;
         rightMotor = speed - angleAdj;
      }
      break;
   }
   setLeftMotorPWM( leftMotor);
   setRightMotorPWM(rightMotor);
}

Turning while driving:
We use the gyro in this code too. The code for this is *almost* exactly the same as above. What we do is control the rate of angular change. How we do that is we change the heading we want to go at each loop. So in the drive straight code above we have a constant heading the entire time (because we want to drive straight). If we were to change the heading as we drive, the code would automatically turn the robot to point in that heading (direction). To do that we need to know how much to turn per unit time (each loop). This takes some experimenting but let’s say at a given speed you do an arc in 2.5 seconds. That’s 180 degrees in 2.5 seconds. The code runs once every 26.2ms so for each loop we need to adjust the heading a certain amount to get 180 degrees in 2.5 seconds. 2.5sec/26.2ms/loop = ~96 loops. 180deg/96loops = 1.875 degrees per loop. Via experimentation you can them come up with actual numbers to replace the 1.875 to get the size of the arcs you need to turn.

So the code in the ‘case RUNNING:’ would have the following added:

Heading += 1.875; //Add the amount of rotation per loop.

You cannot actually add 1.875 to an integer like this, hence why its psuedo code. We actually use units of milli-radians rather than degrees so we can easily add the equivalent of 1.875 degrees per loop. You could also do milli-degrees instead. If you did that 180 degrees is 180,000 and 1.875 degrees is 1875.

[Tom Bottiglieri]

You may want to investigate a velocity level PID control loop based off a gyro rate. This should kill two birds with one stone (arc turns and lock to heading.. kind of)

Your rate control could come to the following

left = speed + calculated_turn_rate;
right = speed - calculated_turn_rate;