int main(int arg, char **argv) {
struct nunchuck_data n_data;
char *device="/dev/i2c-1";
int i2c_fd;
i2c_fd=init_i2c(device);
if (i2c_fd < 0) {
fprintf(stderr,"Error opening i2c dev file %s\n",device);
return -1;
}
if (init_nunchuck(i2c_fd)) {
fprintf(stderr,"Error initializing nunchuck\n");
return -1;
}
while(1) {
read_nunchuck(i2c_fd,&n_data);
printf("Joystick X: %d\n",n_data.joy_x);
printf("Joystick Y: %d\n",n_data.joy_y);
printf("Accelerometer X: %d\n",n_data.acc_x);
printf("Accelerometer Y: %d\n",n_data.acc_y);
printf("Accelerometer Z: %d\n",n_data.acc_z);
printf("Z button pressed: %d\n",n_data.z_pressed);
printf("C button pressed: %d\n",n_data.c_pressed);
usleep(100000);
}
return 0;
}
int main(void){
char i;
unsigned char data[16];
short wiichuck[7], xinit=0, yinit=0, l_vel, r_vel;
int xpow, ypow;
LockoutProtection();
InitializeMCU();
InitializeUART();
InitializeI2C();
InitializeServos();
SetServoPosition(SERVO_0, 140);
InitializeMotors(true, false);
InitializeEncoders(true, false);
// UARTprintf("Initializing Nunchuck\n\n");
// I2CSend(0x52<<1, 2, 0x40, 0x00);
// Wait(25);
init_nunchuck();
// Wireless Nunchucks Zero @ 128
xinit = yinit = 128;
while(1){
//Start Recalculating Values
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){
UARTprintf("Send Zero Error:\n");
switch(I2CMasterErr(I2C0_MASTER_BASE)){
case I2C_MASTER_ERR_ADDR_ACK:
UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n");
break;
case I2C_MASTER_ERR_DATA_ACK:
UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n");
break;
case I2C_MASTER_ERR_ARB_LOST:
UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n");
break;
default:
UARTprintf("WTF: %d\n", I2CMasterErr(I2C0_MASTER_BASE));
}
// Reinitialize Nunchuck on error
init_nunchuck();
}else{
Wait(1);
I2CRecieve(0x52<<1, data, 6); // Nunchuck data is 6 bytes, but for whatever reason, MEMOREX Wireless Nunchuck wants to send 8...
if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){
UARTprintf("Send Zero Error:\n");
switch(I2CMasterErr(I2C0_MASTER_BASE)){
case I2C_MASTER_ERR_ADDR_ACK:
UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n");
break;
case I2C_MASTER_ERR_DATA_ACK:
UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n");
break;
case I2C_MASTER_ERR_ARB_LOST:
UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n");
break;
}
// Reinitialize Nunchuck on error
init_nunchuck();
}else{
//for(i=0; i<6; i++)
// data[i] = (data[i] ^ 0x17) + 0x17; // Nintendo decided to encrypt thir data...
// Save Joystick Data
wiichuck[0] = data[1]; // X Axis Joystick
wiichuck[1] = data[0]; // Y Axis Joystick
wiichuck[2] = (((unsigned short) data[2]) << 2) + (((unsigned short) data[5]) & (3<<2)); // X Axis Accel
wiichuck[3] = (((unsigned short) data[3]) << 2) + (((unsigned short) data[5]) & (3<<4)); // Y Axis Accel
wiichuck[4] = (((unsigned short) data[4]) << 2) + (((unsigned short) data[5]) & (3<<6)); // Z Axis Accel
wiichuck[5] = data[5] & (1 << 1) ? 0 : 1; //'C' Button
wiichuck[6] = data[5] & (1 << 0) ? 0 : 1; //'Z' Button
//if (xinit == 0 && yinit == 0){
// xinit = wiichuck[0]-127;
// yinit = wiichuck[1]-127;
//}else{
xpow = (wiichuck[0]-xinit)/2;
ypow = (wiichuck[1]-yinit)/2;
l_vel = (xpow - ypow)*2;
r_vel = (xpow + ypow)*2;
l_vel = l_vel > 127 ? 127 : l_vel;
r_vel = r_vel > 127 ? 127 : r_vel;
l_vel = l_vel < -127 ? -127 : l_vel;
r_vel = r_vel < -127 ? -127 : r_vel;
//UARTprintf("X: %d\tY: %d\n", xpow*2, ypow*2);
SetMotorPowers(l_vel / (wiichuck[5]==0 ? 2 : 1), r_vel / (wiichuck[5]==0 ? 2 : 1));
UARTprintf("Motor L: %d\tMotor R: %d\n", l_vel, r_vel);
SetServoPosition(SERVO_0, wiichuck[6]==1 ? 255 : 140);
UARTprintf("Nunchuck Data:\n");
for(i=0; i<7; i++){
UARTprintf(" %d\n", wiichuck[i]);
}NL;
Wait(100);
}
}
}
}