EthanT:
I separated the Gyro from the equations and it works perfectly. I’ll describe what was happening more precisely. Any point that is 0 or 180 from the starting position the commands work really well, but when I turn 90 or -90 it does not add both of the vectors together as 2 motors turn on and go diagonal, but its suppose to move straight away from me. A more descriptive version would be if Gyro < 90 then 2 motors activate,but if Gyro > 90 then the other two motors turn on and the previous motors turn off, but within this region all four motors should activate. The only way to get them to both turn on at the same time is to have the joystick perfectly straight with 3 and 4 channel. This happens with channel 3 and 4, but it does not happen at all when originated 0, 180 from starting position with channel 3 and 4. I have another question, with this new code setup. Is there a way to lock rotation as I drive to stabilize accuracy of X drive. I did it before by using > < commands that set the gyro to zero, but I cant use this because of the field oriented driving. If you don’t understand the behavior of the robot from my description. I can make a video of it.
More behaviors I noticed is that from the starting point, 0, if I turn 10 degrees or any degree close to zero. The motor speeds do not change until they reach 45 degrees, then instantly reverse. Is there a way to make this transition smoother. For instance, if I the robot is at 0 degrees and I move joystick forward, it moves away from me, but if I rotate it by ten degrees, it moves away from me diagonally to the right. Then once it passes 45 degrees. The motors reverse and it moves diagonally away from me to the left until it passes 90, then it switches diagonally towards me, then smooths out to normal driving again at 180.
The issue might be that SensorValue[Gyro] is returning as integer and when you divide it by another int to get to radians it gets truncated. Try to divide it by floating point number i.e. 522.0 to ensure floating point math.
Also, try to send power values to motors using true speed mapping for keeping best ratio between X and Y.
It is very easy to add to your program - one global array and a function with a couple of if() statements to check for sign and upper array bound. Just make sure you follow this workaround or latest version of RobotC may have issues uploading the code: