I did a very quick implementation in ROBOTC of the code from the referenced paper in my previous post. I’m not going to post the whole program here as it contains several other “work in progress” libraries but the drive code ended up as follows.
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 );
}
}It’s not very useful out of context, there are other tasks running handling the gyro and motor control but it gives an idea of how the code would be. It actually works really well.