c'est clair je me suis dis que je me coucherai un peu moins bête ce soir
voici le code complet, fonctionnel pour une commande filaire
#include <PS2X_lib.h> //for v1.6
// PS2 pins
const byte PS2_SEL = 16;
const byte PS2_DAT = 14;
const byte PS2_CMD = 15;
const byte PS2_CLK = 17;
// Control pins
const byte chenilleDenA = 2;
const byte chenilleGenB = 3;
const byte brasEnB = 5;
const byte balancierEnA = 6;
const byte godetEnB = 9;
const byte rotofilEnA = 10;
const byte tourelleEnA = 11;
const byte chenilleDin1 = 22;
const byte chenilleDin2 = 24;
const byte chenilleGin3 = 26;
const byte chenilleGin4 = 28;
const byte tourelleIn1 = 31;
const byte tourelleIn2 = 33;
const byte brasIn3 = 34;
const byte brasIn4 = 36;
const byte balancierIn1 = 38;
const byte balancierIn2 = 40;
const byte godetIn3 = 42;
const byte godetIn4 = 44;
const byte rotofilIn1 = 46;
const byte rotofilIn2 = 48;
PS2X ps2x;
static byte PRX = 127, PRY = 128, PLX = 127, PLY = 128;
int nbrPushBtnX = 0; // pour la vitesse du rotofil
void avanceChenilleGauche() {
digitalWrite(chenilleGin3, LOW);
digitalWrite(chenilleGin4, HIGH);
analogWrite(chenilleGenB, 255);
}
void reculChenilleGauche() {
digitalWrite(chenilleGin3, HIGH);
digitalWrite(chenilleGin4, LOW);
analogWrite(chenilleGenB, 255);
}
void avanceChenilleDroite() {
digitalWrite(chenilleDin1, LOW);
digitalWrite(chenilleDin2, HIGH);
analogWrite(chenilleDenA, 255);
}
void reculChenilleDroite() {
digitalWrite(chenilleDin1, HIGH);
digitalWrite(chenilleDin2, LOW);
analogWrite(chenilleDenA, 255);
}
void tourelleTourneDroite(byte v) {
digitalWrite(tourelleIn1, LOW);
digitalWrite(tourelleIn2, HIGH);
analogWrite(tourelleEnA, v);
}
void tourelleTourneGauche(byte v) {
digitalWrite(tourelleIn1, HIGH);
digitalWrite(tourelleIn2, LOW);
analogWrite(tourelleEnA, v);
}
void brasMonte(byte v) {
digitalWrite(brasIn3, LOW);
digitalWrite(brasIn4, HIGH);
analogWrite(brasEnB, v);
}
void brasDescend(byte v) {
digitalWrite(brasIn3, HIGH);
digitalWrite(brasIn4, LOW);
analogWrite(brasEnB, v);
}
void balancierMonte(byte v) {
digitalWrite(balancierIn1, LOW);
digitalWrite(balancierIn2, HIGH);
analogWrite(balancierEnA, v);
}
void balancierDescend(byte v) {
digitalWrite(balancierIn1, HIGH);
digitalWrite(balancierIn2, LOW);
analogWrite(balancierEnA, v);
}
void godetOuvre(byte v) {
digitalWrite(godetIn3, LOW);
digitalWrite(godetIn4, HIGH);
analogWrite(godetEnB, v);
}
void godetFerme(byte v) {
digitalWrite(godetIn3, HIGH);
digitalWrite(godetIn4, LOW);
analogWrite(godetEnB, v);
}
void rotofilVitesse1() {
digitalWrite(rotofilIn1, HIGH);
digitalWrite(rotofilIn2, LOW);
analogWrite(rotofilEnA, 128);
}
void rotofilVitesse2() {
digitalWrite(rotofilIn1, HIGH);
digitalWrite(rotofilIn2, LOW);
analogWrite(rotofilEnA, 192);
}
void rotofilVitesse3() {
digitalWrite(rotofilIn1, HIGH);
digitalWrite(rotofilIn2, LOW);
analogWrite(rotofilEnA, 255);
}
void rotofilArret() {
digitalWrite(rotofilIn1, LOW);
digitalWrite(rotofilIn2, LOW);
analogWrite(rotofilEnA, 0);
}
// --------------- CODE PRINCIPAL ---------------
void setup() {
pinMode(chenilleDenA, OUTPUT);
pinMode(chenilleDin1, OUTPUT);
pinMode(chenilleDin2, OUTPUT);
pinMode(chenilleGenB, OUTPUT);
pinMode(chenilleGin3, OUTPUT);
pinMode(chenilleGin4, OUTPUT);
pinMode(tourelleEnA, OUTPUT);
pinMode(tourelleIn1, OUTPUT);
pinMode(tourelleIn2, OUTPUT);
pinMode(brasEnB, OUTPUT);
pinMode(brasIn3, OUTPUT);
pinMode(brasIn4, OUTPUT);
pinMode(balancierEnA, OUTPUT);
pinMode(balancierIn1, OUTPUT);
pinMode(balancierIn2, OUTPUT);
pinMode(godetEnB, OUTPUT);
pinMode(godetIn3, OUTPUT);
pinMode(godetIn4, OUTPUT);
pinMode(rotofilEnA, OUTPUT);
pinMode(rotofilIn1, OUTPUT);
pinMode(rotofilIn2, OUTPUT);
Serial.begin(115200);
delay(300); //added delay to give wireless ps2 module some time to startup, before configuring it
int error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, true, true);
switch (error) {
case 0: Serial.println(F("Found Controller, configured successful ")); break;
case 1: Serial.println(F("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips")); break;
case 2: Serial.println(F("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips")); break;
case 3: Serial.println(F("Controller refusing to enter Pressures mode, may not support it.")); break;
default: Serial.println(F("Unknown Error")); break;
}
byte type = ps2x.readType();
switch (type) {
case 0: Serial.print(F("Unknown Controller type found ")); break;
case 1: Serial.print(F("DualShock Controller found ")); break;
case 2: Serial.print(F("GuitarHero Controller found ")); break;
case 3: Serial.print(F("Wireless Sony DualShock Controller found ")); break;
default: Serial.println(F("Unknown Type")); break;
}
}
void loop() {
ps2x.read_gamepad(); //read controller
byte LY = ps2x.Analog(PSS_LY);
byte LX = ps2x.Analog(PSS_LX);
byte RY = ps2x.Analog(PSS_RY);
byte RX = ps2x.Analog(PSS_RX);
if ((LX != PLX) || (LY != PLY) || (RX != PRX) || (RY != PRY)) { // joystick data changed
Serial.print(LX); Serial.print(F(", ")); Serial.print(LY); Serial.print(F(", ")); Serial.print(RX); Serial.print(F(", ")); Serial.println(RY);
} // end joystick changed
if (LX != PLX) {
if (LX > 140) tourelleTourneDroite(map(LX, 0, 140, 0, 255)); else digitalWrite(tourelleIn2, LOW);
if (LX < 113) tourelleTourneGauche(map(LX, 113, 0, 0, 255)); else digitalWrite(tourelleIn1, LOW);
PLX = LX;
}
if (RX != PRX) {
if (RX > 140) godetOuvre(map(RX, 0, 140, 0, 255)); else digitalWrite(tourelleIn2, LOW);
if (RX < 113) godetFerme(map(RX, 113, 0, 0, 255)); else digitalWrite(tourelleIn1, LOW);
PRX = RX;
}
if (LY != PLY) {
if (LY > 140) balancierMonte(map(LY, 140, 255, 0, 255)); else digitalWrite(balancierIn2, LOW);
if (LY < 113) balancierDescend(map(LY, 113, 0, 0, 255)); else digitalWrite(balancierIn1, LOW);
PLY = LY;
}
if (RY != PRY) {
if (RY > 140) brasMonte(map(RY, 140, 255, 0, 255)); else digitalWrite(brasIn4, LOW);
if (RY < 113) brasDescend(map(RY, 113, 0, 0, 255)); else digitalWrite(brasIn3, LOW);
PRY = RY;
}
if (ps2x.Button(PSB_L1) && !ps2x.Button(PSB_L2)) avanceChenilleGauche(); else digitalWrite(chenilleGin4, LOW);
if (ps2x.Button(PSB_R1) && !ps2x.Button(PSB_R2)) avanceChenilleDroite(); else digitalWrite(chenilleDin2, LOW);
if (ps2x.Button(PSB_L2) && !ps2x.Button(PSB_L1)) reculChenilleGauche(); else digitalWrite(chenilleGin3, LOW);
if (ps2x.Button(PSB_R2) && !ps2x.Button(PSB_R1)) reculChenilleDroite(); else digitalWrite(chenilleDin1, LOW);
if (ps2x.Button(PSB_SQUARE)) nbrPushBtnX = 0;
if (ps2x.ButtonPressed(PSB_CROSS)) nbrPushBtnX = min(3, nbrPushBtnX + 1);
switch (nbrPushBtnX) {
case 0: rotofilArret(); break;
case 1: rotofilVitesse1(); break;
case 2: rotofilVitesse2(); break;
case 3: rotofilVitesse3(); break;
}
delay(50) ;
}
un petit loupé ici les 'else' ne commande pas les bon pins