[BigJ]

Originally Posted by AmoryG

One reason why I made this thread was also show off their actual programming for controlling their bots. I'm very interested in what other programmers decided to do. I've always liked unique ways of doing things, not for the sake of being different, but being inovative. Inovation means something new and because I'm new to programming new is all I know. If anyone is willing to explain or share their code so I can study it and learn from what other people do, that would be great.

Team 1675 developed their own Mecanum algorithm this year, based off an article explaining a drive system for 3+ wheel Omni systems. The main premise is that it calculates all angles using "b-rads", a measure of an angle using 8-bit numbers instead of degrees or radians.

It has worked pretty well for us, barring some mechanical problems. Here is some example code because Our forum is going through overhaul and I REALLY don't like the code blocks there. too hard to read.

/*!
* \file omniTest.c
* \author Jon C. Anderson <jon.c.anderson@gmail.com>
*
* \note This code is for educational use only.
*
* Controlling an holonomic drive is not very difficult. The force that can be
* exerted by omni-wheels correlate directly with a sine function that is phase
* shifted based on the desired heading. The angular velocity of the wheels is
* basically the magnitude of resulting sine function at the angle of the wheel
* to the "front" of the holonomic drive scaled by the desired fraction of the
* maximum speed.
*
* This file shows how to control an omni-directional robot with 4 wheels
* evenly spaced 90 degrees from each other. This also approximates a robot
* that uses Mecanum wheels with a pitch of 45 degrees in the diamond setup.
*
* This code is based on "Holonomic Drive Platforms: How to Drive a Robot That
* Has No Front" by Jack Buffington published in SERVO Magazine for April 2005
*/

#define WORD unsigned short int
#define BYTE unsigned char
#define INT16 short int
#define INT8 char

#include <stdio.h> /* for printf */

BYTE calcMotorVelocity( BYTE Speed, BYTE Heading, BYTE MotorAngle, INT8 Rotation );

BYTE mySin( BYTE Angle );

const BYTE sinTable[] = {
128, 131, 134, 137, 140, 143, 146, 149, 153, 156, 159, 162, 165, 168, 171,
174, 177, 180, 182, 185, 188, 191, 194, 196, 199, 201, 204, 207, 209, 211,
214, 216, 218, 220, 223, 225, 227, 229, 231, 232, 234, 236, 238, 239, 241,
242, 243, 245, 246, 247, 248, 249, 250, 251, 252, 253, 253, 254, 254, 255,
255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 254, 253, 253, 252, 251,
251, 250, 249, 248, 247, 245, 244, 243, 241, 240, 238, 237, 235, 233, 232,
230, 228, 226, 224, 222, 219, 217, 215, 213, 210, 208, 205, 203, 200, 198,
195, 192, 189, 187, 184, 181, 178, 175, 172, 169, 166, 163, 160, 157, 154,
151, 148, 145, 142, 139, 135, 132, 129, 126, 123, 120, 116, 113, 110, 107,
104, 101, 98, 95, 92, 89, 86, 83, 80, 77, 74, 71, 68, 66, 63, 60, 57, 55,
52, 50, 47, 45, 42, 40, 38, 36, 33, 31, 29, 27, 25, 23, 22, 20, 18, 17, 15,
14, 12, 11, 10, 8, 7, 6, 5, 4, 4, 3, 2, 2, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 1, 2, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12, 13, 14, 16, 17, 19, 21, 23, 24,
26, 28, 30, 32, 35, 37, 39, 41, 44, 46, 48, 51, 54, 56, 59, 61, 64, 67, 70,
73, 75, 78, 81, 84, 87, 90, 93, 96, 99, 102, 106, 109, 112, 115, 118, 121,
124, 128 };

int main( void )
{
   /* This "Omni" bot has 4 wheels located 90 degrees from each other with
    * forward being at North
    */
   
   /*    PWM      Position  Position    Motor    *
    *   Output    (Compass) (b-rads) Orientation *
    *   ------    --------- -------- ----------- */
   BYTE pwm01; /*    NW        224     reverse   */
   BYTE pwm02; /*    NE         31     forward   */
   BYTE pwm03; /*    SW        159     reverse   */
   BYTE pwm04; /*    SE         96     forward   */
   
   BYTE mySpeed   = 254; /* Velocity scalar (0->stop, 255->full speed) */
   BYTE myHeading =   0;
   BYTE myTwist   =   0;
   
   pwm01 = calcMotorVelocity( mySpeed, myHeading, 224, myTwist );
   pwm02 = calcMotorVelocity( mySpeed, myHeading,  31, myTwist );
   pwm03 = calcMotorVelocity( mySpeed, myHeading, 159, myTwist );
   pwm04 = calcMotorVelocity( mySpeed, myHeading,  96, myTwist );
   
   printf( "m1:%i\tm2:%i\r\n\r\nm3:%i\tm4:%i\r\n", pwm01, pwm02, pwm03, pwm04 );
   
   return( 0 );
}

BYTE calcMotorVelocity( BYTE Speed, BYTE Heading, BYTE MotorAngle, INT8 Rotation )
{
   INT16 MotorOutput;
   
   Heading = MotorAngle + Heading;
      /* If your Mecanum drive is in the X configuration phase shift your heading
       * by 63 brads */
   
   /* MotorOutput = V * (scaler/divisor) + (zeroOffset - scaler)/2 *
    * MotorOutput = V * (Speed/256) + (256-Speed)/2                */
   MotorOutput = (((WORD)mySin( Heading ) * (WORD)Speed) >> 8) + ((256 - Speed) >> 1);
   
   MotorOutput += Rotation;
   
   /* Make sure that rotation has not caused an overflow */
   if( MotorOutput < 0 )
   {
      MotorOutput = 0;
   }
   else if( MotorOutput > 255 )
   {
      MotorOutput = 255;
   }
   
   return( (BYTE)MotorOutput );
}

BYTE mySin( BYTE Angle )
{
   /* This sin function is not very accurate, but should be good enough */
   return( sinTable[ Angle ] );
}

Our actual files all have a little more stuff in them, and we worked at making our code object-oriented this year, but nothing very very different from this.


You can check it out more here:

http://team1675.org/forum/viewtopic....start=15#p1352

(Out website is in overhaul so it may be a little funky.) Feel free to ask me any questions!