Kiwi Drive and Integrating PID Loops

[jgrindle]

Hey Everyone,
I got two main questions for today. The first being with a kiwi drive, I know that in general for driving the function would look a little like this:

void DriveBase::Drive(float joy_X, float joy_Y, float joy_Z){
	float Con1 = (joy_X + joy_Z);
	float Con2 = ((-((joy_X)/2))+((sqrt(3)/2)*(joy_Y)) + joy_Z);
	float Con3 = ((-((joy_X)/2))-((sqrt(3)/2)*(joy_Y)) + joy_Z);

	c1->Set(Con1);
	c2->Set(Con2);
	c3->Set(Con3);
}

But in design I do not have an equilateral triangle. I have what I am deeming a "Butterfly" Do I have to change any of the above code, to make this work?

Onto the next part. Along with that I have a custom PD loop that I am working on, I currently have not integrated the I term, as we have not gone over integrals in Calculus yet. I have decided against going with a regular Subsystem, because of how much of a pain they were during build season; I have less time than I did in build season, I have two weeks. Currently my code for the PD Drive is

//Used to fix deadzones on Joysticks, Gyros, or anything else
void DriveBase::dzFixer(float z){
	if((z >-(deadzone)) && (z < (deadzone))){
		z = 0;
	}
}

void DriveBase::PDDrive(float drive, float turn, float strafe, float kp, float kd)
{
	x = g1->GetAngle();
	dzFixer(x);
	dzFixer(turn);
	if(turn == 0 && (strafe > deadzone|| strafe < deadzone))
	{
		output = ((0 - x)* kp) + ((x+kp)*kd);// First Parentheses = Error, Second set = Derivitive of the first term
	}
	Drive(drive, output, strafe); //Correction made here
}

Is this sufficient? Should I go about and add an I term? If so, how would I do that? Is there an easier way?

[z_beeblebrox]

For your first question, here's what my team's programmer wrote as the general form for calculating wheel speed in a holonomic drive. We used this last year on our robot. The actual code we used can be found here.

I have an equation that should work to make a drivetrain move and turn about any centre of rotation with n wheels. It’s very long, but only needs xc, yc, vxc, vyc, ωc, xw, yw, and Θw, which can all be given or constant. c is the centre of rotation and w is the wheel.

pseudocode for each wheel:
double vtan = sqrt(pow((xc-x), 2)+pow((yc-y), 2))*ωc
double Θr = arctan((y-yc)/(x-xc))
double vwp = sqrt(pow(vtan*cos(Θr) + vxc, 2) + pow(vtan*sin(Θr) + vyc, 2))
double speed = vwp*cos(Θr - Θ)

Edit: also see Jared's more detailed description below.

For your second question, integration can be done in discrete timesteps (since the PID controller updates at some interval) and can be implemented without needing to know calculus:

At each iteration:
I += error/(time since last iteration)

Please let me know if you have further questions or don't understand something I wrote.

[Jared Russell]

Answer for first part:

The code you have looks reasonable for a "standard" kiwi drive. Let's look at another way to derive the solution.

For each wheel, determine the angle of the vector formed by forward motion on that wheel with respect to:
* the vehicle's forward axis
* the vehicle's lateral axis
* the center of rotation of the vehicle (which is always 0 or 180 degrees for a 3 wheeled vehicle as long as their axes of rotation all intersect at a point as in your attached drawing)

Additionally, determine how far the wheel is from the center of rotation (e.g. the radius of the circle that a given wheel drives around as the vehicle turns).

Let's call the front left wheel W1, the front right W2, and the rear wheel W3. To drive forward in a straight line, take the speed you would like the vehicle to travel at, and for each wheel multiply by the cosine of the angle that wheel forms with the forward axis.

The angle of W1 w.r.t. the forward axis is something like 150 degrees in your drawing (30 degrees, but let's say forward moves W1 in reverse). (Plug in whatever the actual angle is here). W2 will be something like 30 degrees. W3 is -90 degrees.

To go 1 m/s forward (ignoring conversion between linear and radial velocities):
W1 = 1 m/s * cos(150 degrees) = -.866
W2 = 1 m/s * cos(30 degrees) = .866
W3 = 1 m/s * cos(-90 degrees) = 0

Do the same for strafing. The angle w.r.t. strafing to the left (keep in mind the right hand rule) is simply 90 degrees less than the angle w.r.t. the forward axis. So you can actually use the same numbers, but plug in sine instead.

To go 1 m/s to the left:
W1 = 1 m/s * sin(150 degrees) = .5
W2 = 1 m/s * sin(30 degrees) = .5
W3 = 1 m/s * sin(-90 degrees) = -1

For turning, we already know the angle for each wheel is 0 (or 180 if the motor is flipped). Multiply the desired turning velocity of the vehicle (let's use deg/sec here) by the radius to obtain the desired linear veloctity for each wheel. Let's say the radius of W1 and W2 is 0.5 meters, and for W3 it is 0.25 meters (again, use whatever your numbers actually are).

To turn 1 rad/s counter-clockwise
W1 = 1 rad/s * 0.5 meters
W2 = 1 rad/s * 0.5 meters
W3 = 1 rad/s * 0.25 meters

Now how do you combine the commands? What if you want to move at 1 m/s forwards, 1 m/s to the left, and 1 rad/s counter-clockwise simultaneously? This is where holonomic vehicles are really nice. Just add the three equations together!

W1 = (forward velocity) * cos(150 degrees) + (lateral velocity) * sin(150 degrees) + (radial velocity) * 0.5 meters
W2 = (forward velocity) * cos(30 degrees) + (lateral velocity) * sin(30 degrees) + (radial velocity) * 0.5 meters
W3 = (lateral velocity) * sin(-90 degrees) + (radial velocity) * 0.25 meters

The cos and sin components are actually now constants that you can pre-compute (or use the constexpr version of std::sin to compute, but don't worry about that). Just figure out what the right angles and radii for your robot are and go for it.

Note that things get more interesting when you add together all of the commands and come up with a velocity faster than your wheel can spin. If this happens, you need to scale down the velocity of all of the wheels proportionally to maintain your desired direction of travel.

[jgrindle]

Thank you Both of you,
I will be using Jared's explanation for creating a new driving code. I believe that's actually what my angles were Jared, Inventor was just having trouble allowing me add them to the drawing.
Z_beeblebrox your explanation for an I term will make this much easier for me for writing this. I guess I'll partially be ahead of my Calc class too .
Thank you for the quick reply too .

[GeeTwo]

OK, what exactly do you have? Butterfly is normally a drive system which has an actuated shift between a standard tank/skid-steer system and mecanum (your link doesn't seem to go anywhere). In holonomic mode, it has wheels effectively oriented at 45, 135, 225, and 315 degrees. Kiwi is usually a three-wheel system utilizing omni wheels oriented 120 degrees from each other (probably at 120, 240, and 000, or 60, 180, 300, though other angles are possible. The only thing I've seen recently which might be a cross between the two is what I dubbed a Mackiwi, which was debuted by 585 recently at the Fall Classic. It has wheels at 135, 270, and 315.

How are your holonomic wheels oriented, where are they placed, and where is the center-of-gravity/center-of mass in relation to the wheels?

[jgrindle]

Gee Two,
I'm just calling the drive train a butterfly, because that is what it looks like when you look at the top view. Dropbox, has the link working for me (when I'm not signed in), I'm not sure why it isn't working. The design of the robot is a Kiwi, that is not at an equilateral triangle, but an isosceles. all the wheels are 120 degrees from each other, so they would be 150, 30, 90. Picture of the Base. the three wheels will be 6" VexPro Omnis.

[GeeTwo]

Originally Posted by jgrindle

Gee Two,
I'm just calling the drive train a butterfly, because that is what it looks like when you look at the top view. Dropbox, has the link working for me (when I'm not signed in), I'm not sure why it isn't working. The design of the robot is a Kiwi, that is not at an equilateral triangle, but an isosceles. all the wheels are 120 degrees from each other, so they would be 150, 30, 90. Picture of the Base. the three wheels will be 6" VexPro Omnis.

Your new link gives me a 403 error, but the originals finally resulted in some PDFs with some lengths. I have figured out that the shaft angles are, indeed, quite close to 120 degrees from each other. However, when I extend the three axles, they meet only about 5" from the "front" axle mount, and about 20" from the two "wing" axle mounts. If the CoG were that far forward, you would have to divide all of the speeds for that wheel by something which approximates 4. However, I find it highly unlikely that the CoG is that far forward, unless you've specifically ballasted to make it so. Where is the CoG?

[jgrindle]

According to Inventor the CG rests right behind the back of the opening. When we finalize building it, we will actually have to put some weights in the back, to balance the robot out when we have a full stack on it. Without the weights(With all the Electronics still on it though), the CG will be really close to the center of the robot. This will be because of the Elevator we will be adding, and the built in tote ramp(Sort of like 1902's or 125's) we will be adding to it.

[GeeTwo]

Originally Posted by jgrindle

According to Inventor the CG rests right behind the back of the opening. When we finalize building it, we will actually have to put some weights in the back, to balance the robot out when we have a full stack on it. Without the weights(With all the Electronics still on it though), the CG will be really close to the center of the robot. This will be because of the Elevator we will be adding, and the built in tote ramp(Sort of like 1902's or 125's) we will be adding to it.

Just eyeballing it, this would mean that the CoG is going to be about 10" from the center wheel, and about 20" from the side wheels. Assuming that you're using encoders and controlling based on speed, this would mean that Jared's scaling of W3 at half of that for W1 and W2 is correct. As your CoG moves farther away from the point where the axes cross, you may find that you will have to scale the side wheels' rotational values (W1 and W2) down a bit to compensate. The amount would be by cosФ, where Ф is the angle between the line from the CoG to the wheel's floor contact and the axle for that wheel.

[jgrindle]

The Cg is about 8.45 inches from the back center wheel, while 20.5 inches from the front side wheels. When we adds weight, we will try to make the CG at the point of intersection, to minimize the change wed need to do to theta. Thus making the scaling(if any at that point) very small numbers.

If anyone wants to help contribute to the code, I have a Github Repo that you can look at, modify, and point out any noticeable bugs. I'd suggest avoiding intake, and elevator at the moment, as they are not ready.

[jgrindle]

z_beebleBrox,
I added the I term to the loop, in turn it ended up looking like:

void DriveBase::TimerStart(){
	timer->Start();
}
void DriveBase::TimerStop(){
	timer->Stop();
}
//Used to fix deadzones on Joysticks, Gyros, or anything else
void DriveBase::dzFixer(float z){
	if((z >-(deadzone)) && (z < (deadzone))){
		z = 0;
	}
}

void DriveBase::PIDDrive(float drive, float turn, float strafe)
{
	dzFixer(turn);
	if(turn == 0 && (strafe > deadzone|| strafe < deadzone))
	{
		tim = float(timer->Get());
		x = g1->GetAngle();//get gyro reading
		dzFixer(x);
		I += (error()/tim);
		output = (error()* kp) + ((x+kp)*kd) + (I*ki);// First Parentheses = Error, Second set = Derivitive of the first term, I = integral
		Drive(drive, output, strafe); //Correction made here
		tim -= float(timer->Get());
	}else{
		Drive(drive,turn,strafe);
	}

}
float DriveBase::error(){
	float err;
	err = (0-x);
	return err;
}
void DriveBase::DStop(){
	Drive(0,0,0);
}

I'm hoping this is what it should look like, I should be able to test it soon, I can tell you the results when I do. I'm hoping this is readable code