I simplified and condensed the code to make it a little more readable. I will add in more comments once I get the audio piece working with a wave shield. Two add-on stacked shields will really simplify wire management and looks of the setup.
Updated code:
/***************************************************
This is an example for our Adafruit 16-channel PWM & Servo driver
Servo test - this will drive 8 servos, one after the other on the
first 8 pins of the PCA9685
Pick one up today in the adafruit shop!
------>
Adafruit 16-Channel 12-bit PWM/Servo Driver - I2C interface
These drivers use I2C to communicate, 2 pins are required to
interface.
Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!
Written by Limor Fried/Ladyada for Adafruit Industries.
BSD license, all text above must be included in any redistribution
****************************************************/
#include "ArduinoNunchuk.h"
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//Creates the objects to control the servos
ArduinoNunchuk nunchuk = ArduinoNunchuk();
#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 550 // this is the 'maximum' pulse length count (out of 4096)
int pulseWidth1 = 0; // Amount to pulse the servo 1
int pulseWidth2 = 0; // Amount to pulse the servo 2
int pulseWidth3 = 0; // Amount to pulse the servo 3
long lastPulse1;
long lastPulse2;
long lastPulse3;
int xjoystick;
int yjoystick;
int xtilt;
const int SoundPin1 = 14;
const int SoundPin2 = 150;
const int servoPin1 = 9; // Control pin for servo motor
const int servoPin2 = 11; // Control pin for servo motor
const int servoPin3 = 10; // Control pin for servo motor
const int ledPin1 = 13; // Control pin for LED 1
const byte ledPin2 = 12; // Control pin for LED 2
const int servoPin4 = 6; // Control pin for servo motor
int pulseWidth4 = 0; // Amount to pulse the servo 4
long lastPulse4;
int minPulse = 150; // minimum pulse width
int loop_cnt=0;
int ytilt;
int refreshTime = 20; // the time in millisecs needed in between pulses
void setup() {
Serial.println("PWM Begin"); pwm.begin(); pwm.setPWMFreq(60);
// This moves all servos to minimum positions at start. Good if you do not want overloaded servos
pulseWidth1 = minPulse; pulseWidth2 = minPulse; pulseWidth3 = minPulse; pulseWidth4 = minPulse;
// This initializes the Serial interface functions
Serial.begin(9600); Serial.print("loading sound card init\n"); delay(1000); Serial.print("card initialized");
//Initializes nunchuck and servos
nunchuk.init();
delay(10);
}
void loop() {
// This initializes the servo read and write functions
checkNunchuck1(); updateServo1(); checkNunchuck2(); updateServo2(); checkNunchuck3(); updateServo3(); checkNunchuck4(); updateServo4();
// This checks if buttons are pressed and then turns on two separate LED elements
if(nunchuk.zButton == 1) { pwm.setPWM(ledPin1, 4096, 0); delay(300); pwm.setPWM(ledPin1, 0, 4096); delay(300); }
if(nunchuk.zButton == 1) { pwm.setPWM(SoundPin2, 4096, 0); delay(300); pwm.setPWM(SoundPin2, 0, 4096); delay(300); }
if (nunchuk.cButton == 1) { pwm.setPWM(ledPin2, 4096, 0); delay(300); }
else { pwm.setPWM(ledPin2, 0, 4096); }
// This sets and reads the output from the nunchuck and stores them as floating variables
xjoystick = nunchuk.analogX; xjoystick = constrain(xjoystick, 26, 226); xjoystick = map(xjoystick, 26, 226, 0, 180);
yjoystick = nunchuk.analogY; yjoystick = constrain(yjoystick, 26, 226); yjoystick = map(yjoystick, 26, 226, 180, 0);
xtilt = nunchuk.accelX; xtilt = constrain(xtilt, 320, 720); xtilt = map(xtilt, 320, 720, 180, 0);
ytilt = nunchuk.accelY; ytilt = constrain(ytilt, 320, 720); ytilt = map(ytilt, 320, 720, 0, 180);
// This prints the serial status of the nunchuck.
Serial.print ("Joystick X: "); Serial.print (xjoystick, DEC); Serial.print ("\t");
Serial.print ("Joystick Y: "); Serial.print (yjoystick, DEC); Serial.print ("\t");
Serial.print ("X: "); Serial.print (xtilt, DEC); Serial.print ("\t");
Serial.print ("Y: "); Serial.print (ytilt, DEC); Serial.print ("\t");
nunchuk.update();
if (nunchuk.cButton == 1) { Serial.print("--C-- "); }
if (nunchuk.zButton == 1) { Serial.print("--Z-- "); }
if (nunchuk.cButton == 1 && nunchuk.zButton == 1) { Serial.print("--Z-C--"); }
Serial.print ("\r\n");
}
// These are the functions to check and set the PWM settings for the servos
void checkNunchuck1() { if( loop_cnt > 100 ) { float tilt = xjoystick; pulseWidth1 = map(xjoystick, 0, 180, SERVOMIN, SERVOMAX); loop_cnt = 0; } loop_cnt++; }
void updateServo1() { if (millis() - lastPulse1 >= refreshTime) { pwm.setPWM(servoPin1, 0, pulseWidth1); lastPulse1 = millis(); } }
void checkNunchuck2() { if( loop_cnt > 100 ) { float tilt = yjoystick; pulseWidth2 = map(yjoystick, 0, 180, SERVOMIN, SERVOMAX); loop_cnt = 0; } loop_cnt++; }
void updateServo2() { if (millis() - lastPulse2 >= refreshTime) { pwm.setPWM(servoPin2, 0, pulseWidth2); lastPulse2 = millis(); } }
void checkNunchuck3() { if( loop_cnt > 100 ) { float tilt = xtilt; pulseWidth3 = map(xtilt, 0, 180, SERVOMIN, SERVOMAX); loop_cnt = 0; } loop_cnt++; }
void updateServo3() { if (millis() - lastPulse3 >= refreshTime) { pwm.setPWM(servoPin3, 0, pulseWidth3); lastPulse3 = millis(); } }
void checkNunchuck4() { if( loop_cnt > 100 ) { float tilt = ytilt; pulseWidth4 = map(ytilt, 0, 180, SERVOMIN, SERVOMAX); loop_cnt = 0; } loop_cnt++; }
void updateServo4() { if (millis() - lastPulse4 >= refreshTime) { pwm.setPWM(servoPin4, 0, pulseWidth4); lastPulse4 = millis(); } }