/*********************************************************************

SIMULATEUR 2DOF – VERSION COCKPIT FINALE cablage definitif

@Domochris.fr

ESP32 WROOM + 2x BTS7960 + 2x AS5600 + SimTools v3

 

✔ Pas de shutdown SimTools

✔ Pas de coup de poing logiciel

✔ Position absolue limitée

✔ PID simple (P + D)

✔ Timeout sécurité

*********************************************************************/

 

#include <Arduino.h>

#include <Wire.h>

 

/*********************************************************************

PINOUT

*********************************************************************/

 

// ===== MOTEUR 1 – ROLL (gauche) =====

#define RPWM1 18

#define LPWM1 5

#define REN1  2

#define LEN1  4

 

// ===== MOTEUR 2 – PITCH (droite) =====

#define RPWM2 19

#define LPWM2 23

#define REN2  32

#define LEN2  33

 

// ===== AS5600 =====

#define SDA1 16

#define SCL1 17

#define SDA2 21

#define SCL2 22

 

/*********************************************************************

PARAMÈTRES

*********************************************************************/

 

// Limites mécaniques (à ajuster)

#define MIN_POS  200

#define MAX_POS  3900

 

// Zone morte position

#define DEADZONE 15

 

// Timeout communication SimTools (ms)

#define TIMEOUT 300

 

// PID (valeurs sûres)

float Kp = 1.4;

float Kd = 0.25;

 

/*********************************************************************

VARIABLES

*********************************************************************/

 

TwoWire I2C_1 = TwoWire(0);

TwoWire I2C_2 = TwoWire(1);

 

float pos1, pos2;

float target1 = 2048;

float target2 = 2048;

 

float err1, err2;

float lastErr1 = 0, lastErr2 = 0;

 

float pid1, pid2;

unsigned long lastSerial = 0;

 

/*********************************************************************

LECTURE AS5600

*********************************************************************/

uint16_t readAS5600(TwoWire &bus)

{

bus.beginTransmission(0x36);

bus.write(0x0C);

if (bus.endTransmission(false) != 0) return 2048;

 

if (bus.requestFrom(0x36, 2) != 2) return 2048;

 

uint16_t hi = bus.read();

uint16_t lo = bus.read();

return ((hi << 8) | lo) & 0x0FFF;

}

 

/*********************************************************************

COMMANDE MOTEUR

*********************************************************************/

void setMotor(int rpwm, int lpwm, float val)

{

val = constrain(val, -255, 255);

 

// Zone morte PWM

if (abs(val) < 5)

{

ledcWrite(rpwm, 0);

ledcWrite(lpwm, 0);

return;

}

 

if (val > 0)

{

ledcWrite(rpwm, val);

ledcWrite(lpwm, 0);

}

else

{

ledcWrite(rpwm, 0);

ledcWrite(lpwm, -val);

}

}

 

/*********************************************************************

SETUP

*********************************************************************/

void setup()

{

Serial.begin(115200);

 

// I2C AS5600

I2C_1.begin(SDA1, SCL1, 400000);

I2C_2.begin(SDA2, SCL2, 400000);

 

// Enable BTS7960

pinMode(REN1, OUTPUT); pinMode(LEN1, OUTPUT);

pinMode(REN2, OUTPUT); pinMode(LEN2, OUTPUT);

 

digitalWrite(REN1, LOW); digitalWrite(LEN1, LOW);

digitalWrite(REN2, LOW); digitalWrite(LEN2, LOW);

 

// PWM ESP32 – 20 kHz / 8 bits

ledcAttach(RPWM1, 20000, 8);

ledcAttach(LPWM1, 20000, 8);

ledcAttach(RPWM2, 20000, 8);

ledcAttach(LPWM2, 20000, 8);

}

 

/*********************************************************************

LOOP

*********************************************************************/

void loop()

{

// Lecture position

pos1 = readAS5600(I2C_1);

pos2 = readAS5600(I2C_2);

 

// Lecture SimTools

if (Serial.available())

{

target1 = Serial.parseFloat();

target2 = Serial.parseFloat();

 

target1 = constrain(target1, MIN_POS, MAX_POS);

target2 = constrain(target2, MIN_POS, MAX_POS);

 

lastSerial = millis();

}

 

// Timeout sécurité

if (millis() - lastSerial > TIMEOUT)

{

digitalWrite(REN1, LOW); digitalWrite(LEN1, LOW);

digitalWrite(REN2, LOW); digitalWrite(LEN2, LOW);

setMotor(RPWM1, LPWM1, 0);

setMotor(RPWM2, LPWM2, 0);

return;

}

 

// Calcul erreur

err1 = target1 - pos1;

err2 = target2 - pos2;

 

// Zone morte position

if (abs(err1) < DEADZONE && abs(err2) < DEADZONE)

{

digitalWrite(REN1, LOW); digitalWrite(LEN1, LOW);

digitalWrite(REN2, LOW); digitalWrite(LEN2, LOW);

setMotor(RPWM1, LPWM1, 0);

setMotor(RPWM2, LPWM2, 0);

return;

}

 

// Activer drivers

digitalWrite(REN1, HIGH); digitalWrite(LEN1, HIGH);

digitalWrite(REN2, HIGH); digitalWrite(LEN2, HIGH);

 

// PID

pid1 = Kp * err1 + Kd * (err1 - lastErr1);

pid2 = Kp * err2 + Kd * (err2 - lastErr2);

 

lastErr1 = err1;

lastErr2 = err2;

 

// Commande moteurs

setMotor(RPWM1, LPWM1, pid1);

setMotor(RPWM2, LPWM2, pid2);

 

delay(5);

}