Omni Vex code in easyc?

[xanarchyx]

So im a noob at codeing and im trying to do omni drive in easy c. I would be able to maby do it in mplab but i don't got access to that software. im wondering how to go about doing it in easy c?
any suggestions. o ps.....its v.5 not the shiney new version 2.0. theres not that big of a difference though.
any help would be greatly appreciated.

[Jon236]

You should look at this document:

http://www.chiefdelphi.com/media/papers/1836

[Foster]

When you say Omni drive, what are trying to say?

Are you thinking about a Mecanum drive? That will be hard to do, since there are not Vex Mecanum wheels (unless you fabricate your own)

Are you thinking about a Holononmic drive? 4 wheel 90 degree offset. That would be four omni wheels, one on each face of a square bot.

Are you thinking about a 3 wheel 120 degree offset or Kiwi drive?

This is a good drive description video

Thanks!
Foster

[xanarchyx]

yes....im thinking the 4 wheel 90* off set. Sorry for not clarifying

[Foster]

If you search for holonomic drive, you can find a number of posts about it.

Arthur Dutra wrote this code (and posted it) to support a holononmic drive. One joystick stick moves forward/back and side-side (and diagonally) for translation motion. The other joystic moves left/right to rotate, both can be moved at the same time.

The platform consists of four omni wheels mounted at 180 degrees from the next corner (ie two go "forward / backwards" two go "left / right"

Code is for the WPILibrary, so it should work fine with EasyC.

Let us know how you make out.

#include "API.h"

void main ( void )
{
int LF; // Left Front
int RF; // Right Front
int LR; // Left Rear
int RR; // Right Rear
int leftx;
int lefty;
int rightx;
int spin;
      while ( 1 )
      {
            // Get Data 
            leftx = GetRxInput ( 1 , 4 ) ; // Left Joystick, X Axis
            lefty = GetRxInput ( 1 , 3 ) ; // Left Joystick, Y Axis
            rightx = GetRxInput ( 1 , 1 ) ; // Right Joystick, X Axis
            // Half the input signal (so code does not overflow past 255)
            leftx = leftx / 2 ;
            lefty = lefty / 2 ;
            spin = rightx / 2 ;
            // Drive Code Algorithim
            LF = RR = lefty - leftx + 127 ;
            RF = LR = 255 - lefty - leftx ;
            RR = 255 - RR ; // Reverse Direction of RR motor
            LR = 255 - LR ; // Reverse Direction of LR motor
            // Spin Code Algorithim
            RF = RF - spin + 63 ;
            RR = RR - spin + 63 ;
            LF = LF - spin + 63 ;
            LR = LR - spin + 63 ;
            // Code overflow prevention
            if ( LF < 0 )
            {
                  LF = 0 ;
            }
            else if ( LF > 255 )
            {
                  LF = 255 ;
            }
            if ( RF < 0 )
            {
                  RF = 0 ;
            }
            else if ( RF > 255 )
            {
                  RF = 255 ;
            }
            if ( RR < 0 )
            {
                  RR = 0 ;
            }
            else if ( RR > 255 )
            {
                  RR = 255 ;
            }
            if ( LR < 0 )
            {
                  LR = 0 ;
            }
            else if ( LR > 255 )
            {
                  LR = 255 ;
            }
            // Set Motors
            SetMotor ( 1 , RF ) ;
            SetMotor ( 2 , LF ) ;
            SetMotor ( 3 , LR ) ;
            SetMotor ( 4 , RR ) ;
      }
}

[xanarchyx]

stupid question but how do i upload this into easy c....this looks like its for mp lab.

[Aaron S]

Just look at the code and match it in EasyC with blocks, you can't just copy and paste.

[Kingofl337]

With easyC Pro you can.

[xanarchyx]

ok ill try and match it...but i got pro, i haven't been able to figure out how to manually code....like i know you can....im just missing it.

[Kingofl337]

Here is the easiest way to do what you wish.
1.) Right click on User Functions -> Add New -> Name it omni and click ok
2.) Click on the project tab.
3.) Right Click on the function name -> Convert to 'C' Code -> Yes
4.) After you function will look like this....

#include "Main.h"

void omni ( void )
{
<---Paste the code in here
}

5.) Paste the code provided between the braces starting at the variables and ending with the last brace.
6.) Rename SetMotor to SetPWM
7.) Goto the UserInclude.h File and under where it says "Add User Code Here"
Add a line : void omni (void );


This should work, I tested it out.

[daltore]

Why is it necessary to cap the values at the maximums and minimums? Does the processor not automatically do this? I've always found it easier to subtract values from full drive and having the motors that need to be full power just have values greater than 255 and subtracting values from the other wheels instead of just starting from half power for everything. That way, you get full speed, but everything still works fine. I've never done a holonomic function, but this usually works fine for homemade arcade drives and the similar.

Also, please correct me when I say the obvious thing I'm missing, but the algorithm doesn't quite look right:

            LF = RR = lefty - leftx + 127 ;
            RF = LR = 255 - lefty - leftx ;

The first one would seem to stop the front left and rear right motors if the joystick goes to the upper right or bottom left, which would defeat the purpose of being able to translate diagonally, wouldn't it? As for the second one, it looks like it would barely have any power even when the joystick was straight forward, and again stopped when going to the upper left or bottom right. I know I'm probably missing something here, but I can't seem to get it. Is this what the adding 63 is supposed to solve? I figured that was to offset the below 127 motor values for spinning.

I'm designing a robot to drive this way for practicing during Elevation, and I've not yet had time to start (notice the 4 weeks between BEST competition and VEX).

Any clue what I'm missing?

[artdutra04]

Originally Posted by daltore

Why is it necessary to cap the values at the maximums and minimums? Does the processor not automatically do this? I've always found it easier to subtract values from full drive and having the motors that need to be full power just have values greater than 255 and subtracting values from the other wheels instead of just starting from half power for everything. That way, you get full speed, but everything still works fine. I've never done a holonomic function, but this usually works fine for homemade arcade drives and the similar.

It's much better to code defensively, as opposed to rely on other things to fix your mistakes. I could have used unsigned char for the calculations to save memory, but then any overflow values would wrap back around and reverse motor direction. So I used int variables for the calculations and then manually check to make sure they are all within bounds. It's a safety measure.

Originally Posted by daltore

Also, please correct me when I say the obvious thing I'm missing, but the algorithm doesn't quite look right:

            LF = RR = lefty - leftx + 127 ;
            RF = LR = 255 - lefty - leftx ;

The first one would seem to stop the front left and rear right motors if the joystick goes to the upper right or bottom left, which would defeat the purpose of being able to translate diagonally, wouldn't it? As for the second one, it looks like it would barely have any power even when the joystick was straight forward, and again stopped when going to the upper left or bottom right. I know I'm probably missing something here, but I can't seem to get it. Is this what the adding 63 is supposed to solve? I figured that was to offset the below 127 motor values for spinning.

I'm designing a robot to drive this way for practicing during Elevation, and I've not yet had time to start (notice the 4 weeks between BEST competition and VEX).

Any clue what I'm missing?

I derived those equations from taking the eight directions (N, NE, E, SE, E, SW, W, NW) and writing down the necessary motor values needed to make them work. From there, I worked backwards to get the algorithm above. You can test this by plugging in values that you would get from a transmitter for the above scenarios, and then testing calculate the output signal at the motors.

For example, the robot is moving right at "full speed" while spinning slightly:
255 = GetRxInput ( 1 , 4 ) ; // Left Joystick, X Axis
127 = GetRxInput ( 1 , 3 ) ; // Left Joystick, Y Axis
180 = GetRxInput ( 1 , 1 ) ; // Right Joystick, X Axis

// Half the input signal (so code does not overflow past 255)
128 = leftx / 2 ;
64 = lefty / 2 ;
90 = rightx / 2 ;

// Drive Code Algorithim (note how "full speed" only sends half speed values to the motors to avoid saturation).
63 = 63 = 64 - 128 + 127 ;
63 = 63 = 255 - 64 - 128 ;
192 = 255 - RR ; // Reverse Direction of RR motor
192 = 255 - LR ; // Reverse Direction of LR motor

// Spin Code Algorithim (Since we were only at half speed values before, the spin values have room to work with)
36 = 63 - 90 + 63 ; // RF
165 = 192 - 90 + 63 ; // RR
36 = 63 - 90 + 63 ; // LF
165 = 192 - 90 + 63 ; // LR

Now is this algorithm perfect? No. In the N, E, S, and W directions the robot never travels at maximum speed; only when the robot goes diagonally do the motors reach their limits of zero and 255. You can overcompensate this by saturating the equation by not diving the leftx or lefty variables in half, but this will take away maneuverability. You can experiment with this by tweaking the constants to see how the system responds.

A video of this code working is available here: http://www.team228.org/media/video/view/9

Another thing to keep in mind is that this is entirely open-loop; there are no sensors for feedback. Forward is always relative to the front of the robot. By adding a gyro to the robot, getting the angle of the joystick ( angle = atan(lefty/leftx); // I'm pretty sure math.h does everything in radians. You can also write a lookup table to save the processor from imploding. ), getting the angle of the robot (you'll also need to integrate results to keep track of robot angle over time), then you can then use slightly different algoritms to make sure the robot always goes in the direction of the joystick - no matter what way the robot is pointing.

But that's a good exercise that one should experiment with and learn on their own. I posted the my basic holonomic drive code just so people can get past that initial hump of figuring out how to make it work (and judging by the number of teams I see with omni/mecanum drive that never strafe, this hump can be high).

[grim]

Originally Posted by Kingofl337

Here is the easiest way to do what you wish.
1.) Right click on User Functions -> Add New -> Name it omni and click ok
2.) Click on the project tab.
3.) Right Click on the function name -> Convert to 'C' Code -> Yes
4.) After you function will look like this....

#include "Main.h"

void omni ( void )
{
<---Paste the code in here
}

5.) Paste the code provided between the braces starting at the variables and ending with the last brace.
6.) Rename SetMotor to SetPWM
7.) Goto the UserInclude.h File and under where it says "Add User Code Here"
Add a line : void omni (void );


This should work, I tested it out.

I'm not seeing the the Convert to 'C' code option on the right click menu. Is this an EasyC Pro function? We have a trial version and it doesn't appear.

[daltore]

For those of you interested, I've posted a scaled holonomic drive library on VEX Forums here. The features of this library:

-squarejoy() - A function which will perform squaring on a PWM value (still has a range of about 0-255, one or two points less).
-charabs() - Perform an absolute value function with 127 as center instead of 0.
-holodrive() - Function for holonomic driving that can be used portably.
-Scaled - you can always reach full value of the motor speeds, but scaling maintains maneuverability. When a motor goes out of range, it scales all of the motors down to the wheel that is most out of range.
-Documented code! - I've tried to comment everything as much as possible so you can tell what's going on. Not to mention the description on the VEX Forum page.


Anyone is free to use this code, and ask me questions about it. As for use with EasyC, I'm not quite sure about using actual code in !EasyCPro. All of this was done with WPILib under MPLAB. But maybe someone here could transform the header and .c file I loaded into EasyC project files? I'm not sure if you can just include source code files in an EasyC project, but if you can, that should work too.