#include <Wire.h> int loop_cnt=0; byte accx,accy,zbut,cbut; byte joyx,joyy; int angle, vel; #include <Servo.

h> /* A properly calibrated continuous rotation servo will be stationary at 90 degr ees, * and will move faster the farther away it is from 90 in either direction. E.g. , setting * it to 100 will move at the same speed as setting it to 80, but in opposite di rections. * The SPEED macro converts this to the more intuitive 0, +10, -10. */ #define SPEED(x) (90 + (x))// Given three speed components, this sets all three motors to that speed #define DRIVE(x,y,z) servo1.write(SPEED(x)); servo2.write(SPEED(y)); servo3.write(SPEED(z)) // Multiplier for the maximum speed. This is in arbitrary units, determined by guessing and checking. // Your motors may differ. #define MAXSPEED 20 Servo servo1; Servo servo2; Servo servo3; void setup() { // Serial.begin(19200); nunchuck_setpowerpins(); nunchuck_init(); servo1.attach(9); servo2.attach(10); servo3.attach(8); DRIVE(0,0,0);

delay(2000); // Serial.print("Debug on\n"); } void loop() { if( loop_cnt > 100 ) { // every 100 msecs get new data loop_cnt = 0; joyx =//ps2 joystick x coordinate values.. the ps2 functions will return the val ue here a range of 0-255 joyy =//ps2 joystick y coordinate values // Calculate the angle from the x and y component of joystick position

angle = (int) ((180/3.1416) * atan2((double) (joyy-128), (double) (joyx-128))); //read about the atan2 function and the hypot function and the values they retur n from cplusplus.org // Calculate the magnitude of the joystick position vector to set the speed vel = (int) (hypot((double) joyx-128, (double) joyy-128)*MAXSPEED*2/12 8); } setDrive(angle, vel); } loop_cnt++; delay(1); } void setDrive(int angle, int maxspeed){ // Computer the x and y components of the drive vector float x = cos(3.14159 * (float) angle / 180); float y = sin(3.14159 * (float) angle / 180); // Set the motors according to this vector DRIVE((int)(x*maxspeed),(int)((-0.5*x + 0.866*y)*maxspeed),(int)((-0.5*x-0.866 *y)*maxspeed)); } void setDrive(int angle){ setDrive(angle, MAXSPEED); } void setDrive(int angle, int maxspeed, int duration){ setDrive(angle, maxspeed); delay(duration); stopDrive(); }

void stopDrive(){ // Set the speed to 0 for all motors DRIVE(0,0,0); } void stopDrive(int time){ stopDrive(); delay(time); }