help with drive code

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

can anyone please explain to me what this code means


task
DriveSystem()
{
    int drive_l_front;
    int drive_l_back;
    int drive_r_front;
    int drive_r_back;
    
    int forward, right, clockwise;
    int temp;
    float theta;
    
    while(true)
        {
        // Get joystick values 
        // deadband near center of joysticks
        forward = vexRT Ch3 ];
        if( abs( forward ) < 10 )
            forward = 0;
        
        right = vexRT Ch4 ];
        if( abs( right ) < 10 )
            right = 0;
        
        clockwise = vexRT Ch1 ];
        if( abs( clockwise ) < 10 )
            clockwise = 0;

        // Get gyro angle in radians
        theta = degreesToRadians(gyro_angle);
        
        // rotate coordinate system - gyro positive angle is CCW
        temp  = forward * cos(theta) - right * sin(theta);
        right = forward * sin(theta) + right * cos(theta);
        forward = temp;

        // Set drive
        drive_l_front = forward + clockwise + right;
        drive_l_back  = forward + clockwise - right;
        drive_r_back  = forward - clockwise + right;
        drive_r_front = forward - clockwise - right;

        // normalize drive so max is 127 if any drive is over 127
        int max = abs(drive_l_front);
        if (abs(drive_l_back)  > max)
            max = abs(drive_l_back);
        if (abs(drive_r_back)  > max)
            max = abs(drive_r_back);
        if (abs(drive_r_front) > max)
            max = abs(drive_r_front);
            
        if (max>127) {
            drive_l_front = 127 * drive_l_front / max;
            drive_l_back  = 127 * drive_l_back  / max;
            drive_r_back  = 127 * drive_r_back  / max;
            drive_r_front = 127 * drive_r_front / max;
            }
        
        // Send to motors
        // left drive
        SetMotor( MotorLF, drive_l_front);
        SetMotor( MotorLB, drive_l_back);

        // right drive
        SetMotor( MotorRF, drive_r_front);
        SetMotor( MotorRB, drive_r_back);

        wait1Msec( 25 );
        }
}

I know what it is supposed to do but I’m confused by the syntax and what it actually means.
please bear with me I’m new to robotc
If I could have line by line explanation that would be great
thx

#2

so what this is, if I am not mistaken is code for a mechanum drive to allow strafing
he is setting variables to joystick values, this is for an arcade style control layout.
the variable “forward” is the movement to the front or back of the robot, “right” is staffing right or left (a negative right value would make you move left) and the variable “clockwise” is just general rotation.

the code also sets deadbands, what this does, is it cancels out any small joystick movements so that the robot doesn’t move if the joysticks are just slightly off center. as for the bit about the gyro I’ll have to get back to you

#3

my best guess about the gyro code is that it is used to keep the robot pointed in the right direction, but I am not really sure

#4

It look likes code based on (copied from ??) one of my old examples. Yes, this is part of what we call “field centric” drive code, forward is always away from you no matter what the orientation of the robot is. The code as you have posted it is incomplete, do you need the full version?

Need Help with Gyroscope
#5

Sorry it took so long. I just came back from driving school.
Yes I do need the full version possibly with an explanation:D.
Thanks

#6

It’s posted somewhere on the forum already but I will find the original and repost later tonight.

#7

okay thank you

#8

Looks like that code came from this old post.
omni directional motion

That’s a 3 year old post, not sure what I was exactly working on back then.

Learn about holonomic drives by reading the explanation by Cody.
Holonomic drives 2.0: A video tutorial by Cody

The idea behind the field centric control is to then modify the “forward” and “sideways” control values based on which way a gyroscope tells us the robot is pointing. The “front” of the robot always points away from you, we use simple trig equations to do the modifications.

Here is some UNTESTED (I’m tired tonight) code that has all of the necessary functions, I will check it at the weekend if I have time.

#pragma config(Sensor, in1,    gyro,           sensorGyro)
#pragma config(Motor,  port2,           MotorLF,       tmotorVex393_MC29, openLoop, encoderPort, None)
#pragma config(Motor,  port3,           MotorLB,       tmotorVex393_MC29, openLoop, encoderPort, None)
#pragma config(Motor,  port8,           MotorRB,       tmotorVex393_MC29, openLoop, reversed, encoderPort, None)
#pragma config(Motor,  port9,           MotorRF,       tmotorVex393_MC29, openLoop, reversed, encoderPort, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*-----------------------------------------------------------------------------*/
/*    Field centric control demo code                                          */
/*    James Pearman 2015                                                       */
/*                                                                             */
/*    Licensed under the Apache License, Version 2.0 (the "License");          */
/*    you may not use this file except in compliance with the License.         */
/*    You may obtain a copy of the License at                                  */
/*                                                                             */
/*      http://www.apache.org/licenses/LICENSE-2.0                             */
/*                                                                             */
/*    Unless required by applicable law or agreed to in writing, software      */
/*    distributed under the License is distributed on an "AS IS" BASIS,        */
/*    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */
/*    See the License for the specific language governing permissions and      */
/*    limitations under the License.                                           */
/*                                                                             */
/*    The author can be contacted on the vex forums as jpearman                */
/*    or electronic mail at jbpearman_at_mac_dot_com                           */
/*    Mentor for team 8888 RoboLancers, Pasadena CA.                           */
/*-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------*/
/** @brief      Get the gyro angle                                             */
/** @returns    gyro angle in 0 - 360 deg range                                */
/*-----------------------------------------------------------------------------*/

float
GyroGetAngle()
{
    return( SensorValue gyro ] / 10.0 );
}

/*-----------------------------------------------------------------------------*/
/** @brief      control the mecanum drive                                      */
/** @param[in]  forward The forward power/speed (-127 to 127)                  */
/** @param[in]  turn The turning power/speed (-127 to 127)                     */
/** @param[in]  right The strafing power/speed (-127 to 127)                   */
/*-----------------------------------------------------------------------------*/

void
DriveSystemMecanumDrive( int forward, int turn, int right )
{
    long drive_l_front;
    long drive_l_back;
    long drive_r_front;
    long drive_r_back;

    // Set drive
    drive_l_front = forward + turn + right;
    drive_l_back  = forward + turn - right;

    drive_r_front = forward - turn - right;
    drive_r_back  = forward - turn + right;

    // normalize drive so max is 127 if any drive is over 127
    int max = abs(drive_l_front);
    if (abs(drive_l_back)  > max)
        max = abs(drive_l_back);
    if (abs(drive_r_back)  > max)
        max = abs(drive_r_back);
    if (abs(drive_r_front) > max)
        max = abs(drive_r_front);
    if (max>127) {
        drive_l_front = 127 * drive_l_front / max;
        drive_l_back  = 127 * drive_l_back  / max;
        drive_r_back  = 127 * drive_r_back  / max;
        drive_r_front = 127 * drive_r_front / max;
    }

    // Send to motors
    // left drive
    motor MotorLF ] = drive_l_front;
    motor MotorLB ] = drive_l_back;
    
    // right drive
    motor MotorRF ] = drive_r_front;
    motor MotorRB ] = drive_r_back;    
}

/*-----------------------------------------------------------------------------*/
/** @brief      Driver control of mecanum drive system                         */
/*-----------------------------------------------------------------------------*/

void
DriveSystemMecanum()
{
    int forward, right, turn;
    int temp;
    float theta;

    // Get joystick values
    // deadband near center of joysticks
    forward = vexRT Ch3 ];
    if( abs( forward ) < 10 )
        forward = 0;

    right = vexRT Ch4 ];
    if( abs( right ) < 10 )
        right = 0;

    turn = vexRT Ch1 ];
    if( abs( turn ) < 10 )
        turn = 0;

    // Field centric control
    // Get gyro angle in radians
    theta = degreesToRadians( GyroGetAngle() );

    // rotate coordinate system
    temp  = forward * cos(theta) - right * sin(theta);
    right = forward * sin(theta) + right * cos(theta);
    forward = temp;

    // Send to drive
    DriveSystemMecanumDrive( forward, turn, right );
}

task main()
{
    // Init the gyro
    SensorValue gyro ] = sensorNone;
    wait1Msec(100);
    SensorValue gyro ] = sensorGyro;
    wait1Msec(1000);
    
    while(1)
        {
        DriveSystemMecanum();   

        wait1Msec(25);
        }
}
Field Centirc Meca Wheel Code
#9

thanks it looks great

#10

Could someone please test this code on their mecanum or x drive
I wont have my robot with me until about a week and a half.

#11

This will work. The syntax for the driver control code is correct, and as long as syntax/math is correct, it’ll work. Also, JPearman wrote it, which generally means it’ll work, since he tests everything alot.

#12

I tested it this morning, it works fine. You may need to change the motor setup to match your design (ie. which motors are right and left and their direction based on the gearing you use). Gyro’s are also notorious for drifting so the “forward” direction may change over time.

#13

thanks a lot. I mean it really. U guys are really helpful. Thanks

#14