Using the two hacked servos from the other day I decided to make them into a simple mobile platform. In addition I have a
from www.solarbotics.com. I am using the joystick to control the rotation of the two servos. The buttons and
data however are not used for anything so far with this experiment. I used some code that I found on the web that used the
via the I2C connection and then send it to a terminal program using a serial interface. I then modified it to give abbreviated data data to the serial terminal, and also to drive the motors using
digital I/O pins.
So.... here is the code.... and yes... I know I need better comments....
/*
example code from www.windmeadow.com/node/42 modified to drive two continuous
rotation servos as PWM controlled gear motors.
uses the analog pins for nunchuck connection
analog 4= data
analog 5= clock
nunchuck uses 6 bytes for full set of data
0: x axis of analog stick left(min) 0x1e, center 0x7e, right 0xe1
1: y axis of analog stick down (min) 0x1d, center 0x7b, max 0xdf
2: x acceleration 1g (min) 0x48, medium 0x7d, max 0xb0
3: y acceleration 1g (min) 0x 46, medium 0x7a, max 0xaf
4: z acceleration 1g (min) 0x4a, medium 0x7e, max 0xb1
5: buttons/ alleleration LSB:
bit 0 z button (0= pressed)
bit 1 c button (0= pressed)
bit 2 and 3 x acceleration LSB
bit 4 and 5 y acceleration LSB
bit 6 and 7 z acceleration LSB
Scott Powers
6-10-2009
*/
#include Servo.h //stuff in italics needs to go in angle brackets
#include string.h
#include Wire.h
#undef int
#include stdio.h
Servo leftMotor;
Servo rightMotor;
uint8_t outbuf[6]; // array to store arduino output
int cnt = 0;
int ledPin = 13;
int delayTime = 50000;
void
setup ()
{
Serial.begin (19200);
Serial.print ("Finished setup\n");
Wire.begin (); // join i2c bus with address 0x52
leftMotor.attach(10);
rightMotor.attach(9);
nunchuck_init (); // send the initilization handshake
}
void
nunchuck_init ()
{
Wire.beginTransmission (0x52); // transmit to device 0x52
Wire.send (0x40); // sends memory address
Wire.send (0x00); // sends sent a zero.
Wire.endTransmission (); // stop transmitting
}
void
send_zero ()
{
Wire.beginTransmission (0x52); // transmit to device 0x52
Wire.send (0x00); // sends one byte
Wire.endTransmission (); // stop transmitting
}
void
loop ()
{
Wire.requestFrom (0x52, 6); // request data from nunchuck
while (Wire.available ())
{
outbuf[cnt] = nunchuk_decode_byte (Wire.receive ()); // receive byte as an integer
digitalWrite (ledPin, HIGH); // sets the LED on
cnt++;
}
// If we recieved the 6 bytes, then go print them
if (cnt >= 5)
{
move ();
}
cnt = 0;
send_zero (); // send the request for next bytes
delayMicroseconds(delayTime);
}
// Print the input data we have recieved
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits. That is why I
// multiply them by 2 * 2
void
move ()
{
int joy_x_axis = map(outbuf[0],0x1E, 0xE1, 0, 180);
int joy_y_axis = map(outbuf[1],0x1d, 0xdf, 0, 180);
int turn= joy_x_axis-88;
int z_button = 0;
int c_button = 0;
int left=(joy_y_axis+turn);
int right=(180-joy_y_axis+turn);
rightMotor.write(right);
leftMotor.write(left);
// byte outbuf[5] contains bits for z and c buttons
// it also contains the least significant bits for the accelerometer data
// so we have to check each bit of byte outbuf[5]
if ((outbuf[5] >> 0) & 1)
{
z_button = 1;
}
if ((outbuf[5] >> 1) & 1)
{
c_button = 1;
}
Serial.print (joy_x_axis, DEC);
Serial.print ("\t");
Serial.print (joy_y_axis, DEC);
Serial.print ("\t");
digitalWrite(13, z_button);
Serial.print (z_button, DEC);
Serial.print ("\t");
Serial.print (c_button, DEC);
Serial.print ("\t");
Serial.print ("\r\n");
}
// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char
nunchuk_decode_byte (char x)
{
x = (x ^ 0x17) + 0x17;
return x;
}
