Quote:
Originally Posted by Ether
@caume: I'm curious to see how you would go about this. Please post your pseudocode with field orientation.
|
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:
Code:
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