/* Trax, Définition du MCU
/// Répartition des calculateurs ///////////////
1 : Calculateur central
Gère : les alarmes; les témoins; les fonctions de sécurité
L'allimentation électrique des calculateurs
La direction, l'angle; et les bloquages de différentiels;
Calcul l'ouverture des distributeurs d'avancement gauche et droite en fonction de l'angle de braquage
Pilote le bloquage de différentiel AVANT
Relève les valeurs des contrôleurs de la partie avant, sauf les joysticks de pilotage hydraulique
divers de la partie avant
~~ coordonne les calculateurs au besion
2 : Moteur thermique
Gère : calcul la charge du moteur thermique et la charge hydraulique
L'aquisition du régime moteur thermique et des pressions hydrauliques
La position de l'accélérateur (papion des gaz) du moteur thermique
Pilote le bloquage de différentiel CENTRAL et ARRIERE
Relève les valeurs des contrôleurs de la partie arrière, sauf les joystick de pilotage hydraulique
divers de la partie arrière
3 : Distributeur AVANT
Gère : les joysticks et switch de pilotage hydraulique de la partie avant
La position et le pilotage des distributeurs de la partie avant
4 : Distributeur ARRIERE
Gère : les joysticks et switch de pilotage hydraulique de la partie arrière
La position et le pilotage des distributeurs de la partie arrière
/// Fin de la répartion des MCU ////////////////
*/
// MCU Distributeur AVANT
#define Version Joy-Av_Beta_0_0_00 // Du 09.12.2024
//#define EEPROMcontrole 247 // pour valider que l'Eeprom correpond à ce modul, le 3ème byte de l'Eeprom doit correspondre à cette valeur
#define CODE_EEPROM 247 // pour valider que l'Eeprom correpond à ce modul, le 3ème byte de l'Eeprom doit correspondre à cette valeur
/* - suite de la définition:
*Joy-Av_Beta_0_x_x Intégration du tableau de capteurs
******************************************************
Info: (distributeurs hydraulique Flow_Sharing)
- https://www.walvoil.com/allegati/catalogo/D1WWED01_ENG.pdf#page=28
// Input MCU:
//input PIN: Données acquise par ce MCU
//
//input DATA: Données reçu des autres calculateurs
// Débit hydraulique total de consigne actuel (calculé par {Calculateur central})
// Débit hydraulique total Réel actuel (débit téhorique calculé) (calculé par {Calculateur central})
// Mode de fonctionnement actuel (mode route; mode régime moteur; ...)
// STOP : Arrêt de sécurité (arrêt coup de poing)
// OnOff Moteur (L'arrêt du moteur thermique est en cours)
// Position de consigne du bloquage de différentiel central et arrière (calculé par {Calculateur central})
// Angle de braquage actuel
// Passer en mode arrêt de la machine
// Coupe ton auto-allimentation (ordre donné par {Calculateur central})
// Output MCU:
//output PIN: Actionneurs pilotés par ce MCU
// Piloatage position
//output DATA: Données transmise aux autres calculateurs
//
/// END Input / Output MCU ///////////////////
* END Définition du MCU. */
#define DEBUG
#ifdef DEBUG
#define DEBUG_1 //Debug version 1
#define DEBUG_2 //Debug version 2
#define DEBUG_CAPTEURS //Debug Concernant les capteurs "dans la loop()" (dans les fonctions répétées, pour le setup, c'est DEBUG)
#endif
void aFaire(){/*Reperrage des fonctions à faire ou à completter*/}
// Pré-déclaration des fonctions
bool autoAllAnalogMoyen(bool start = false);
#include <Arduino.h>
#include "MoyenneurAnalogique.h" // Assurez-vous d'inclure votre fichier d'en-tête
#include "pagesConfigModul.h"
#include <freertos/FreeRTOS.h>
/*Répartion des tâches sur les coeurs du MCU
* loop() Ne sert plus, elle est vide : Par défaut sur le coeur1
* loopFreeRTOS Coeur1 Prioryty 0 Remplace la loop() pour tous gérer par FreeRTOS
* SetupServer Coeur0 Priority 2 Wifi Appelle la fonction de configuration du serveur
* HandleClient Coeur0 Priority 2 Wifi Gère les requêtes si le point d'accès est démarré
* TimingTask Coeur1 Priority 1 Capteur Gère l'interval entre les lectures des capteurs
* CapteursTimeSet A_choix Priority 3 Capteur li les pins des capteurs et traite les données lues
*/
#include <freertos/task.h>
// Communications et moduls
#include <Wire.h> // For I2C
#include <SPI.h> // For SPI
#include <OneWire.h> // For DS température
// I2C : ADS_1x15
#include <Adafruit_ADS1X15.h>
#define I2C_SDA 21
#define I2C_SCL 22
Adafruit_ADS1115 ads1; //Adafruit_ADS1115 ads1(0x48); /* Use this for the 16-bit version */
Adafruit_ADS1115 ads2; //Adafruit_ADS1115 ads2(0x49);
Adafruit_ADS1115 ads3; //Adafruit_ADS1115 ads3(0x4A);
Adafruit_ADS1115 ads4; //Adafruit_ADS1115 ads3(0x4B);
#ifdef CODE_EEPROM
// #define EEPROM_I2C_ADDRESS 0x50
// // Définir le code par défaut en tant que uint8_t
// #if defined(EEPROM_I2C_ADDRESS) && !defined(CODE_EEPROM)
// #define CODE_EEPROM 123 // Assurez-vous que la valeur est entre 0 et 255
// #endif
#define addrAT24C256 0x52 // 82 = 0x52;
#endif
// Pré-déclaration des fonctions Eeprom
#if defined(CODE_EEPROM) && defined(addrAT24C256)
void EepromI2Cwrite(byte byteWrite, int numBitEeprom, int addrI2cEeprom = addrAT24C256);
void EepromI2CwriteInt16(int16_t value, int numBitEeprom, int addrI2cEeprom = addrAT24C256);
byte EepromI2Cread(int numBitEeprom, int addrI2cEeprom = addrAT24C256);
int16_t EepromI2CreadInt16(int numBitEeprom, int addrI2cEeprom = addrAT24C256)
#endif
// SPI : SD_CARD et CAN_BUS
#include <SD.h>
#include <mcp_can.h>
#define MOSI 23
#define MISO 19
#define SCK 18
#define SD_CS 5 /*Chip Select SD Card*/
#define CAN_CS 4 /*Chip Select CAN Bus (MCP2515)*/
MCP_CAN CAN(CAN_CS);
// Autres
const int oneWireBus = 4; // Pin où les DS18B20 sont connectés
OneWire oneWire(oneWireBus); // Créer une instance de OneWire
//J'utilise plus la lib:DsTemperature // #include <DallasTemperature.h>
//J'utilise plus la lib:DsTemperature // DallasTemperature DsTemperature(&oneWire); // Passer l'instance OneWire à DallasTemperature
/// END Communications et moduls /////////////
/*INFO: composition des dénominations de varialbes: (unitées = uintées généralement utilisées mais pas obligatior)
* ...th afectée au moteur thérmique
* ...hy afectée à l'hydraulique
* ...ls afectée au circuit hydraulique load-sensing
* P... Puissance (en [W])
* p... Pression (en [déci-bar])
* Q... Débit (en [déci-litre/min])
* S... Chemin ou course (en [%])
* C... Consigne (en [%] de la plage disponible min -> max)
* v... Volume (en [ccm] (centimètre cube ou 0,01dL))
* N... Charge utilisée (Utilisation de la force / la force maximum) (en [%])
* F... Charge envoyé au récepteur (un peux similaire à S...) (en [%])
* t... Température (en [°C])
*
* ..._In Identification de la pin pour un capteur soumis à "Moyenneur"
* ..._Pin Identification de la pin non soumis à "Moyenneur"
* ..._Ca Objet de "Moyenneur" contenant entre-autre ça variable de valeur
** par ex: pls_Ca correspond à la pression load-sensing don le capteur et les varibles de valeur son traités par la librairie "MoyenneurAnalogique.h"
INFO : Capteur de pression 0.5-4.5v// valeur du capteur vers déci-bars
//échelle: 0.5v = 0 bar; 4.5v = 250bar; autre = ERREUR || https://fr.aliexpress.com/i/1005002062993317.html
//5v = 32767 => 0.5v = 3276; 4.5v = 29490
*/
// Constantes
#define timeSteepLoop 1 // temps en miliss() entre chaque steep de la loop
const uint8_t CthMin = 10; // Charge ou le moteur thermique décolle du ralenti en [%] (position du papion des gaz)
const uint8_t pntRPM = 4; // Nombre de points pour un tour moteur (pour compte tour moteur thermique)
const uint16_t rpmMax = 3600; // Rotation par minutte maximum du moteur thérmique et de la pompe
const uint16_t rpmMin = 600; // Rotation par minutte minimum du moteur thérmique et de la pompe (ralenti sous legère charge)
const uint8_t vPompe = 18; // Volume d'huile pour un tour de pompe en [ccm] (centimètre cube ou 0,01dL)
//1 dL = 0,1 L = 0,1 dcm = 100 ccm
const uint8_t tarageLS = 140;
// Définition des canaux PWM et de la résolution et de la fréquence
const int pwmResolution = 8; // Résolution de 8 bits
const int pwmFrequency = 5000; // Fréquence de 5 kHz
// PINs (-1 pin à définir; 0 pas de pin physique; >200 (pin = nbr-200) sortie PWM; >=70 aquisition par ADS_1x15)
// VOIR AUSSI : le groupe {Communications et moduls}
#define pinRelaisContact -1 // Relais pour maintenir l'alimentation après que la clée de contact est passé à off
/* SCHEMAT pour contrôler la coupure de l'allimentation.
*********************************************************
(a1)-+ +------------------------GND GND ESP32 & (31)véhicule
4N25
| +------R2----------------3.3V Pin ESP32(OUTPUT) temps qu'il est à HIGH le module sera alimenté
|
30----+---TIP120----LM7805----------5V Vin ESP32 éventuellement ajouter un condo pour les micro coupures
|
15--(a2) R1 (a2)--LM1117-3.3---3.3V Pin ESP32(INPUT) informe le module si le contact est coupé
|
15-----D1---+----(a1)
********************************************************
15) clée de contact; 30)Batterie véhicule; 31) masse véhicule
a1) pont pour allimenter [TIP120] par [4N25]; [D1] diode pour éviter d'envoyer (a1) dans le (15) du véhicule
a2) détection par l'ESP de la coupure du contact ((a2) = (15) = clée de contact;)
[4N25] Optocoupleur (max35V)
[TIP120] Transistor Darlington : NPN max 60V 5A
//[LM7805] Remplacer par [LM2596T-5.0] //Régulateur de tension 5V : In 7V à 35V OUT 5V(±5%) max 1A
[LM2596T-5.0] Régulateur de tension 5.0V : In 7 à 40V OUT 5V max max 3A ==> 3A mieux adapter que le [LM7805]
//[LM1117-3.3] Remplacer par [LM2596T-3.3] // Régulateur de tension 3.3V : In 5.3V à 15V OUT 3.3V(±5%) max 800mA
[LM2596T-3.3] Régulateur de tension 3.3V : In 5.3V à 40V OUT 3.3V max max 3A ==> Compatible véhicule 12V ou véhicule 24V
*/
// #define Cth_PIN -1 // Piloatage position accélérateur (Pwm ou steeper)
// #define Cth_Dir_PIN -1 // Pilote la direction du moteur
// #define diffC_PIN -1 // Piloatage position diff central (Pwm ou steeper)
// #define diffC_Dir_PIN -1 // Pilote la direction du moteur
// #define diffAr_PIN -1 // Piloatage position diff arrière (Pwm ou steeper)
#define diffAr_Dir_PIN -1 // Pilote la direction du moteur
// #define Onhy_PIN -1 // Electrovanne de sécuritée du bloc hydraulique arrière
// #define Offth_PIN -1 // Arrêt de sécurité du moteur thermique
//#define venthy_PIN -1 // Piloatage ventilateur refroidisseur d'huile hydraulique (On-Off)
#define contactLong_PIN -1 // Relais d'allimentation du MCU pour prolonger l'allimentation après la coupure de contact (15) (pour permettre au MCU de passer en mode arrêt)
//END Pins
// Capteurs "MCU: 2 Moteur thermique" :
//// Pour les capteurs : ////
const char erreurtxt[] = "< ERREUR ! >"; // Texte d'entête pour les erreures (Serial.print(erreurtxt);)
enum CapteurType { // Désignation ==> pinMode() par défaut pour les pins 1 à 59 (chercher dans setup() "// Définir le pinMode() des capteurs")
RES_0_5V = 1, // Capteur analogique 0-5V (de résistance ) ==> INPUT
RES_0_10V, // Capteur analogique 0-10V ==> INPUT
INT_0_20mA, // Capteur de courant 0-20mA (de tension ) ==> INPUT
ON_OFF, // Capteur numérique avec état binaire (ex: capteur de présence) ==>INPUT_PULLUP
NUM_PULSE, // Capteur numérique qui compte des impulsions (vitesse) ==>INPUT_PULLUP
NUM_I2C, // Capteur utilisant le protocole I2C
NUM_SPI, // Capteur utilisant le protocole SPI
NUM_ONE_WIRE, // Capteur utilisant le protocole One Wire // Info: DS18B20: addresse[0] = 0x28 || DS18S20: addresse[0] = 0x26
CAPT_NON_DEFINI = 0 // Indique que capteur n'est pas défini ou qu'il n'entre pas dans cette énnumération (valeur par défaut)
};
// IMPORTANT pour le pilotage des distributeurs hydraulique :
// La mise à l'échelle de la valeur lue par le joystick doit correspondre au critaires ci-dessous :
// la position neutre du bistributeur correspond à une valeur de 15'000 au joystick,
// La position débit maximum du distributeur correspond à 0 d'un côté et à 30'000 de l'autre côté,
// En dessus de 30'000, si le distributeur et équipé, il passe dans ça zone flottante (de 30'001 à 32'767)
struct StDistributeurs{
uint8_t size;
int16_t position;
bool flottant;
bool floating;
uint8_t pinA;
uint8_t pinB;
};
enum DistribName { // Enumération des distributeurs hydraulique
distrArm, // 0)
distrBucket, // 1)
distrAccAv1, // 2)
distrAccAv2, // 3)
distrLift, // 4)
distrTilt, // 5)
distrAccArr1, // 6)
distrAccArr2, // 7)
distrWheelAvR,
distrWheelAvL,
distrWheelArrR,
distrWheelArrL,
NOMBRE_DE_DISTRIBUTEURS // Ceci donne le nombre total de distributeurs hydraulique
};
StDistributeurs distributeursTbl[NOMBRE_DE_DISTRIBUTEURS];
//size, pos, flotOn, foaling, pinA, pinB
distributeursTbl[distrArm] = {30, 15000, false, false, 0, 0};
distributeursTbl[distrBucket] = {30, 15000, false, true, 0, 0};
distributeursTbl[distrAccAv1] = {50, 15000, false, true, 0, 0};
distributeursTbl[distrAccAv2] = {10, 15000, false, true, 0, 0};
distributeursTbl[distrLift] = {30, 15000, false, true, 0, 0};
distributeursTbl[distrTilt] = {30, 15000, false, false, 0, 0};
distributeursTbl[distrAccArr1] = {50, 15000, false, true, 0, 0};
distributeursTbl[distrAccArr2] = {10, 15000, false, true, 0, 0};
distributeursTbl[distrWheelAvR] = {30, 15000, false, false, 0, 0};
distributeursTbl[distrWheelAvL] = {30, 15000, false, false, 0, 0};
distributeursTbl[distrWheelArrR] = {30, 15000, false, false, 0, 0};
distributeursTbl[distrWheelArrL] = {30, 15000, false, false, 0, 0};
struct StCapteurs{
void* lien; // *Lien pour le stockage et traitement des données. (voir fonction: [getCapteur()] et [setCapteur()])
uint8_t pin; //0=sans num de pin; <60: pin du MCU; 60 à 69 pin pour protocole; 70 à 73 port de l'ads1; 80 à 83 port de l'ads2; 90 à 93 port de l'ads3; 100 à 103 port de l'ads14
uint8_t type; // Type de capteur selon [enum CapteurType{}] . Défini la façon don le capteur Transmet ces données et comment il travail
uint8_t nbrMoyenne; // Nombre de valeurs moyennées par la [class Moyenneur] ;
// Si 0 : absente de la [class Moyenneur] ;
// Si 1 : Pas de calcul de moyenne, mais possible d'ajouter l'option de surveillance et/ou l'option mise à l'échelle d'étalonnage
uint16_t byteStartEeprom; // Position dans l'Eeprom où commencent les données du capteur.(si 0 pas de données dans l'Eeprom)
};
enum CapteursName { // Les "ON_OFF" ne serons normalement par présent dans le tableau des capteurs "capteursTbl[]""
joyArm, // 0) Bras de pelle
joyBucket, // 1) Inclinaison du gobet
joyAccAv1, // 2) Accessoire avant 50lt/min
joyAccAv2, // 3) Accessoire avant 10lt/min
joyLift, // 4) Relevage Arrière
joyTilt, // 5) 3èmme point Arrière
joyAccArr1, // 6) Accessoire arrière 50lt/min
joyAccArr2, // 7) Accessoire arrière 10lt/min
joyGaz, // 8) Pédal des gaz
joyAvance, // 9) Levier d'avancement
joyInching, //10) Pédal d'inching
PotiDiff1, //11) Différenciel Gauche/droite
PotiDiff2, //12) Différenciel Centrale
PotiGaz2, //13) Gaz à main
// pWR_Ca, //14)
// CAPTEUR_15, //15)
// CAPTEUR_16, //16)
// CAPTEUR_17, //17)
// CAPTEUR_18, //18)
// CAPTEUR_19, //19)
// CAPTEUR_20, //20)
// CAPTEUR_21, //21)
NOMBRE_DE_CAPTEURS // Ceci donne le nombre total de capteurs
};
StCapteurs capteursTbl[NOMBRE_DE_CAPTEURS];
//lien , pin, type, nbrMoyenne, byteStartEeprom
capteursTbl[joyArm] = {&distributeursTbl[distrArm].position, 70, RES_0_5V, 8, 50}; //
capteursTbl[joyBucket] = {nullptr, 71, RES_0_5V, 8, 100}; //
capteursTbl[joyAccAv1] = {nullptr, 72, RES_0_5V, 10, 350}; //
capteursTbl[joyAccAv2] = {nullptr, 73, RES_0_5V, 8, 450}; //
capteursTbl[joyLift] = {nullptr, 80, RES_0_5V, 8, 200}; //
capteursTbl[joyTilt] = {nullptr, 81, RES_0_5V, 8, 250}; //
capteursTbl[joyAccArr1]= {nullptr, 82, RES_0_5V, 10, 300}; //
capteursTbl[joyAccArr2]= {nullptr, 83, RES_0_5V, 8, 150}; //
capteursTbl[joyGaz] = {nullptr, 90, RES_0_5V, 8, 500}; //
capteursTbl[joyAvance] = {nullptr, 91, RES_0_5V, 10, 400}; //
capteursTbl[joyInching]= {nullptr, 92, RES_0_5V, 10, 0}; //
capteursTbl[PotiDiff1] = {nullptr, 0, RES_0_5V, 10, 550}; //
capteursTbl[PotiDiff2] = {nullptr, 0, RES_0_5V, 10, 600}; //
capteursTbl[PotiGaz2] = {nullptr, 93, RES_0_5V, 10, 650}; //
// capteursTbl[pWR_Ca] = {nullptr, 0, CAPTEUR_NON_DEFINI, 0, 700}; //
// ...
/* //// Pour les <OneWire> : DS18B20 ////
#define DS_READ_TIME 190 //en[ms] Temps nécessaire pour la convertion d'un DS18B20 (selon nbr bits configuré)
// Résolution (bits) Temps d'acquisition (ms) Facteur de conversion (°C par bit)
// 9 bits 93.75 0.5 // Pour DS18S20, toujours 9bits
//=> 10 bits 187.5 0.25
// 11 bits 375 0.125
// 12 bits 750 0.0625
#define DS_RESTART_TIME 1000 //en[ms] Espacement entre deux lectures de la température (ds_sensors[])
#define MAX_DS_SENSORS 0 // Nombre maximum de capteurs sur le port <OneWire>
struct ST_DsSensor {
int16_t temperature; //en[1/100ème °C] Température
uint32_t startTime;
uint32_t nextStartTime;
bool readyRead;
bool conversionStarted;
uint8_t indexCapt;
uint8_t addresse[8]; // Adresse du capteur
};
ST_DsSensor ds_sensors[MAX_DS_SENSORS]; // Tableau de capteurs
uint8_t dsSensCount = 0; // Compteur de capteurs trouvés
*/
//END Constante ////////////////////////////////////////
//Variables
//Moteur thermique
volatile uint8_t NbPntRpm = 0; // Compteur d'impulsions compte-tour moteur thermique
uint16_t SrpmMin = rpmMin; // Consigne de rotation minimum par minutte du moteur thérmique (gaz à main)
uint32_t lastRpmTime = millis();
// uint16_t rpm = 0; // Rotation par minutte du moteur thérmique (et de la pompe)
// int16_t Fth = 0; // position actuel du papion des gaz en [%]
// int16_t KPth = 0; // facteur de correction de la position de consigne du papion des gaz en fonction de la charge mécanique
int8_t Cth = 0; // Consigne de positionnement du papion des gaz (avant correction de puissance et débit) en [%]
int8_t Sth = 0; // Consigne de positionnement du papion des gaz (APRES correction de puissance et débit) en [%]
int8_t IPth = 0; // Correction de Cth selon le débit de la pompe
int16_t KPth = 0; // facteur de correction de la position de consigne du papion des gaz en fonction de la charge mécanique [+/-%]
int8_t Nth = 100; // Charge actuel du moteur thermique en [%] (~~ RPM / position du papion des gaz) 100% le moteur et au régime correspondant à Fth, > 100% le moteur penne à atteindre le régime
uint16_t Pth = 0; // Puissance théorique du moteur thermique en [W]
//Transmission
int16_t angleDir = 0; // Angle de la direction en [1/10°]
uint8_t ModeWhell = 1; // Mode de conduite: 1 = vitesse en fonction de la pédale des gaz
int8_t diffAv = 0; // en [%]
//Pompe hydraulique
int16_t redPhy = 100; // Facteur de correction du débit hydraulique selon la charge moteur
int8_t Nhy = 100; // Charge actuel de la correction LS
uint16_t Phy = 0; // Puissance théorique de l'hydraulique en [W]
uint16_t Qav = 0; // Débit hydraulique utilisé par la partie avant en [dL/min] (sans la direction débit théorique)
uint16_t QavCons = 0; // Débit hydraulique de consigne pour la partie avant en [dL/min] (sans la direction)
uint16_t QavNext = 0; // Débit hydraulique de consigne avant moyennage pour la partie avant en [dL/min] (sans la direction)
uint16_t Qarr = 0; // Débit hydraulique utilisé par la partie arrière en [dL/min] (débit théorique)
uint16_t QarrCons = 0; // Débit hydraulique de consigne pour la partie avant en [dL/min] (sans la direction)
uint16_t ΔQavNext = 0; // Débit hydraulique de consigne avant moyennage - Qav pour la partie av [dL/min] (Pour prédire l'auguementation de la puissance hydraulique)
uint16_t ΔQarrNext = 0; // Débit hydraulique de consigne avant moyennage - Qarr pour la partie arr [dL/min] (Pour prédire l'auguementation de la puissance hydraulique)
//uint16_t ΔQtotNext = 0; // Débit hydraulique de consigne avant moyennage - Qarr pour la partie arr [dL/min] (Pour prédire l'auguementation de la puissance hydraulique)
uint16_t Qdir = 0; // Débit hydraulique réservé pour la direction en [dL/min] (débit théorique)
uint16_t QdirDef = 50; // Débit hydraulique par défaut pour la direction en [dL/min]
uint16_t Qtot = Qav + Qarr + Qdir; // Débit hydraulique total utilisé en [dL/min] (débit théorique)
uint16_t Qpompe = rpm * vPompe / 100; // Débit hydraulique maximum au régime moteur actuel en [dL/min]
uint16_t Qmax = rpmMax * vPompe / 100; // Débit hydraulique maximum au régime moteur Maximum en [dL/min]
//Autre variables
float facteurTime = 1.0;
int8_t steepLoop = 0; // Steep actuel dans la loop
int32_t lastSteepLoop = millis(); // = mllis() lors du dernier steep dans la loop
//END Variables ///////////////////////////////////////
//// TACHES ////////////////////////////
// Tâche pour gérer le timing entre les mesures des capteurs
volatile bool OnCapteursTimeSet = false; // Variable pour suivre l'état de la tâche CapteursTimeSet
void TimerCapteurs(void *pvParameters) { // Créer une tâche pour lire les capteurs sur le coeur disponible
while (true) {
if (!OnCapteursTimeSet) { // [OnCapteursTimeSet] évite de relancer la tâche avant que l'exécution précédante soit finie !
OnCapteursTimeSet = true; // Indiquer que la tâche est en cours
xTaskCreate(
CapteursTimeSet, // Fonction de la tâche
"CapteursTask", // Nom de la tâche
2048, // Taille de la pile (en octets)
NULL, // Paramètre d'entrée de la tâche
2, // Priorité de la tâche
NULL // Gestionnaire de la tâche
);
vTaskDelay(pdMS_TO_TICKS(int(8.0 * facteurTime))); // Attendre 8 millisecondes
// un temps de 50ms (20Hz) est un temps de latence considéré comme non percéptible par l'opérateur. Avec un relevé toutes les 8ms et une moyenne des 8 dernières valeurs la latence devrais pas être percéptible
} else vTaskDelay(pdMS_TO_TICKS(3));
}
}
// Tâche pour lire les capteurs et mettre à jour les moyennes (Lancer par [TimerCapteurs] sur le coeur disponible)
void CapteursTimeSet(void *pvParameters) {
Moyenneur::CapteursSetPinsAuto(); // Appeler la fonction pour mettre à jour les Moyenneurs (Fonction configurées dans "Moyenneur.h")
bool readAutreCapteur(){} // Lire les capteur non setPinsAuto
// SendCanDataJoy(); // Fonction pour envoyer la position des joystick via le CanBus
bool calcDistrib(){} // Calculer les valeurs à envoyer aux distributeurs
bool sendDistrib(){} // Fonction pour envoyer leur position aux distributeurs
vTaskDelay(pdMS_TO_TICKS(1)); // Pour la mise à jour du watchDog
OnCapteursTimeSet = false;
vTaskDelete(NULL); // Terminer cette tâche après l'exécution
}
//// SETUP //////////////////////////////
void setup() {
#ifdef DEBUG
Serial.begin(115200);
#endif
Wire.begin(); // Initialiser le bus I2C
// Initialisation de la carte SD
if (!SD.begin(SD_CS)) {
return; //Serial.println("Erreur d'initialisation de la carte SD");
}
// Initialisation du module CAN
if (CAN.begin(MCP_STDEXT, CAN_500KBPS, MCP_8MHZ) != CAN_OK) { //if (CAN.begin(500E3) != CAN_OK) {
return; //Serial.println("Erreur d'initialisation du CAN");
}
// Initialisation des ADS1115
ads1.begin(0x48); //Adafruit_ADS1115 ads1(0x48);
ads2.begin(0x49); //Adafruit_ADS1115 ads2(0x49);
ads3.begin(0x4A); //Adafruit_ADS1115 ads3(0x4A); //Serial.println("ADS1115 initialisés");
// Initialisation des Sondes de température DS_18B20
//J'utilise plus la lib:DsTemperature // DsTemperature.begin(); // Initialiser les capteurs
// Vérifier si CODE_EEPROM correspond à l'Eeprom installé si non l'Eeprom désactivée
#if defined(CODE_EEPROM) && defined(addrAT24C256)
// Lire le code stocké dans l'EEPROM
uint8_t codeStockeInt = EepromI2Cread(2);
if (CODE_EEPROM != codeStockeInt) { // Comparer le code défini avec le code stocké
#undef CODE_EEPROM //supprime la définition de CODE_EEPROM si l'Eeprom est invalide
#ifdef DEBUG
Serial.println("Le code EEPROM est différent, l'EEPROM est invalide.");
#endif
}
else {
#ifdef DEBUG
Serial.println("Le code EEPROM est correct.");
#endif
}
#ifdef DEBUG
Serial.print("Code stocké dans l'EEPROM : "); // Afficher le code stocké
Serial.println(codeStockeInt);
#endif
#else
#undef CODE_EEPROM //supprime la définition de CODE_EEPROM si l'Eeprom est invalide
#ifdef DEBUG
Serial.println("CODE_EEPROM n'est pas défini. Aucune opération EEPROM effectuée.");
#endif
#endif
//Contrôle avant démarage
// Initialiser le minimum pour contrôler si le MCU redémare après une micro-coupure et ajire en conséquence
bool microcoupure = false;
if (microcoupure){ aFaire();
//autoAllAnalogMoyen(true);
aFaire(/*Alarme microcoupure*/);
}
else aFaire(); // si non démarrage standart
//else autoAllAnalogMoyen(true); //remplissage des tableaux
//DefVariablesAnalogique();
//...
{ // Tâche qui configure le minimum en cas de micro-coupure pour que la machine ce bloque au minimum.
bool _bloquage = true;
bool _speedStart = microcoupure;
microcoupure = true;
/*Tâche qui fait le minimum si _speedStart avant de débloquer :*/_bloquage = false; /* puis fait le reste pendant que le programme continu. à la fin elle tourne micro-coupure à false pour indiquer quelle a fini.*/
xTaskCreatePinnedToCore([](void*) {
// Enumération des objets "Moyenneur"
enum lesCapteur_Ca {
Fth,
pPumphy,
pls,
ENUM_COUNT // Utilisé pour obtenir le nombre d'états
};
// Variation entre un démarrage rapide ou standrat
if (_speedStart){ // Configuration spécifique à un démarrage rapide (_speedStart)
// Aquerrire Fth en abrégé= (valeur du pin) * (une valeur ampirique représentant l'échelle étalon par défaut) * (mapage selon unitée final)
Fth_Ca.setSpeedStartValue(map(int16_t(getValPin() * 1),-32768,32767,-100,100));
// Cth et Sth doivent être égale à Fth
Cth = Fth_Ca.getLastValue();
Sth = Fth_Ca.getMoyen();
// Gérer les pressions et calcul de charge;
//...
//Fin de configuration spécifique à _speedStart
_bloquage = false; // Débloque l'exécution du Setup
// Choses à remettres à jour après débloquage si démarrage _speedStart
}
else {
// Chose spécifique à !_speedStart remplacant celle du démarrage rapide
}
// Initialisation des objets "Moyenneur" est de leurs options
int _posEEprom = Fth_Ee;
Moyenneur* ObjetTraite = nullptr; // Pointeur pour l'objet en cours de traitement
for (int _k=0; _k<ENUM_COUNT; _k++){
switch (_k) {
case Fth:
ObjetTraite = &Fth_Ca;
_posEEprom = Fth_Ee;
break;
case pPumphy:
ObjetTraite = &pPumphy_Ca;
_posEEprom = pPumphy_Ee;
break;
case pls:
ObjetTraite = &pls_Ca;
_posEEprom = pls_Ee;
break;
default:
#ifdef DEBUG
Serial.println("État inconnu");
#endif
continue; // Passer à l'itération suivante si l'état est inconnu
}
byte _configEeCapteur = EepromI2Cread(_posEEprom);
if (bitRead(_configEeCapteur,0) == 1){
_posEEprom += 10;
if (bitRead(_configEeCapteur,1) == 1){ // le capteur conteint des valeurs de contrôle
int16_t _min = EepromI2CreadInt16(_posEEprom += 2);
int16_t _max = EepromI2CreadInt16(_posEEprom += 2);
int16_t _def = EepromI2CreadInt16(_posEEprom += 2);
ObjetTraite->setCapteurControle(_min, _max, _def);
}
if (bitRead(_configEeCapteur,2) == 1){ // Le capteur conteint des valeurs d'échelle étalons
uint8_t _size = EepromI2Cread(_posEEprom ++);
if (_size > 0){
std::vector<int16_t> _ref(_size); // Utilisation de std::vector pour les tableaux
std::vector<int16_t> _etalon(_size);
for (int _i = 0; _i < _size; _i++) {
_ref[_i] = EepromI2CreadInt16(_posEEprom += 2);
_etalon[_i] = EepromI2CreadInt16(_posEEprom += 2);
}
ObjetTraite->setCapteurScale(_ref.data(), _etalon.data(), _size);
}
}
ObjetTraite->addValPin();
}
}
//...
microcoupure = false;
if (!_speedStart) _bloquage = false; // avec _speedStart ce drapeau aura déjà disparu
// La tâche à exécuter tous ce qu'elle avait à faire
vTaskDelete(NULL); // Supprime la tâche une fois terminée
}, // Fonction de la tâche
"SetupServer", // Nom de la tâche
2048, // Taille de la pile
NULL, // Paramètres de la tâche
1, // Priorité de la tâche
NULL, // Handle de la tâche créée
1); // S'exécute sur le coeur1 avec priorité 0
while(!_bloquage);
// continue le Setup
}
// Initialisation des variables depuis la meéoire Eeprom
#ifdef CODE_EEPROM // si l'Eeprom n'est pas définie correctement, les variables garderons les valeurs par défaut du programme
if (!loadEepromVal()) {
#undef CODE_EEPROM //supprime la définition de CODE_EEPROM si l'Eeprom est invalide
}
#endif
// Initialisation des capteurs et recepteurs
// Plus utiliser ==>InitAnalogMoyen();
// Initialisation des autres capteurs
aFaire(); //Ajouter les capteurs de température
// Charger les data de démarrage depuis les autres modules
aFaire(); // contrôler si le bloc arrière n'a pas envoyer de message
// send_CAN_je-démarre
// Attendre la raiponce du contrôleur arrière
// traiter la raiponce du bloc arrière
// PINs init (Les pins utilisés par "Moyenneur" sont déclarer par la librairie)
pinMode(rpm_In, INPUT_PULLUP); // Configure le pin en entrée avec pull-up
attachInterrupt(digitalPinToInterrupt(rpm_In), RpmPulse, RISING); // Interruption sur front montant
pinMode(pinRelaisContact, OUTPUT);
if (Cth_PIN > 0 && Cth_PIN < 70) pinMode(Cth_PIN, OUTPUT);
else if(Cth_PIN > 200) ledcAttach(Cth_PIN-200, pwmFrequency, pwmResolution); //valeur PWM (Broche, fréquence en Hz, résolution en bits)
if (diffC_PIN > 0)pinMode(Cth_Dir_PIN, OUTPUT);
if (diffC_PIN > 0 && diffC_PIN < 70) pinMode(diffC_PIN, OUTPUT);
else if(diffC_PIN > 200) ledcAttach(diffC_PIN-200, pwmFrequency, pwmResolution);
if (diffC_PIN > 0)pinMode(diffC_Dir_PIN, OUTPUT);
if (diffAr_PIN > 0 && diffAr_PIN < 70) pinMode(diffAr_PIN, OUTPUT);
else if(diffAr_PIN > 200) ledcAttach(diffAr_PIN-200, pwmFrequency, pwmResolution);
if (diffC_PIN > 0)pinMode(diffAr_Dir_PIN, OUTPUT);
if (Onhy_PIN > 0 && Onhy_PIN < 70) pinMode(Onhy_PIN, OUTPUT);
if (Offth_PIN > 0 && Offth_PIN < 70) pinMode(Offth_PIN, OUTPUT);
// if (venthy_PIN > 0 && venthy_PIN < 70) pinMode(venthy_PIN, OUTPUT);
// /*if(PWM)*/else if(venthy_PIN > 200) ledcAttach(venthy_PIN-200, pwmFrequency, pwmResolution);
if (contactLong_PIN > 0 && contactLong_PIN < 70) pinMode(contactLong_PIN, OUTPUT);
digitalWrite(pinRelaisContact, HIGH);
// TACHES FreeRTos
// Créer la tâche de timing
xTaskCreatePinnedToCore( TimerCapteurs, "TimingTask", 2048, NULL, 1, NULL, 1 ); // Coeur1 avec priorité 1
// Créer la tâche pour remplacer loop()
xTaskCreatePinnedToCore(loopFreeRTOS, "loopFreeRTOS", 4096 48, NULL, 0, NULL, 1); // Coeur1 avec priorité 0
// END xTaskCreate
}
/// END SETUP ////////////////////////////
//// STARTING // (LOOP) /////////////////
void loop() {} // La fonction loop() reste vide, elle est remplacé par loopFreeRTOS()
void loopFreeRTOS(void *pvParameters){ while (true) { // Fonction pour exécuter le code de loop() en boucle
// Lire les capteurs
/* 2 : Moteur thermique
Gère : calcul la charge du moteur thermique et la charge hydraulique
L'aquisition du régime moteur thermique et des pressions hydrauliques
La position de l'accélérateur (papion des gaz) du moteur thermique
Pilote le bloquage de différentiel CENTRAL et ARRIERE
Relève les valeurs des contrôleurs de la partie arrière, sauf les joystick de pilotage hydraulique
Divers de la partie arrière
*/
// pls_Ca.setPin(); // Appeler la fonction pour mettre à jour les Moyenneurs qui ont autoSetPin à true
aFaire(); //readCAN();
// Calculer
//vTaskDelay(pdMS_TO_TICKS(100)); // Délai pour éviter une boucle trop rapide
} //END While
} //END loopFreeRTOS
/// END Loop() //////////////////////////
//// FONCTIONS /////////////////////////
// Autres fonctions
// A faire: !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
bool readCAN() {return true;}
bool sendCAN() {return true;}
bool readAutreCapteur(){}
bool calcDistrib(){}
bool sendDistrib(){}
aFaire(); // Ajuster les valeurs des blocages de différanciels
}