/*This program is for controlling a kiwi-drive base.  The right joystick is point-and-shoot, the left joystick left and right rotates the robot.  This assumes that motor B is oriented so that the wheel will go forward with an increasing stick value.  If the motors are backwards, add this before the while loop:

SetInvertedMotor(1);
SetInvertedMotor(2);
SetInvertedMotor(3);

For any questions, comments, or problems, contact Aaron Osmer at ajosmer@gmail.com */

#include "Builtins.h"

//Trig value definitions
#define cos(30) 173/200
#define sin(30) 1/2
#define sin(150) 1/2
#define cos(150) -173/200

//Joystick definitions
#define JOYX GetOIAInput(1, 1)-127  //Right joystick x axis
#define JOYY GetOIAInput(1, 2)-127  //Right joystick y axis
#define ROT GetOIAInput(1, 4)-127   //Left joystick x axis

void main() {
	while(1) {
		/*Your other code*/
		SetPWM(1, cos(150)*JOYX - sin(30)JOYY - ROT - 127); //Motor A, port 1, inverted
		SetPWM(2, 127 + sin(30)*JOYY - cos(30)*JOYX + ROT); //Motor B, port 2
		SetPWM(3, 127 + JOYX +ROT);			    //Motor C, port 3
	}
}