I developed a simple kiwi drive (wheels mounted at 45 degree angles) code for a prototype; let’s see if I remember it correctly. I used code very similar to the default 1 joystick code for translational (no spinning) movement. You can put your angle adjusted p1_x and p1_y axis into the translational code. Obviously, the motors have to be linked diagonally.
LF/---\RF
....| |
LR\---/RR
LF - left front motor
LR - left rear motor
RF - right front motor
RR - right rear motor
Code:
RF = LR = Limit_Mix(2000 + p1_y + p1_x - 127);
LF = RR = Limit_Mix(2000 + p1_y - p1_x + 127);
Now for spin:
Still it is pretty simple and much like the default code. You need a spin joystick (p2_x). Now left and right sides are pretty much linked.
Code:
RF = Limit_Mix(2000 + RF + p2_x - 127);
RR = Limit_Mix(2000 + RR + p2_x - 127);
LF = Limit_Mix(2000 + LF - p2_x + 127);
LR = Limit_Mix(2000 + LR - p2_x + 127);
As I remember, this works pretty well. Now just keep track of your gyro. Watch out for drift over time, as the error compounds. I would put some filter caps in the gyro to reduce the error. Also make sure your motors are all running in the same direction. GOOD LUCK!