lerxstrulz
Well-known member
Hey guys, I have been trying to get a Teensy 3.2 talking to a PCA9685 servo controller using the AdaFruit library with no luck so far. I was originally using an Arduino Uno, but decided to try a 3.2 as I need more memory (the sketch was bumping up against Arduino limits) and the arduino took up too much space in the project (I need to add an MP3 controller and speaker later on...)
The sketch works fine on the Arduino, but I can't get the Teensy and the PCA9685 talking. I wired in 4.7k pull-up resistors for SDA and SCL across 3.3V per this blog post.
Any help would be appreciated!
The sketch works fine on the Arduino, but I can't get the Teensy and the PCA9685 talking. I wired in 4.7k pull-up resistors for SDA and SCL across 3.3V per this blog post.
Any help would be appreciated!
Code:
#include <PS2X_lib.h> //for v1.6
//aAdafruit ServBoard setup
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// create PS2 Controller Class
PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(&Wire, 0x40);
int Wheel1;
int Wheel2;
#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)
void setup(){
Serial.begin(9600);
while(!Serial);
pinMode(18,INPUT_PULLUP); // Control for Send
pinMode(19,INPUT_PULLUP); // Control for Receive
Wire.setSDA(18);
Wire.setSCL(19);
Wire.begin();
pwm.begin();
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
delay(10);
pwm.setPWM(0, 0, 0);
pwm.setPWM(1, 0, 0);
// 13 = SCK, 11 = MOSI, 10 = SS (CS), 12 = MISO
//setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
error = ps2x.config_gamepad(13,11,10,12, false, false);
if(error == 0){
Serial.println("Found Controller, configured successful");
Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
Serial.println("holding L1 or R1 will print out the analog stick values.");
Serial.println("Go to www.billporter.info for updates and to report bugs.");
}
else if(error == 1)
Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
else if(error == 2)
Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
else if(error == 3)
Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
type = ps2x.readType();
switch(type) {
case 0:
Serial.println("Unknown Controller type");
break;
case 1:
Serial.println("DualShock Controller Found");
break;
case 2:
Serial.println("GuitarHero Controller Found");
break;
}
}
int last_right_x = 0;
int last_right_y = 0;
int wheel1 = 0;
int wheel2 = 0;
void loop(){
if(error == 1) //skip loop if no controller found
return;
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed
Serial.println("Start is being held");
if(ps2x.Button(PSB_SELECT))
Serial.println("Select is being held");
if(ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed
Serial.print("Up held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
}
if(ps2x.Button(PSB_PAD_RIGHT)){
Serial.print("Right held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
}
if(ps2x.Button(PSB_PAD_LEFT)){
Serial.print("LEFT held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
}
if(ps2x.Button(PSB_PAD_DOWN)){
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
}
vibrate = ps2x.Analog(PSAB_BLUE); //this will set the large motor vibrate speed based on
//how hard you press the blue (X) button
if (ps2x.NewButtonState()) //will be TRUE if any button changes state (on to off, or off to on)
{
if(ps2x.Button(PSB_L3))
Serial.println("L3 pressed");
if(ps2x.Button(PSB_R3))
Serial.println("R3 pressed");
if(ps2x.Button(PSB_L2))
Serial.println("L2 pressed");
if(ps2x.Button(PSB_R2))
Serial.println("R2 pressed");
if(ps2x.Button(PSB_GREEN))
Serial.println("Triangle pressed");
}
if(ps2x.ButtonPressed(PSB_RED)) //will be TRUE if button was JUST pressed
Serial.println("Circle just pressed");
if(ps2x.ButtonReleased(PSB_PINK)) //will be TRUE if button was JUST released
Serial.println("Square just released");
if(ps2x.NewButtonState(PSB_BLUE)) //will be TRUE if button was JUST pressed OR released
Serial.println("X just changed");
if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE
{
Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);
}
int right_y = ps2x.Analog(PSS_RY);
int right_x = ps2x.Analog(PSS_RX);
if (right_y != last_right_y || right_x != last_right_x) {
last_right_y = right_y;
last_right_x = right_x;
if (right_y == 127 or right_y == 128) {
wheel1 = 0;
wheel2 = 0;
if (right_x != 127 and right_x != 128) {
if (right_x < 127) {
wheel1 = map(right_x, 0, 128, 255, 340) ;
} else if (right_x > 128) {
wheel2 = map(right_x, 0, 128, 255, 340);
}
} else {
pwm.setPWM(0, 0, 0);
pwm.setPWM(1, 0, 0);
}
} else {
wheel1 = map(right_y, 0, 255, 255, 430);
wheel2 = map(right_y, 0, 255, 430, 255);
int modifier1 = 0;
int modifier2 = 0;
// modify the speed of either wheel to create a
// turn based on how far left/right the joystick is
if (right_x != 127 and right_x != 128) {
if (right_x < 127) {
modifier1 = wheel1 - map(right_x, 0, 128, 255, 430);
Serial.print("MODIFIER 1: ");
Serial.println(modifier1);
wheel1 -= modifier1;
Serial.print("WHEEL1 MODIFIED: ");
Serial.println(wheel1);
} else if (right_x > 128) {
modifier2 = wheel2 - map(right_x, 0, 128, 255, 430);
Serial.print("MODIFIER 2: ");
Serial.println(modifier2);
wheel2 -= modifier2;
Serial.print("WHEEL2 MODIFIED: ");
Serial.println(wheel2);
}
}
}
pwm.setPWM(0, 0, wheel1);
pwm.setPWM(1, 0, wheel2);
}
delay(50);
}