Hey this is a code to get a single servo working with a wii nunchuck. Can anyone modify this to work with two servos. To go with the up and down movements of the nunchuck (Z axis?) with one servo. And the side to side (XY Axis?) with the other.
??
#include <Wire.h>
int ledPin = 13;
int servoPin = 7; // Control pin for servo motor
int pulseWidth = 0; // Amount to pulse the servo
int refreshTime = 20; // the time in millisecs needed in between pulses
long lastPulse;
int minPulse = 700; // minimum pulse width
int loop_cnt=0;
void setup()
{
Serial.begin(19200);
pinMode(servoPin, OUTPUT); // Set servo pin as an output pin
pulseWidth = minPulse; // Set the motor position to the minimum
nunchuck_setpowerpins(); // use analog pins 2&3 as fake gnd & pwr
nunchuck_init(); // send the initilization handshake
Serial.print("NunchuckServo ready\n");
}
void loop()
{
checkNunchuck();
updateServo(); // update servo position
if( nunchuck_zbutton() ) // light the LED if z button is pressed
digitalWrite(ledPin, HIGH);
else
digitalWrite(ledPin,LOW);
delay(1); // this is hear to give a known time per loop
}
void checkNunchuck()
{
if( loop_cnt > 100 ) { // loop()s is every 1msec, this is every 100msec
nunchuck_get_data();
nunchuck_print_data();
float tilt = nunchuck_accelx(); // x-axis, in this case ranges from ~70 - ~185
tilt = (tilt - 70) * 1.5; // convert to angle in degrees, roughly
pulseWidth = (tilt * 9) + minPulse; // convert angle to microseconds
loop_cnt = 0; // reset for
}
loop_cnt++;
}
// called every loop().
// uses global variables servoPin, pulsewidth, lastPulse, & refreshTime
void updateServo()
{
// pulse the servo again if rhe refresh time (20 ms) have passed:
if (millis() - lastPulse >= refreshTime) {
digitalWrite(servoPin, HIGH); // Turn the motor on
delayMicroseconds(pulseWidth); // Length of the pulse sets the motor position
digitalWrite(servoPin, LOW); // Turn the motor off
lastPulse = millis(); // save the time of the last pulse
}
}
//
// Nunchuck functions
//
static uint8_t nunchuck_buf[6]; // array to store nunchuck data,
// Uses port C (analog in) pins as power & ground for Nunchuck
static void nunchuck_setpowerpins()
{
#define pwrpin PORTC3
#define gndpin PORTC2
DDRC |= _BV(pwrpin) | _BV(gndpin);
PORTC &=~ _BV(gndpin);
PORTC |= _BV(pwrpin);
delay(100); // wait for things to stabilize
}
// initialize the I2C system, join the I2C bus,
// and tell the nunchuck we're talking to it
void nunchuck_init()
{
Wire.begin(); // join i2c bus as master
Wire.beginTransmission(0x52); // transmit to device 0x52
Wire.send(0x40); // sends memory address
Wire.send(0x00); // sends sent a zero.
Wire.endTransmission(); // stop transmitting
}
// Send a request for data to the nunchuck
// was "send_zero()"
void nunchuck_send_request()
{
Wire.beginTransmission(0x52); // transmit to device 0x52
Wire.send(0x00); // sends one byte
Wire.endTransmission(); // stop transmitting
}
// Receive data back from the nunchuck,
// returns 1 on successful read. returns 0 on failure
int nunchuck_get_data()
{
int cnt=0;
Wire.requestFrom (0x52, 6); // request data from nunchuck
while (Wire.available ()) {
// receive byte as an integer
nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.receive());
cnt++;
}
nunchuck_send_request(); // send request for next data payload
// If we recieved the 6 bytes, then go print them
if (cnt >= 5) {
return 1; // success
}
return 0; //failure
}
// 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 nunchuck_print_data()
{
static int i=0;
int joy_x_axis = nunchuck_buf[0];
int joy_y_axis = nunchuck_buf[1];
int accel_x_axis = nunchuck_buf[2]; // * 2 * 2;
int accel_y_axis = nunchuck_buf[3]; // * 2 * 2;
int accel_z_axis = nunchuck_buf[4]; // * 2 * 2;
int z_button = 0;
int c_button = 0;
// byte nunchuck_buf[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 ((nunchuck_buf[5] >> 0) & 1)
z_button = 1;
if ((nunchuck_buf[5] >> 1) & 1)
c_button = 1;
if ((nunchuck_buf[5] >> 2) & 1)
accel_x_axis += 2;
if ((nunchuck_buf[5] >> 3) & 1)
accel_x_axis += 1;
if ((nunchuck_buf[5] >> 4) & 1)
accel_y_axis += 2;
if ((nunchuck_buf[5] >> 5) & 1)
accel_y_axis += 1;
if ((nunchuck_buf[5] >> 6) & 1)
accel_z_axis += 2;
if ((nunchuck_buf[5] >> 7) & 1)
accel_z_axis += 1;
Serial.print(i,DEC);
Serial.print("\t");
Serial.print("joy:");
Serial.print(joy_x_axis,DEC);
Serial.print(",");
Serial.print(joy_y_axis, DEC);
Serial.print(" \t");
Serial.print("acc:");
Serial.print(accel_x_axis, DEC);
Serial.print(",");
Serial.print(accel_y_axis, DEC);
Serial.print(",");
Serial.print(accel_z_axis, DEC);
Serial.print("\t");
Serial.print("but:");
Serial.print(z_button, DEC);
Serial.print(",");
Serial.print(c_button, DEC);
Serial.print("\r\n"); // newline
i++;
}
// 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;
}
// returns zbutton state: 1=pressed, 0=notpressed
int nunchuck_zbutton()
{
return ((nunchuck_buf[5] >> 0) & 1) ? 0 : 1; // voodoo
}
// returns zbutton state: 1=pressed, 0=notpressed
int nunchuck_cbutton()
{
return ((nunchuck_buf[5] >> 1) & 1) ? 0 : 1; // voodoo
}
// returns value of x-axis joystick
int nunchuck_joyx()
{
return nunchuck_buf[0];
}
// returns value of y-axis joystick
int nunchuck_joyy()
{
return nunchuck_buf[1];
}
// returns value of x-axis accelerometer
int nunchuck_accelx()
{
return nunchuck_buf[2]; // FIXME: this leaves out 2-bits of the data
}
// returns value of y-axis accelerometer
int nunchuck_accely()
{
return nunchuck_buf[3]; // FIXME: this leaves out 2-bits of the data
}
// returns value of z-axis accelerometer
int nunchuck_accelz()
{
return nunchuck_buf[4]; // FIXME: this leaves out 2-bits of the data
}