[Ether]

Originally Posted by caume

With this model, the battery is the back of the drive, the point opposite it is the front. flMotor would be the motor on the left side, closest the point opposite the battery.

Psuedocode:

if (fieldOriented) {
	cosA = cos(( target - gyroAngle ) * 3.14159 / 180 )
	sinA = sin(( target - gyroAngle ) * 3.14159 / 180 )

	x = (driverLX * cosA) - (driverLY * sinA)
	y = (driverLX * sinA) + (driverLY * cosA)
}


flMotor =   y*sind(60) + x*cosd(60) + turn
mlMotor =   y + turn
blMotor =   y*sind(60) - x*cosd(60) + turn 
frMotor = - y*sind(60) - x*cosd(60) + turn
mrMotor = - y + turn
brMotor = - y/sind(60) + x/cosd(60) + turn

If I assume the following:

driverLX is the driver's field-centric move right command
driverLY is the driver's field-centric move downfield command
turn is the driver's rotate clockwise command
target is an angle offset which defines the "front" of the robot relative to the point opposite the battery

... then your code gives incorrect answers for the wheel speeds. See example with target=0 (front is defined as the point opposite the battery as stated above) and gyroAngle=0 (robot front is facing straight downfield).

Originally Posted by caume

this guy did it just how I did.

Apparently not. The code I posted gives the correct wheel speeds for robot-centric driver FWD and STR commands.

To make that code give the correct wheel speeds for field-centric driver FWD and STR commands, just modify those commands as follows before calculating the wheel speeds:

temp =  FWD·cos(θ) + STR·sin(θ);
STR  = -FWD·sin(θ) + STR·cos(θ);
FWD  =  temp;

... where θ is the gyro angle, measured clockwise from straight downfield.