Offseason Project: Holonomic Kiwi Drive Robot

[Ether]

Originally Posted by EthanMiller

Okay... bear with me here.

Not a problem.

The 157 was a typo. It should have been 1.57


As for your other concerns, I should have explained more fully.

The maximum speed of a Kiwi drive is different in different directions. If the max speed that a Kiwi can drive in the sideways direction is, say, 6 feet per second, then the max speed in the forward direction will be (6)(2/sqrt(3)) = 6.93 feet per second... 15% faster.

When doing an inverse kinematic calculation to obtain wheel speeds, the inputs to the formulas (the joystick values) represent vehicle speed commands. The inverse kinematic formulas convert those into the individual wheel speeds necessary to achieve the desired overall vehicle speed.

The formulas I gave you were scaled to present the driver with a uniform vehicle speed response in all directions.

So for example, suppose your Xj and Yj joystick values were first scaled to a range of -6 to +6 (representing +/-6 feet per second vehicle speed) before feeding them to the inverse kinematic formulas.


To command the vehicle to go forward at 6 feet per second, you would issue joystick commands Xj=0 and Yj=-6.

Calculating the wheel speeds for Xj=0 and Yj=-6, you would get:

W1 = 0

W2 = -6*0.866 = -5.2 feet per second tangential wheel velocity

W3 = -6*(-0.866) = +5.2 feet per second tangential wheel velocity

... which would cause the vehicle to go forward at 6 feet per second, as commanded.


Now look at what happens if you command the vehicle to go sideways to the right at 6 feet per second. To do this, you would have Xj=6, Yj=0.

Putting Xj=6 and Yj=0 into the inverse kinematic formulas would give:

W1 = 6

W2 = -3

W3 = -3

... which would cause the vehicle to go straight to the right at 6 feet per second, as commanded.


Suppose you wanted to command the vehicle to go 6 feet per second diagonally. You would set Xj=6/sqrt(2) and Yj =-6/sqrt(2), so that sqrt(Xj^2 + Yj^2) would be 6:


W1 = 6/sqrt(2) = 4.24

W2 = -(6/sqrt(2))/2 +0.866*(-6/sqrt(2)) = -5.8

W3 = -(6/sqrt(2))/2 -0.866*(-6/sqrt(2)) = 1.55

... which would cause the vehicle to go 6 feet per second diagonally


But what if you give joystick commands Xj=6, Yj=-6. You would be commanding the vehicle to go 6*sqrt(2) = 8.5 feet per second diagonally. What would happen?

Plugging in Xj=6 and Yj=-6:

W1 = 6

W2 = -6/2 +0.866*(-6) = -8.2

W3 = -6/2 -0.866*(-6) = 2.2

Notice that the maximum absolute value exceeds 6. Multiplying all 3 wheel speeds by 6/(8.2) gives:

W1 = 4.4

W2 = -6

W3 = 1.6

... and the normalization gives wheel speeds pretty close to the previous example

[WizenedEE]

#include <AFMotor.h>
#include <math.h>

AF_DCMotor motor1 (1, MOTOR12_1KHZ);
AF_DCMotor motor3 (3, MOTOR12_1KHZ);
AF_DCMotor motor4 (4, MOTOR12_1KHZ);

long cosOneTwenty = -500;
long sinOneTwenty = 866;
long sinTwoForty = -866;
long cosTwoForty = -500;

struct motorspeeds {long speed4; long speed3; long speed1;};
struct axis {int x; int y;};
struct moveRobotOutputs{
  struct motorspeeds speeds;
  struct axis axis;
};

int runMtr(AF_DCMotor mtr, long spd) { //given a motor name and a speed -1,000,000 to 1,000,000, runs the motor at the speed
  uint8_t drctn;
  if      (spd < -1000000){spd = -1000000;}
  else if (spd >  1000000){spd =  1000000;}
  if (spd < 0) {drctn = BACKWARD;}
  else {drctn = FORWARD;}
  int fspd = fabs(spd) * .00018F + 75; //fspd is between 0 and 255
  if (fspd <= 75) {fspd = 0;}
  mtr.setSpeed(fspd); //set the speed
  mtr.run(drctn); //run the motor
  return fspd;
}

struct moveRobotOutputs moveRobot(int x, int y, float gyro) { //taking x, y, (-1000 to 1000), and gyro (in milliradians, 0 to 360,000) values, runs the motors at speeds to move robot in correct direction
  struct motorspeeds speeds;
  struct axis axis;
  struct moveRobotOutputs Outputs;
  gyro /= 1000;
  float cosgyro = cos(gyro);
  float singyro = sin (gyro);
  long xp = x * cosgyro - y * singyro; // rotating vectors
  long yp = x * singyro + y * cosgyro; // with respect to gyro angle
  long speed4 = sinTwoForty * yp + cosTwoForty * xp;
  long speed3 = sinOneTwenty * yp + cosOneTwenty * xp;
  long speed1 = xp * 1000; //normally, it would just be xp, but the trig values are all fractions of 1000
  axis.x = xp;
  axis.y = yp;
  
  long highSpeed = fabs(fmax(fmax(fabs(speed1),fabs(speed3)),fabs(speed4)));

  float speedDivisor = float(fmax(1000000, highSpeed))/1000000;
  
  speed4 /= speedDivisor;
//  speed4 *= 1000000;
  speed3 /= speedDivisor;
//  speed3 *= 1000000;
  speed1 /= speedDivisor;
//  speed1 *= 1000000;
  
  speeds.speed4 = runMtr(motor4, speed4);
  speeds.speed3 = runMtr(motor3, -speed3);
  speeds.speed1 = runMtr(motor1, speed1);
//  speeds.speed4 = speed4;
//  speeds.speed3 = speed3;
//  speeds.speed1 = speed1;
  Outputs.speeds = speeds;
  Outputs.axis = axis;
  return Outputs;
}

This was written for Arduino, using a motor shield (the one from adafruit if you're into that). The interesting part is probably the function "moveRobotOutputs" which takes values -1000 to 1000 for joysticks and 0 to 360,000 for gyroscope, and spits out a value from -1,000,000 to 1,000,000 for each motor. I tried to make it efficient to run without much floating point. It's written in some sort of pseudo C.