Test of Kiwi Drive

kiwiTest.c

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*
 * Simple C program to give test parameters for Kiwi Drive robot: x, y, p
 * Two joysticks required:
 *   one provides vector of robot direction ('x' + 'y' components),
 *   the other provides pivot component (one axis only, called 'p')
 * Values are input on command line as space separated values: x y p
 * output shows speeds.
 *
 * It appears to work for our example robot with simple values like
 * straight ahead and stafing sideways.
 *
 * $ ./tmp 0 1 0 # Forwards
 *  x: 0.00,  y: 1.00,  p: 0.00
 * s1: 0.00, s2: 0.58, s3: -0.58
 *
 * $ ./tmp 0 -1 0 # Backwards
 *  x: 0.00,  y: -1.00,  p: 0.00
 * s1: 0.00, s2: -0.58, s3: 0.58
 *
 * $ ./tmp 0 0 1 # Pivot CW
 *  x: 0.00,  y: 0.00,  p: 1.00
 * s1: 0.33, s2: 0.33, s3: 0.33
 *
 *  $ ./tmp 0 0 -1 # Pivot CCW
 *  x: 0.00,  y: 0.00,  p: -1.00
 * s1: -0.33, s2: -0.33, s3: -0.33
 *
 * $ ./tmp 0 1 1 # Fwd + Pivot CW
 *  x: 0.00,  y: 1.00,  p: 1.00
 * s1: 0.33, s2: 0.91, s3: -0.24
 */

int main(int argc, char** argv)
{
  double x = atof(argv[1]);
  double y = atof(argv[2]);
  double p = atof(argv[3]);

  double s1, s2, s3;

  double inversion[3][3] = {
    { ( 2.0/3.0),  0        , (1.0/3.0) },
    { (-1.0/3.0),  1/sqrt(3), (1.0/3.0) },
    { (-1.0/3.0), -1/sqrt(3), (1.0/3.0)}
  };

  s1 = (x*inversion[0][0] + y*inversion[0][1] + p*inversion[0][2]);
  s2 = (x*inversion[1][0] + y*inversion[1][1] + p*inversion[1][2]);
  s3 = (x*inversion[2][0] + y*inversion[2][1] + p*inversion[2][2]);

  printf(" x: %.2f,  y: %.2f,  p: %.2f\n",  x,  y,  p);
  printf("s1: %.2f, s2: %.2f, s3: %.2f\n", s1, s2, s3);

  return 0;
}