PPRK Motion Control

Related
Products

PPRK BrainStem Easy in Black

PPRK BrainStem Easy Kit

Contents

Introduction
PPRK Motion
Servo Speed
Vector Drive
Source Code
Going Further

Introduction

The Palm Pilot Robot Kit (or PPRK) is a robotics platform developed by Carnegie Mellon University for use with palmtop computers. The robot uses a unique 3-wheel holonomic drive system. Source code for the libraries that perform motion control and sensor processing are available at the CMU PPRK Web Site (see the robot.cpp file in the Software section). These libraries use vector operations and floating point math and require the power of a palmtop computer. An external servo and sensor interface module is also required to control a PPRK. With a little bit of work, some of the control routines can be adapted to run in a small programmable controller that uses integer math. One such device is the BrainStem GP 1.0 module. With its built-in servo drivers and analog inputs, it can act as the sole control device for a PPRK.

Contents

PPRK Motion

The holonomic drive system enables a robot like the PPRK to move in any direction without having to change the orientation of its body. It can also rotate in place or rotate as it moves. This type of motion is made possible by omni-directional wheels. These wheels can drive in one direction while also rolling laterally.

A derivation of motion control equations for the PPRK may be found at the CMU PPRK Web Site (under the Physics section). These equations define a motion as a vector and a rotational velocity. The vector gives the direction and speed at which the body will travel. The rotational velocity determines how fast and in which direction the robot will turn as it is traveling. The control equations take those inputs and calculate motor speeds that will perform the desired motion.

Contents

Servo Speed

The PPRK uses modified servos for locomotion. The servos rotate at different speeds based on the positioning signals they receive. Unfortunately, the speed at which a modified servo rotates does not map linearly to its input. This makes it difficult to implement the motion control equations. The CMU group performed some experiments to determine the relationship between the speed of a servo and the servo input. They came up with a cubic regression formula to "linearize" a servo. Details can be found at the CMU PPRK Web Site (see the Technical Info section). Implementing their formula is not possible on the BrainStem since its programming language TEA does not support floating point math. However, it is possible to get a good approximation by doing some scaling and rounding off the floating point values to integers. This is what the "vel_to_pos" function does in the example code listed below.

Contents

Vector Drive

The motion control equations require calculating a vector dot product and adding a rotational velocity term to get the individual wheel velocities that produce a desired motion. The CMU code has a subroutine called "Vector_Drive" that performs this task. This routine use a C++ vector class and floating point math in its motion control calculations. This is not possible with TEA, but it is possible to use scaled integers as vector components to approximate the vector math. This is what the "vector_drive" function does in the example code listed below.

Because of all the scaling involved to fake floating point math with integer math, the units become very unwieldy. In the example code below, it is easier to think of the inputs to the vector drive function as relative amounts. For example, a vector (vx,vy) of (0,100) and "omega" of 0 will drive the robot "north" at its maximum speed. A vector (vx,vy) of (0,0) and "omega" of 100 will make the robot spin counterclockwise at its maximum speed. The integer math implementation of the motion control is not perfect by any means, but it is good enough for making the robot roll around in a controlled manner. However, at slow speeds, the motion control can be rather unpredictable.

Contents

Source Code

The example program contains the motion control routines discussed above, a "gyrate" routine that demonstrates how the robot can move in any direction, and a wall-hug routine that combines motion control with some sensor feedback.

/* filename: bspprk.tea */ /* example code for driving a PPRK using only a BrainStem */ #include <aCore.tea> #include <aServo.tea> #include <aA2D.tea> #define GDELAY 2500 /************************************************************* A BrainStem-controlled PPRK Start with the BrainStem's default servo settings. Configure servos so that absolute positions of 128 will stop each servo and absolute positions of 255 will make the robot spin counterclockwise. Use GP.EXE to configure the servos. You may need to tweak the sliders a bit and set all the reverse bits. When viewed from the top, the servo and sensor assignments are: Servo Assignment 0 C 1 2 Sensor Assignment 2 1 C 0 (C is the location of the controller viewed right-side up.) Use fresh NiMH batteries. Once they start getting low, the controller may begin to experience spontaneous resets and possibly other malfunctions. Another option is to use a separate supply for the logic power. *************************************************************/ /* * This function is derived from the the * int Robot::Vel_To_Value(double vel) function * in the robot.cpp file found in the software section * of the http://www-2.cs.cmu.edu/~pprk website. * It approximates the floating point math with integers. * The input must be scaled by 100 to get numbers that * are similar to the output of the Vel_To_Value function. * * This function converts a "velocity" into an absolute * servo position which roughly corresponds to that * velocity. (Servos are not very linear.) * */ int vel_to_srvpos(int w) { int p; p=((40*w)/100)-13; p=((p*w)/100)+22; p=((p*w)/100)+128; return p; } /* * This function is derived from the the * void Robot::Vector_Drive(vector V, double omega) routine * in the robot.cpp file found in the software section * of the http://www-2.cs.cmu.edu/~pprk website. * It approximates the floating point math with * scaled integers. * * Instead of a vector, the routine requires vector * components vx and vy. This routine attempts to drive * the robot in the direction (vx,vy) with a angular * rotation rate of omega. * * Because the integer math is a total hack, the input * units should only be considered relative amounts. * * If vx=0 and vy=0, and omega=100 the robot will rotate * counterclockwise at roughly its maximum speed. If vx=0 and * vy=100 and omega=0, the robot will go "north" at roughly * its maximum speed. The individual wheel velocities are * limited at absolute values of 110 otherwise the math * will overflow. * */ void vector_drive(int vx, int vy, int omega) { int omega1; int omega2; int omega3; int bb; int bx; int by; int s1; int s2; int s3; bb=90*omega; bx=50*vx; by=86*vy; omega1 = ((-100*vx) + bb) /100; omega2 = ((bx - by) + bb) /100; omega3 = ((bx + by) + bb) /100; if (omega1 > 110) omega1 = 110; if (omega1 < -110) omega1 = -110; if (omega2 > 110) omega2 = 110; if (omega2 < -110) omega2 = -110; if (omega3 > 110) omega3 = 110; if (omega3 < -110) omega3 = -110; s1=vel_to_srvpos(omega1); s2=vel_to_srvpos(omega2); s3=vel_to_srvpos(omega3); aServo_SetAbsolute(0,(unsigned char)s1); aServo_SetAbsolute(1,(unsigned char)s2); aServo_SetAbsolute(2,(unsigned char)s3); } /* * This function shows how the robot can move * in any direction and still keep its orientation * nearly constant. The integer math introduces * some inaccuracy. * */ void gyrate() { vector_drive(0,100,0); aCore_Sleep(GDELAY); vector_drive(50,86,0); aCore_Sleep(GDELAY); vector_drive(86,50,0); aCore_Sleep(GDELAY); vector_drive(100,0,0); aCore_Sleep(GDELAY); vector_drive(86,-50,0); aCore_Sleep(GDELAY); vector_drive(50,-86,0); aCore_Sleep(GDELAY); vector_drive(0,-100,0); aCore_Sleep(GDELAY); vector_drive(-50,-86,0); aCore_Sleep(GDELAY); vector_drive(-86,-50,0); aCore_Sleep(GDELAY); vector_drive(-100,0,0); aCore_Sleep(GDELAY); vector_drive(-86,50,0); aCore_Sleep(GDELAY); vector_drive(-50,86,0); aCore_Sleep(GDELAY); } /* * This function implements one iteration of * "north" traveling right wall-hug. The robot tries * to go north, but rotates its direction based on * a reading from sensor 1. * * This is a proportional control loop. A gain factor * of 0.5 (divide by 2) is applied to the wall-hug error * measurement in an attempt to get a better hug. * */ void wall_hug_iter(int set) { int r; r=(aA2D_ReadInt(1)-set)/2; if (r< -60) r= -60; if (r> 60) r= 60; vector_drive(0,100,r); } /* * pause 2 seconds * do two gyrations * start endless right wall-hug * */ void main() { aCore_Sleep(20000); gyrate(); gyrate(); while (1) { wall_hug_iter(200); } }

Contents

Going Further

The motion control routines in the example code above may be adapted to other small controllers that use integer math. An alternate approach for small controllers is to make routines that perform a single motion with a fixed speed, such as "translate left", "rotate right", "go straight", etc. With this technique, it is possible to determine the servo inputs that correspond to the desired motions experimentally and hard-code them.

Revision History:

2004-12-03: Article Created.