View Single Post
  #2   Spotlight this post!  
Unread 20-03-2007, 20:22
meatmanek meatmanek is offline
Programmer/physicist/mathematician
FRC #0868 (TechHounds)
Team Role: Programmer
 
Join Date: Mar 2004
Rookie Year: 2004
Location: Carmel, Indiana
Posts: 142
meatmanek is a splendid one to beholdmeatmanek is a splendid one to beholdmeatmanek is a splendid one to beholdmeatmanek is a splendid one to beholdmeatmanek is a splendid one to beholdmeatmanek is a splendid one to beholdmeatmanek is a splendid one to behold
Re: Mecanum drivetrain programming problems

Last year we used mecanum wheels.

I don't have access to our old code at the moment, but it looked something like this (values are centered at 0 rather than 127)

Code:
x = p1_x;
y = p1_y;
rotate = p1_wheel;

drive_FR = y + rotate + x;
drive_FL = y - rotate - x;
drive_BR = y + rotate - x;
drive_BL = y - rotate + x;

scale = 128;
if (drive_FR > scale)
 scale = drive_FR;
if (drive_FR < -scale)
 scale = -drive_FR;
if (drive_FL > scale)
 scale = drive_FL;
if (drive_FL < -scale)
 scale = -drive_FL;
// and so on for BR and BL

drive_FR = drive_FR * 128 / scale; 
// and so on for the rest
The first part determines which direction and speed each wheel should be going.
The second part scales all of the wheels back if one is above 127. We scaled back rather than clip because if you clip, it won't go in the direction you want.

EDIT:
I suppose I ought to include an explanation as well.
Forward and turning are identical to tank drive. In fact, we use code very similar to this on this year's tank drive robot for one joystick drive.
To go sideways, you drive each wheel on one side of the robot in a different direction. Because of the angled rollers, if you drive a wheel forward but pull backward on the axle, it will pull sideways. To get a combination of all of these, you simply add the values together.
__________________
Real programmers use vim.

Last edited by meatmanek : 20-03-2007 at 20:29. Reason: Included explanation.