From 6032da49675a42f7269d5b26f6e36785e1fa1028 Mon Sep 17 00:00:00 2001 From: Ploush Date: Fri, 17 Jul 2026 13:54:31 +0200 Subject: [PATCH] Add programme --- README.md | 126 +++++++++ drone.lua | 610 ++++++++++++++++++++++++++++++++++++++++++++ lib/calibration.lua | 421 ++++++++++++++++++++++++++++++ lib/conf.lua | 153 +++++++++++ lib/etat.lua | 61 +++++ lib/ihm.lua | 176 +++++++++++++ lib/journal.lua | 42 +++ lib/materiel.lua | 254 ++++++++++++++++++ lib/navigation.lua | 77 ++++++ lib/pid.lua | 61 +++++ lib/pilotage.lua | 135 ++++++++++ 11 files changed, 2116 insertions(+) create mode 100644 README.md create mode 100644 drone.lua create mode 100644 lib/calibration.lua create mode 100644 lib/conf.lua create mode 100644 lib/etat.lua create mode 100644 lib/ihm.lua create mode 100644 lib/journal.lua create mode 100644 lib/materiel.lua create mode 100644 lib/navigation.lua create mode 100644 lib/pid.lua create mode 100644 lib/pilotage.lua diff --git a/README.md b/README.md new file mode 100644 index 0000000..f3b9a51 --- /dev/null +++ b/README.md @@ -0,0 +1,126 @@ +# Quadricoptere Create Aeronautics : controleur centralise + +Nouvelle architecture: un SEUL ordinateur (PC central) pilote tout via +le reseau filaire. Les anciens scripts multi-PC (helice.lua, +central.lua, input.lua, console.lua, propulsion.lua) sont obsoletes. + +## Materiel + +Directement sur le PC central: +- altitude_sensor, Create_DisplayLink (affichage de conduite) +- ender modem (uniquement pour le GPS) +- sortie redstone BOTTOM vers le moteur (15 = arret, 14 = vitesse + minimale, 0 = vitesse maximale) + +Via le reseau filaire (wired modems): +- 6 Create_RotationSpeedController: 4 helices + 2 propulseurs + (roles affectes par `calib rsc`, persistes dans drone.etat) +- 3 velocity_sensor, un par composante (axes affectes par + `calib capteurs`) +- 1 gimbal_sensor, 1 optical_sensor (vers le sol, 15.5 = rien), + 1 stressometer, 1 redstone_relay (joystick), 1 advanced monitor + (tactile), 1 linked typewriter (EN ATTENTE: relever ses signatures + avec test_signatures, voir lib/ihm.lua) + +## Installation + +1. Copier drone.lua sur le PC, puis: `drone maj ` pour + telecharger les modules depuis votre depot (Forgejo: URL "raw" du + dossier). Autoriser le domaine dans la config http de CC:Tweaked. + Ou copier a la main drone.lua + lib/*.lua. +2. Premier lancement: `drone` cree `drone.conf`. RELIRE chaque + parametre (Y_VOL, Y_MINI, ANGLE_MAX, DIST_SOL, offsets, joystick, + moteur...) puis passer `verifie = true`. Un warning s'affiche tant + que ce n'est pas fait. +3. Calibrations depuis le moniteur tactile, DANS CET ORDRE (les + modes non calibres sont verrouilles en rouge). Les calibrations + assistees affichent leurs consignes sur l'ecran de conduite et se + valident en touchant le moniteur: + - `gimbal` (assistee, en premier): incliner le nez vers le bas + puis pencher a gauche; associe les 2 angles bruts au tangage et + au roulis (l'association CHANGE d'un vaisseau a l'autre). + - `joystick` (assistee): pousser a fond dans chaque direction pour + identifier les faces du redstone relay. + - `typew` (assistee, OPTIONNELLE): le typewriter emet des codes + ASCII (qwerty), le mappage est integre par defaut (0..9, + Entree = valider, Backspace = effacer, + / - = consigne +-1). + Ne calibrer que si le clavier a un mappage exotique. + - `rsc` (mode INACTIF, drone pose): impulsion sur chaque RSC + inconnu; l'effet d'assiette designe le coin; sans effet = + propulseur. prop_l / prop_r arbitraires: si un virage gauche + part a droite, les echanger dans drone.etat. + - `capteurs` (en vol): montee, avancee puis translation gauche + identifient l'axe ET la direction de chaque velocity sensor + (repere du drone: avant / lateral / vertical, positif = + avant / gauche / haut). + - `pid` (en vol stationnaire, marge d'altitude): recherche + iterative, critere ZERO depassement + temps de reponse minimal + + assiette <= ANGLE_MAX. EXPERIMENTAL. Toute modification des + parametres de conf dont dependent ces gains (VITESSE_RSC_MAX, + VITESSE_RAMPE, ANGLE_MAX) INVALIDE la calibration au prochain + demarrage (retour aux gains de la conf + alerte). + +## Modes (boutons du moniteur; verrouilles en rouge si une +calibration manque, la raison s'affiche au toucher) + +- `off` : moteur coupe (bottom a 15), tous les RSC a 0 +- `inactif` : moteur ALLUME, tous les RSC a 0, commandes desactivees, + pret pour les calibrations. Dans tous les modes sauf off, une + erreur s'affiche si le moteur ne tourne pas (stressometer a + capacite nulle) alors qu'il le devrait +- `stationnement` : helices a pleine puissance inversee (plaquage), + propulseurs a 0, commandes desactivees. Conditions d'entree: au sol + (optical <= DIST_SOL) ET horizontal (<= ANGLE_MAX). Sortie + automatique vers `vol` si les conditions sont perdues. +- `vol` : assiette maintenue horizontale (PID tangage/roulis a 0), + altitude au PID double regime, deplacement horizontal au joystick + via les PROPULSEURS (avance/virage differentiels) +- `drone` : le joystick commande l'INCLINAISON (jusqu'a + ANGLE_DRONE_MAX), deplacement par les helices, propulseurs a 0 +- `atterrissage` : descente douce (VITESSE_ATTERRISSAGE) jusqu'au + sol, puis stationnement. Sous Y_MINI, la descente exige un sol + POSABLE (optical: bloc detecte, ni eau ni lave): garde-fou ocean +- `auto` : au clavier du PC: `auto `. Decollage a Y_VOL, + ralliement en avancant (cap estime par GPS), atterrissage sur place +- `position` : au clavier: `position `. Rallie le point; + la descente est bloquee si le sol est detecte + +La consigne d'altitude se regle aux boutons +-1/+-10 du moniteur ou +au TYPEWRITER: taper les chiffres (la saisie s'affiche sur l'ecran +de conduite) puis Entree; Backspace corrige; + et - ajustent la +consigne de 1 quand aucune saisie n'est en cours. + +Points nommes (clavier du PC, 10 max): +`pt save ` | `pt go ` | `pt del ` | `pt list` | `rtb` +(retour au point de decollage de la derniere mission auto). + +## Regulation + +- PID altitude: deux jeux de gains (montee / descente), derivee sur + la mesure et integrale conditionnelle (concu pour le + zero-depassement), consigne rampee (VITESSE_RAMPE). +- PID assiette: tangage et roulis vers leurs cibles; mixage par coin: + base + corrections signees. +- Moteur asservi au stressometer: augmente si l'utilisation depasse + STRESS_HAUT, diminue sous STRESS_BAS (economie). Le moteur est + dimensionne pour suffire; en cas de SURCHARGE malgre le maximum, + les PROPULSEURS sont reduits en priorite (facteur 0.3) + alerte, + puis retablis quand le stress redescend. +- Carburant: deux valeurs sur le 2e display link: POURCENTAGE de + reserve (ligne 1) et AUTONOMIE en minutes (ligne 2), estimee par + apprentissage glissant (EMA, CARBURANT.LISSAGE) de la + consommation; les ravitaillements sont exclus de l'apprentissage. + Alerte sous SEUIL_ALERTE. + +## Fichiers + +- `drone.conf` : parametres utilisateur (cree si absent) +- `drone.etat` : mode, consigne, roles RSC, axes capteurs, gains + calibres, point de decollage (restaures au reboot; en vol, la + consigne repart de l'altitude reelle) +- `drone.log` : journal de vol (rotation a 64 Ko) + +## Points d'attention + +- tanks(): si la capacite n'est pas fournie par le jeu, la conf + CARBURANT.CAPACITE_TANK sert de repli. diff --git a/drone.lua b/drone.lua new file mode 100644 index 0000000..3ce5f74 --- /dev/null +++ b/drone.lua @@ -0,0 +1,610 @@ +-------------------------------------------------------------------- +-- drone.lua : controleur central du quadricoptere Create Aeronautics +-- +-- Usage : +-- drone lance le controleur +-- drone maj installe / met a jour depuis un depot +-- +-- Modes : off, inactif, stationnement, vol, drone, atterrissage, +-- auto, position +-- Clavier du PC : auto | position +-- pt save | pt go | pt del | pt list +-- rtb (retour au point de decollage) +-------------------------------------------------------------------- + +local ARGS = { ... } +local BASE = fs.getDir(shell.getRunningProgram()) + +-------------------------------------------------------------------- +-- INSTALLEUR / MISE A JOUR INTEGRE +-------------------------------------------------------------------- +local FICHIERS = { + "drone.lua", + "lib/journal.lua", "lib/conf.lua", "lib/etat.lua", "lib/pid.lua", + "lib/materiel.lua", "lib/pilotage.lua", "lib/navigation.lua", + "lib/ihm.lua", "lib/calibration.lua", +} + +if ARGS[1] == "maj" then + local url = ARGS[2] + if not url then error("usage: drone maj ", 0) end + if not http then error("API http desactivee (config CC:Tweaked)", 0) end + url = url:gsub("/$", "") + for _, fichier in ipairs(FICHIERS) do + write(fichier .. " ... ") + local reponse, err = http.get(url .. "/" .. fichier) + if not reponse then error("echec: " .. tostring(err), 0) end + local contenu = reponse.readAll() + reponse.close() + local chemin = fs.combine(BASE, fichier) + fs.makeDir(fs.getDir(chemin)) + local f = fs.open(chemin, "w") + f.write(contenu) + f.close() + print("ok") + end + print("mise a jour terminee, relancer: drone") + return +end + +-------------------------------------------------------------------- +-- CHARGEMENT DES MODULES +-------------------------------------------------------------------- +local function charger(chemin) + local complet = fs.combine(BASE, chemin) + if not fs.exists(complet) then + error(chemin .. " manquant: lancer 'drone maj '", 0) + end + return dofile(complet) +end + +local journal = charger("lib/journal.lua") +local ModConf = charger("lib/conf.lua") +local conf = ModConf.charger(journal) +local Etat = charger("lib/etat.lua") +local etat = Etat.charger(journal) +local Pid = charger("lib/pid.lua") +local materiel = charger("lib/materiel.lua").initialiser(conf, etat, journal) + +-- invalidation de la calibration PID si la conf a change +local empreinte = ModConf.empreintePid(conf) +if etat.pid and etat.empreintePid ~= empreinte then + journal.alerte("conf modifiee: gains PID calibres INVALIDES, " + .. "retour aux gains de la conf (relancer calib pid)") + etat.pid = nil +end + +local pilotage = charger("lib/pilotage.lua") + .nouveau(conf, etat, materiel, Pid, journal) +local navigation = charger("lib/navigation.lua") + .nouveau(conf, materiel, journal) +local Calibration = charger("lib/calibration.lua") +local ihm = charger("lib/ihm.lua").nouveau(conf, materiel, journal) + +os.setComputerLabel("PC_central") + +-------------------------------------------------------------------- +-- ETAT DE VOL +-------------------------------------------------------------------- +local DT = 0.1 +local mode = etat.mode or "off" +local consigneY = etat.consigneY or materiel.lireAltitude() +local cibleNav = nil +local phaseAuto = nil +local stress = nil +local carburantPct, autonomieMin = nil, nil +local saisieTypewriter = "" +local enCalibration = false +local alerteMoteurA = 0 + +local function sauverEtat() + etat.mode, etat.consigneY = mode, consigneY + Etat.sauver(etat) +end + +-------------------------------------------------------------------- +-- VERROUS: modes indisponibles tant que des calibrations manquent +-------------------------------------------------------------------- +local function verrouDe(m) + local gimbalOk = etat.gimbal ~= nil + local helicesOk = #materiel.helicesManquantes() == 0 + local joystickOk = etat.joystick ~= nil + + if m == "off" or m == "inactif" then return nil end + if not gimbalOk then return "calib gimbal requise" end + if m == "stationnement" then return nil end + if not helicesOk then return "calib rsc requise" end + if (m == "vol" or m == "drone") and not joystickOk then + return "calib joystick requise" + end + if (m == "auto" or m == "position") and not materiel.lirePosition() then + return "pas de GPS" + end + return nil +end + +local function verrous() + local v = {} + for _, m in ipairs({ "off", "inactif", "stationnement", "vol", + "drone", "atterrissage", "auto", "position" }) do + v[m] = verrouDe(m) + end + return v +end + +-------------------------------------------------------------------- +-- CHANGEMENTS DE MODE +-------------------------------------------------------------------- +local function changerMode(nouveau, raison) + if nouveau == mode then return end + + local verrou = verrouDe(nouveau) + if verrou then + journal.alerte(nouveau .. " refuse: " .. verrou) + ihm.message(nouveau .. ": " .. verrou) + return + end + if nouveau == "stationnement" + and not (materiel.auSol() and materiel.horizontal()) then + journal.alerte("stationnement refuse: au sol et horizontal requis") + ihm.message("stationnement: sol+plat requis") + return + end + + mode = nouveau + pilotage.raz() + navigation.razCap() + + if mode == "vol" or mode == "drone" then + consigneY = materiel.lireAltitude() + elseif mode == "auto" then + phaseAuto = "decollage" + local x, y, z = materiel.lirePosition() + etat.decollage = { x = x, y = y, z = z } + consigneY = conf.Y_VOL + elseif mode == "off" then + pilotage.arreter() + end + + journal.info("mode: " .. mode .. (raison and (" (" .. raison .. ")") or "")) + sauverEtat() +end + +-------------------------------------------------------------------- +-- DESCENTE SECURISEE (garde-fou ocean) +-- La descente sous Y_MINI exige un sol POSABLE detecte par +-- l'optical sensor; sinon: maintien a Y_MINI + alerte. +-------------------------------------------------------------------- +local function descendre() + local cible = consigneY - conf.VITESSE_ATTERRISSAGE * DT + if cible < conf.Y_MINI and not materiel.solPosable() then + ihm.message("sol non posable: maintien Y_MINI") + return math.max(consigneY, conf.Y_MINI) + end + return cible +end + +-------------------------------------------------------------------- +-- UN PAS DE CONTROLE PAR MODE +-------------------------------------------------------------------- +local function pasControle() + if enCalibration then return end + + -- alerte moteur: dans tous les modes ou il devrait tourner + if mode ~= "off" and not materiel.moteurTourne() then + if os.clock() - alerteMoteurA > 5 then + alerteMoteurA = os.clock() + journal.erreur("le moteur ne tourne pas alors qu'il le devrait") + ihm.message("ERREUR: MOTEUR ARRETE", 5) + end + end + + if mode == "off" then + pilotage.arreter() + + elseif mode == "inactif" then + -- moteur allume, tout a zero, pret pour les calibrations + materiel.toutArreter() + + elseif mode == "stationnement" then + if not (materiel.auSol() and materiel.horizontal()) then + changerMode("vol", "conditions de stationnement perdues") + return + end + pilotage.plaquer() + + elseif mode == "vol" then + local avance, virage = materiel.lireJoystick() + pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, + avance, virage, DT) + + elseif mode == "drone" then + local avance, virage = materiel.lireJoystick() + pilotage.reguler(consigneY, { + tangage = avance * conf.ANGLE_DRONE_MAX, + roulis = -virage * conf.ANGLE_DRONE_MAX, + }, 0, 0, DT) + + elseif mode == "atterrissage" then + if materiel.auSol() then + changerMode("stationnement", "sol atteint") + return + end + consigneY = descendre() + pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, 0, 0, DT) + + elseif mode == "auto" then + if phaseAuto == "decollage" then + local alt = pilotage.reguler(conf.Y_VOL, + { tangage = 0, roulis = 0 }, 0, 0, DT) + if math.abs(alt - conf.Y_VOL) <= 0.5 then + phaseAuto = "croisiere" + journal.info("auto: croisiere vers la cible") + end + elseif phaseAuto == "croisiere" then + local avance, virage, distance = navigation.rallier( + cibleNav.x, cibleNav.z) + if not avance then + changerMode("vol", "GPS perdu") + return + end + pilotage.reguler(conf.Y_VOL, { tangage = 0, roulis = 0 }, + avance, virage, DT) + if distance <= conf.NAV.SEUIL_ARRIVEE then + phaseAuto = "atterrissage" + consigneY = materiel.lireAltitude() + journal.info("auto: cible atteinte, atterrissage") + end + else + if materiel.auSol() then + changerMode("stationnement", "mission terminee") + return + end + consigneY = descendre() + pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, 0, 0, DT) + end + + elseif mode == "position" then + local avance, virage = navigation.rallier(cibleNav.x, cibleNav.z) + if not avance then + changerMode("vol", "GPS perdu") + return + end + local cibleY = cibleNav.y + if cibleY < materiel.lireAltitude() then + -- descente limitee par la presence d'un sol + if materiel.lireDistanceSol() <= conf.DIST_SOL + 0.5 then + cibleY = materiel.lireAltitude() + elseif cibleY < conf.Y_MINI and not materiel.solPosable() then + cibleY = conf.Y_MINI + end + end + pilotage.reguler(cibleY, { tangage = 0, roulis = 0 }, + avance, virage, DT) + end +end + +-------------------------------------------------------------------- +-- CALIBRATIONS +-------------------------------------------------------------------- +local ui = { + inviter = function(texte) + ihm.message(texte, 3600) + pcall(ihm.rafraichirConduite, + { mode = mode, consigneY = consigneY }) + journal.info("[calib] " .. texte) + end, + attendreToucher = ihm.attendreToucher, +} + +local function lancerCalibration(quoi) + enCalibration = true + local ok, err = pcall(function() + if quoi == "gimbal" then + Calibration.gimbal(conf, etat, materiel, journal, Etat, ui) + elseif quoi == "joystick" then + Calibration.joystick(conf, etat, materiel, journal, Etat, ui) + elseif quoi == "typew" then + Calibration.typewriter(conf, etat, materiel, journal, Etat, ui) + elseif quoi == "rsc" then + if mode ~= "inactif" then + journal.alerte("calib rsc: passer en mode inactif d'abord") + else + Calibration.rsc(conf, etat, materiel, journal, Etat, ui) + end + elseif quoi == "capteurs" or quoi == "pid" then + if mode ~= "vol" then + journal.alerte("calib " .. quoi .. ": passer en mode vol d'abord") + elseif quoi == "pid" then + Calibration.pid(conf, etat, materiel, pilotage, journal, Etat, + empreinte) + else + local function tenir(duree, angles, avance) + local cible = materiel.lireAltitude() + return function() + for _ = 1, math.floor(duree / DT) do + pilotage.reguler(cible, angles, avance or 0, 0, DT) + sleep(DT) + end + end + end + Calibration.capteurs(conf, etat, materiel, journal, Etat, ui, { + monter = function() + local cible = materiel.lireAltitude() + 3 + for _ = 1, 30 do + pilotage.reguler(cible, { tangage = 0, roulis = 0 }, + 0, 0, DT) + sleep(DT) + end + end, + avancer = tenir(3, { tangage = 0, roulis = 0 }, 0.6), + gauche = tenir(3, + { tangage = 0, roulis = -conf.ANGLE_DRONE_MAX / 2 }, 0), + }) + end + end + end) + if not ok then journal.erreur("calibration: " .. tostring(err)) end + ihm.message("", 0) + enCalibration = false +end + +-------------------------------------------------------------------- +-- POINTS NOMMES / RETOUR DECOLLAGE +-------------------------------------------------------------------- +local function nbPoints() + local n = 0 + for _ in pairs(etat.points) do n = n + 1 end + return n +end + +local function commandePoint(mots) + if mots[2] == "save" and mots[3] then + if nbPoints() >= conf.NAV.MAX_POINTS and not etat.points[mots[3]] then + print(("maximum %d points"):format(conf.NAV.MAX_POINTS)) + return + end + local x, y, z = materiel.lirePosition() + if not x then print("pas de GPS") return end + etat.points[mots[3]] = { x = x, y = y, z = z } + Etat.sauver(etat) + print(("point '%s' enregistre (%.0f %.0f %.0f)"):format(mots[3], x, y, z)) + elseif mots[2] == "go" and mots[3] then + local p = etat.points[mots[3]] + if not p then print("point inconnu") return end + cibleNav = { x = p.x, z = p.z } + changerMode("auto", "pt go " .. mots[3]) + elseif mots[2] == "del" and mots[3] then + etat.points[mots[3]] = nil + Etat.sauver(etat) + print("point supprime") + elseif mots[2] == "list" then + for nom, p in pairs(etat.points) do + print((" %s : %.0f %.0f %.0f"):format(nom, p.x, p.y, p.z)) + end + else + print("pt save | pt go | pt del | pt list") + end +end + +-------------------------------------------------------------------- +-- TACHES PARALLELES +-------------------------------------------------------------------- +local function tacheControle() + while true do + local ok, err = pcall(pasControle) + if not ok then + journal.erreur("controle: " .. tostring(err)) + pilotage.arreter() + end + sleep(DT) + end +end + +-- moteur asservi au stressometer; surcharge: reduction propulseurs +local function tacheMoteur() + local niveau = 8 + local enSurcharge = false + while true do + if mode == "off" then + materiel.reglerMoteur(0) + else + stress = materiel.lireStress() + if stress then + if stress > conf.MOTEUR.STRESS_HAUT and niveau < 15 then + niveau = niveau + 1 + elseif stress < conf.MOTEUR.STRESS_BAS and niveau > 1 then + niveau = niveau - 1 + end + -- surcharge malgre moteur au maximum: delester les propulseurs + if stress > conf.MOTEUR.SURCHARGE and niveau >= 15 then + if not enSurcharge then + enSurcharge = true + journal.alerte("surcharge de stress: propulseurs reduits") + ihm.message("SURCHARGE: propulseurs reduits", 10) + end + pilotage.reglerFacteurProp(0.3) + elseif enSurcharge and stress < conf.MOTEUR.STRESS_HAUT then + enSurcharge = false + pilotage.reglerFacteurProp(1.0) + journal.info("surcharge resorbee") + end + else + niveau = 15 + end + materiel.reglerMoteur(niveau) + end + sleep(conf.MOTEUR.PERIODE) + end +end + +-- carburant: pourcentage de reserve + autonomie estimee par +-- APPRENTISSAGE GLISSANT (EMA) de la consommation. Les hausses de +-- quantite (ravitaillement) sont exclues de l'apprentissage. +local function tacheCarburant() + local quantitePrec, tPrec = nil, nil + local consoLissee = nil -- mB/s apprise + while true do + local quantite, capacite = materiel.lireCarburant() + if quantite then + carburantPct = quantite / capacite + local t = os.clock() + + if quantitePrec and t > tPrec then + local conso = (quantitePrec - quantite) / (t - tPrec) + if conso >= 0 then -- conso negative = ravitaillement: ignore + local a = conf.CARBURANT.LISSAGE + consoLissee = consoLissee + and (a * conso + (1 - a) * consoLissee) + or conso + end + end + quantitePrec, tPrec = quantite, t + + autonomieMin = nil + if consoLissee and consoLissee > 0 then + autonomieMin = quantite / consoLissee / 60 + end + + if carburantPct <= conf.CARBURANT.SEUIL_ALERTE then + ihm.message("CARBURANT BAS", 4) + end + pcall(ihm.rafraichirCarburant, carburantPct, autonomieMin) + end + sleep(conf.CARBURANT.PERIODE) + end +end + +-- typewriter: saisie de la consigne d'altitude (scrutation des +-- codes ASCII, clavier qwerty). Mappage par defaut integre; la +-- calibration 'typew' (etat.typewriter) le remplace si presente. +-- 0..9 chiffres de la saisie +-- Entree valider la saisie comme nouvelle consigne +-- Backspace effacer le dernier chiffre +-- + / - ajuster la consigne de +-1 (hors saisie en cours) +local ASCII = { + [13] = "valider", [10] = "valider", + [8] = "effacer", [127] = "effacer", + [43] = "plus", [45] = "moins", +} +for code = 48, 57 do ASCII[code] = string.char(code) end + +local function tacheTypewriter() + local pressees = {} + while true do + if not enCalibration then + local courantes = {} + for _, code in ipairs(materiel.lireTouches()) do + courantes[code] = true + if not pressees[code] then + local symbole = (etat.typewriter or ASCII)[code] + if symbole == "valider" then + local valeur = tonumber(saisieTypewriter) + if valeur then + consigneY = valeur + journal.info(("typewriter: consigne %d"):format(valeur)) + sauverEtat() + end + saisieTypewriter = "" + elseif symbole == "effacer" then + saisieTypewriter = saisieTypewriter:sub(1, -2) + elseif symbole == "plus" and #saisieTypewriter == 0 then + consigneY = consigneY + 1 + sauverEtat() + elseif symbole == "moins" and #saisieTypewriter == 0 then + consigneY = consigneY - 1 + sauverEtat() + elseif symbole and #symbole == 1 then + saisieTypewriter = (saisieTypewriter .. symbole):sub(1, 6) + end + end + end + pressees = courantes + end + sleep(0.1) + end +end + +local function tacheIhm() + local prochainDessin = 0 + while true do + if os.clock() >= prochainDessin and not enCalibration then + local px, py, pz = materiel.lirePosition() + pcall(ihm.rafraichirConduite, { + mode = mode, consigneY = consigneY, saisie = saisieTypewriter, + }) + pcall(ihm.rafraichirMoniteur, { + mode = mode, + consigneY = consigneY, + stress = stress, + moteurOk = mode == "off" or materiel.moteurTourne(), + gimbalOk = etat.gimbal ~= nil, + carburantPct = carburantPct, + minutes = autonomieMin, + position = px and { x = px, y = py, z = pz } or nil, + points = nbPoints(), + verrous = verrous(), + }) + prochainDessin = os.clock() + 0.5 + end + + local minuteur = os.startTimer(0.5) + local ev, a, b, c = os.pullEvent() + if ev == "monitor_touch" and not enCalibration then + local action = ihm.traiterToucher(b, c) + if action then + if action.type == "mode" then + if action.verrou then + ihm.message(action.valeur .. ": " .. action.verrou) + elseif action.valeur == "auto" or action.valeur == "position" then + ihm.message("cible au clavier: " .. action.valeur .. " x [y] z") + else + changerMode(action.valeur, "moniteur") + end + elseif action.type == "delta" then + consigneY = consigneY + action.valeur + sauverEtat() + elseif action.type == "calib" then + lancerCalibration(action.valeur) + end + end + end + if ev ~= "timer" or a ~= minuteur then os.cancelTimer(minuteur) end + end +end + +local function tacheClavier() + while true do + local ligne = read() + local mots = {} + for mot in ligne:gmatch("%S+") do table.insert(mots, mot) end + if mots[1] == "auto" and tonumber(mots[2]) and tonumber(mots[3]) then + cibleNav = { x = tonumber(mots[2]), z = tonumber(mots[3]) } + changerMode("auto", "clavier") + elseif mots[1] == "position" and tonumber(mots[2]) + and tonumber(mots[3]) and tonumber(mots[4]) then + cibleNav = { x = tonumber(mots[2]), y = tonumber(mots[3]), + z = tonumber(mots[4]) } + changerMode("position", "clavier") + elseif mots[1] == "pt" then + commandePoint(mots) + elseif mots[1] == "rtb" then + if etat.decollage then + cibleNav = { x = etat.decollage.x, z = etat.decollage.z } + changerMode("auto", "retour decollage") + else + print("pas de point de decollage enregistre") + end + elseif mots[1] then + print("auto | position | pt ... | rtb") + end + end +end + +journal.info(("drone: demarrage en mode %s (consigne %.0f)") + :format(mode, consigneY)) +if mode == "vol" or mode == "drone" then + consigneY = materiel.lireAltitude() +end +parallel.waitForAny(tacheControle, tacheMoteur, tacheCarburant, + tacheTypewriter, tacheIhm, tacheClavier) diff --git a/lib/calibration.lua b/lib/calibration.lua new file mode 100644 index 0000000..4b85e04 --- /dev/null +++ b/lib/calibration.lua @@ -0,0 +1,421 @@ +-------------------------------------------------------------------- +-- lib/calibration.lua : calibrations du drone. +-- Les calibrations assistees utilisent `ui` fourni par drone.lua : +-- ui.inviter(texte) affiche l'invite sur l'ecran de conduite +-- ui.attendreToucher() attend un toucher du moniteur (validation) +-- +-- gimbal : association des 2 angles bruts aux axes tangage / +-- roulis + signes, PROPRE A CHAQUE VAISSEAU. Assistee: +-- l'utilisateur incline le drone nez bas puis a gauche. +-- joystick : identification des faces du redstone relay. Assistee. +-- typewriter : apprentissage des codes de touches. Assistee. +-- rsc : affectation des RSC par impulsions (mode inactif, au +-- sol, EXIGE gimbal calibre). +-- capteurs : axes ET directions des velocity sensors, par +-- mouvements commandes (en vol). +-- pid : gains d'altitude, zero depassement (en vol). +-------------------------------------------------------------------- +local Calibration = {} + +local function copierGains(g) + return { kp = g.kp, ki = g.ki, kd = g.kd } +end + +-------------------------------------------------------------------- +-- GIMBAL (assistee, drone au sol ou stable) +-------------------------------------------------------------------- +function Calibration.gimbal(conf, etat, materiel, journal, Etat, ui) + local seuil = conf.CALIB.IMPULSION_SEUIL + + ui.inviter("Drone a plat, touchez l'ecran") + ui.attendreToucher() + local x0, z0 = materiel.lireAnglesBruts() + + ui.inviter("Inclinez NEZ vers le BAS, touchez") + ui.attendreToucher() + local x1, z1 = materiel.lireAnglesBruts() + local dx, dz = x1 - x0, z1 - z0 + if math.max(math.abs(dx), math.abs(dz)) < seuil then + journal.alerte("calib gimbal: inclinaison insuffisante, abandon") + ui.inviter("Echec: inclinaison trop faible") + return false + end + local indexTangage = (math.abs(dx) >= math.abs(dz)) and 1 or 2 + local deltaT = (indexTangage == 1) and dx or dz + -- convention: nez bas => tangage normalise NEGATIF + local signeTangage = (deltaT < 0) and 1 or -1 + + ui.inviter("Revenez a plat, touchez") + ui.attendreToucher() + x0, z0 = materiel.lireAnglesBruts() + + ui.inviter("Penchez a GAUCHE, touchez") + ui.attendreToucher() + x1, z1 = materiel.lireAnglesBruts() + local indexRoulis = (indexTangage == 1) and 2 or 1 + local deltaR = (indexRoulis == 1) and (x1 - x0) or (z1 - z0) + if math.abs(deltaR) < seuil then + journal.alerte("calib gimbal: roulis insuffisant, abandon") + ui.inviter("Echec: roulis trop faible") + return false + end + -- convention: penche a gauche => roulis normalise NEGATIF + local signeRoulis = (deltaR < 0) and 1 or -1 + + etat.gimbal = { + indexTangage = indexTangage, signeTangage = signeTangage, + indexRoulis = indexRoulis, signeRoulis = signeRoulis, + } + Etat.sauver(etat) + journal.info(("calib gimbal: tangage=angle%d(x%+d) roulis=angle%d(x%+d)") + :format(indexTangage, signeTangage, indexRoulis, signeRoulis)) + ui.inviter("Gimbal calibre, remettez a plat") + return true +end + +-------------------------------------------------------------------- +-- JOYSTICK (assistee) +-------------------------------------------------------------------- +function Calibration.joystick(conf, etat, materiel, journal, Etat, ui) + if not materiel.relay then + journal.alerte("calib joystick: pas de redstone_relay") + return false + end + local seuil = conf.CALIB.JOYSTICK_SEUIL + local mapping = {} + + local function faceActive(exclues) + for face, valeur in pairs(materiel.lireFacesRelay()) do + if valeur >= seuil and not exclues[face] then + return face + end + end + return nil + end + + local function attendreNeutre() + while true do + local actif = false + for _, v in pairs(materiel.lireFacesRelay()) do + if v > 0 then actif = true end + end + if not actif then return end + sleep(0.1) + end + end + + local exclues = {} + for _, direction in ipairs({ "devant", "derriere", "gauche", "droite" }) do + ui.inviter("Joystick a fond: " .. direction:upper()) + local face = nil + while not face do + face = faceActive(exclues) + sleep(0.1) + end + mapping[direction] = face + exclues[face] = true + journal.info(("calib joystick: %s = %s"):format(direction, face)) + ui.inviter(direction .. " = " .. face .. ", relachez") + attendreNeutre() + end + + etat.joystick = mapping + Etat.sauver(etat) + ui.inviter("Joystick calibre") + journal.info("calib joystick: terminee") + return true +end + +-------------------------------------------------------------------- +-- TYPEWRITER (assistee): apprentissage des codes de touches +-------------------------------------------------------------------- +function Calibration.typewriter(conf, etat, materiel, journal, Etat, ui) + if not materiel.typewriter then + journal.alerte("calib typewriter: pas de linked_typewriter") + return false + end + local mapping = {} + local connus = {} + + local function attendreRelachement() + while #materiel.lireTouches() > 0 do sleep(0.1) end + end + + local symboles = { "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", + "valider", "effacer" } + for _, symbole in ipairs(symboles) do + ui.inviter("Typewriter: appuyez sur " .. symbole:upper()) + local code = nil + while not code do + for _, c in ipairs(materiel.lireTouches()) do + if not connus[c] then code = c end + end + sleep(0.1) + end + mapping[code] = symbole + connus[code] = true + journal.info(("calib typewriter: %s = code %s"):format(symbole, + tostring(code))) + attendreRelachement() + end + + etat.typewriter = mapping + Etat.sauver(etat) + ui.inviter("Typewriter calibre") + journal.info("calib typewriter: terminee") + return true +end + +-------------------------------------------------------------------- +-- RSC (mode inactif, au sol, gimbal deja calibre) +-------------------------------------------------------------------- +function Calibration.rsc(conf, etat, materiel, journal, Etat, ui) + if not etat.gimbal then + journal.alerte("calib rsc: calibrer le GIMBAL d'abord") + return false + end + if not materiel.auSol() then + journal.alerte("calib rsc: le drone doit etre pose au sol") + return false + end + materiel.toutArreter() + local C = conf.CALIB + + local nonAffectes = {} + for nom in pairs(materiel.rsc) do + if not etat.roles[nom] then table.insert(nonAffectes, nom) end + end + table.sort(nonAffectes) + if #nonAffectes == 0 then + journal.info("calib rsc: tous les RSC sont deja affectes") + return true + end + + local propulseurs = {} + for _, nom in ipairs(nonAffectes) do + journal.info("calib rsc: impulsion sur " .. nom) + ui.inviter("Impulsion: " .. nom) + local p = materiel.rsc[nom] + local t0, r0 = materiel.lireAssiette() + local vitesse, dt_, dr = 0, 0, 0 + + while vitesse < conf.VITESSE_RSC_MAX do + vitesse = math.min(vitesse + C.IMPULSION_PAS, conf.VITESSE_RSC_MAX) + p.setTargetSpeed(vitesse) + sleep(0.5) + local t, r = materiel.lireAssiette() + dt_, dr = t - t0, r - r0 + if math.abs(dt_) >= C.IMPULSION_SEUIL + and math.abs(dr) >= C.IMPULSION_SEUIL then + break + end + end + p.setTargetSpeed(0) + sleep(1.0) + + if math.abs(dt_) >= C.IMPULSION_SEUIL + and math.abs(dr) >= C.IMPULSION_SEUIL then + -- assiette normalisee: tangage>0 = nez haut => helice a l'AVANT + -- roulis>0 = penche a droite => helice a GAUCHE + local role = ((dr > 0) and "l" or "r") .. ((dt_ > 0) and "f" or "b") + etat.roles[nom] = role + journal.info(" -> helice " .. role) + else + table.insert(propulseurs, nom) + journal.info(" -> pas d'effet d'assiette: propulseur") + end + end + + if #propulseurs == 2 then + etat.roles[propulseurs[1]] = "prop_l" + etat.roles[propulseurs[2]] = "prop_r" + journal.alerte("calib rsc: prop_l/prop_r affectes ARBITRAIREMENT: " + .. "si un virage gauche part a droite, les echanger dans drone.etat") + elseif #propulseurs > 0 then + journal.alerte(("calib rsc: %d propulseur(s), 2 attendus") + :format(#propulseurs)) + end + + Etat.sauver(etat) + journal.info("calib rsc: terminee (relancer drone pour recharger)") + ui.inviter("RSC calibres: relancer drone") + return true +end + +-------------------------------------------------------------------- +-- VELOCITY SENSORS (en vol): axes ET directions. +-- mouvements = { monter, avancer, gauche } (fonctions bloquantes qui +-- commandent le deplacement pendant que ce module mesure) +-------------------------------------------------------------------- +function Calibration.capteurs(conf, etat, materiel, journal, Etat, ui, + mouvements) + local noms = {} + for nom in pairs(materiel.velocite) do table.insert(noms, nom) end + table.sort(noms) + if #noms ~= 3 then + journal.alerte(("calib capteurs: %d velocity sensors, 3 attendus") + :format(#noms)) + return false + end + + -- lance un mouvement en parallele des mesures; retourne, pour le + -- capteur non exclu a la plus grande amplitude, (nom, moyenne) + local function mesurerPendant(mouvement, exclus) + local sommes, maxis, n = {}, {}, 0 + parallel.waitForAll(mouvement, function() + for _ = 1, 30 do + for _, nom in ipairs(noms) do + if not exclus[nom] then + local v = materiel.velocite[nom].getVelocity() or 0 + sommes[nom] = (sommes[nom] or 0) + v + maxis[nom] = math.max(maxis[nom] or 0, math.abs(v)) + end + end + n = n + 1 + sleep(0.1) + end + end) + local meilleur, amplitude = nil, -1 + for _, nom in ipairs(noms) do + if not exclus[nom] and (maxis[nom] or 0) > amplitude then + meilleur, amplitude = nom, maxis[nom] + end + end + return meilleur, (sommes[meilleur] or 0) / math.max(n, 1) + end + + local function signeDe(moyenne) return (moyenne >= 0) and 1 or -1 end + + ui.inviter("Calib capteurs: montee...") + local capteur, moyenne = mesurerPendant(mouvements.monter, {}) + etat.velocite[capteur] = { axe = "vertical", signe = signeDe(moyenne) } + journal.info((" vertical: %s (x%+d)"):format(capteur, signeDe(moyenne))) + local exclus = { [capteur] = true } + + ui.inviter("Calib capteurs: avancee...") + capteur, moyenne = mesurerPendant(mouvements.avancer, exclus) + etat.velocite[capteur] = { axe = "avant", signe = signeDe(moyenne) } + journal.info((" avant: %s (x%+d)"):format(capteur, signeDe(moyenne))) + exclus[capteur] = true + + ui.inviter("Calib capteurs: translation gauche...") + capteur, moyenne = mesurerPendant(mouvements.gauche, exclus) + etat.velocite[capteur] = { axe = "lateral", signe = signeDe(moyenne) } + journal.info((" lateral: %s (x%+d)"):format(capteur, signeDe(moyenne))) + + Etat.sauver(etat) + ui.inviter("Capteurs calibres: relancer drone") + journal.info("calib capteurs: terminee (relancer drone pour recharger)") + return true +end + +-------------------------------------------------------------------- +-- PID D'ALTITUDE (en vol stationnaire, marge d'altitude requise) +-------------------------------------------------------------------- +function Calibration.pid(conf, etat, materiel, pilotage, journal, Etat, + empreinte) + local C = conf.CALIB + local base = materiel.lireAltitude() + local gains = pilotage.gains() + local dt = 0.1 + + local function mesurer(cible, direction) + local debut = os.clock() + local extremum = materiel.lireAltitude() + local assietteMax = 0 + local dansBande, atteint, tempsReponse = nil, false, nil + + while os.clock() - debut < C.TIMEOUT do + local alt = pilotage.reguler(cible, { tangage = 0, roulis = 0 }, + 0, 0, dt) + local t, r = materiel.lireAssiette() + assietteMax = math.max(assietteMax, math.abs(t), math.abs(r)) + if direction > 0 then extremum = math.max(extremum, alt) + else extremum = math.min(extremum, alt) end + + if math.abs(alt - cible) <= C.BANDE then + dansBande = dansBande or os.clock() + if os.clock() - dansBande >= C.DELAI_STABLE then + atteint, tempsReponse = true, dansBande - debut + break + end + else + dansBande = nil + end + sleep(dt) + end + + return { + atteint = atteint, + tempsReponse = tempsReponse or math.huge, + depassement = (direction > 0) + and math.max(0, extremum - cible) + or math.max(0, cible - extremum), + assietteMax = assietteMax, + } + end + + for _, dir in ipairs({ + { nom = "montee", sens = 1 }, + { nom = "descente", sens = -1 }, + }) do + local g = gains[dir.nom] + local meilleurs, meilleurTemps = nil, math.huge + + for iter = 1, C.MAX_ITER do + journal.info(("[calib %s %d/%d] kp=%.1f ki=%.1f kd=%.1f") + :format(dir.nom, iter, C.MAX_ITER, g.kp, g.ki, g.kd)) + pilotage.reglerGains(gains) + + local m = mesurer(base + dir.sens * C.AMPLITUDE, dir.sens) + mesurer(base, -dir.sens) -- retour a la base + + local valide = m.atteint + and m.depassement <= C.TOL_DEPASSEMENT + and m.assietteMax <= conf.ANGLE_MAX + journal.info((" t=%.1fs dep=%.2f incl=%.2f %s"):format( + m.tempsReponse == math.huge and -1 or m.tempsReponse, + m.depassement, m.assietteMax, valide and "VALIDE" or "rejete")) + + if valide and m.tempsReponse < meilleurTemps then + meilleurs, meilleurTemps = copierGains(g), m.tempsReponse + end + + if not m.atteint then + g.kp = g.kp * 1.3 + elseif m.depassement > C.TOL_DEPASSEMENT then + g.kd = g.kd * 1.4 + g.kp = g.kp * 0.9 + elseif m.assietteMax > conf.ANGLE_MAX then + g.kp = g.kp * 0.8 + else + g.kp = g.kp * 1.25 + end + g.ki = g.kp * 0.15 + end + + if meilleurs then + gains[dir.nom] = meilleurs + journal.info(("calib %s: kp=%.1f ki=%.1f kd=%.1f (t=%.1fs)") + :format(dir.nom, meilleurs.kp, meilleurs.ki, meilleurs.kd, + meilleurTemps)) + else + journal.alerte("calib " .. dir.nom .. ": aucun essai valide") + end + end + + pilotage.reglerGains(gains) + etat.empreintePid = empreinte + Etat.sauver(etat) + + local fin = os.clock() + C.TIMEOUT + while os.clock() < fin do + local alt = pilotage.reguler(base, { tangage = 0, roulis = 0 }, 0, 0, dt) + if math.abs(alt - base) <= C.BANDE then break end + sleep(dt) + end + journal.info("calib pid: terminee") + return true +end + +return Calibration diff --git a/lib/conf.lua b/lib/conf.lua new file mode 100644 index 0000000..5d00da9 --- /dev/null +++ b/lib/conf.lua @@ -0,0 +1,153 @@ +-------------------------------------------------------------------- +-- lib/conf.lua : fichier de configuration utilisateur (drone.conf) +-- Cree le fichier avec des valeurs par defaut commentees s'il +-- n'existe pas; avertit tant que `verifie` n'est pas passe a true. +-------------------------------------------------------------------- +local Conf = {} + +local CHEMIN = "drone.conf" + +local MODELE = [[ +-- drone.conf : parametres utilisateur du quadricoptere. +-- RELIRE ET AJUSTER chaque valeur, puis passer verifie a true. +-- NB: les mappages (gimbal, joystick, typewriter, capteurs, RSC) +-- ne sont PAS ici: ils sont calibres en jeu et stockes dans drone.etat. +return { + verifie = false, -- passer a true apres relecture + + ------------------------------------------------------------------ + -- Altitudes (plan de l'altitude sensor du PC central) + ------------------------------------------------------------------ + Y_VOL = 90, -- altitude de croisiere du mode auto + Y_MINI = 70, -- plancher de securite: en dessous, la + -- descente exige un sol POSABLE detecte + -- (evite d'atterrir dans un ocean) + + ------------------------------------------------------------------ + -- Assiette + ------------------------------------------------------------------ + ANGLE_MAX = 1.5, -- inclinaison max toleree hors mode drone (deg) + ANGLE_DRONE_MAX = 15, -- inclinaison commandee max en mode drone (deg) + + ------------------------------------------------------------------ + -- Sol + ------------------------------------------------------------------ + DIST_SOL = 1.2, -- optical sensor: distance <= => au sol + -- (15.5 = aucun bloc detecte) + VITESSE_ATTERRISSAGE = 1.0, -- descente du mode atterrissage (blocs/s) + BLOCS_INTERDITS = { -- blocs sur lesquels ne PAS se poser + ["minecraft:water"] = true, + ["minecraft:lava"] = true, + }, + + ------------------------------------------------------------------ + -- Position reelle affichee = position GPS + ces offsets + ------------------------------------------------------------------ + OFFSET_X = 0, + OFFSET_Y = 0, + OFFSET_Z = 0, + + ------------------------------------------------------------------ + -- Regulation + ------------------------------------------------------------------ + VITESSE_RAMPE = 3.0, -- rampe de consigne d'altitude (blocs/s) + VITESSE_RSC_MAX = 256, -- borne des rotation speed controllers + PID = { -- gains par defaut (remplaces par calib) + montee = { kp = 12, ki = 2.0, kd = 18 }, + descente = { kp = 10, ki = 1.5, kd = 22 }, + tangage = { kp = 4, ki = 0.5, kd = 8 }, + roulis = { kp = 4, ki = 0.5, kd = 8 }, + }, + + ------------------------------------------------------------------ + -- Ecrans (faces des deux Create_DisplayLink sur le PC) + ------------------------------------------------------------------ + ECRANS = { + conduite = "front", -- vitesse, altitudes, invites + carburant = "back", -- reserve + autonomie estimee + }, + + ------------------------------------------------------------------ + -- Carburant (2 cuves create:fluid_tank sur le reseau) + ------------------------------------------------------------------ + CARBURANT = { + CAPACITE_TANK = 8000, -- mB par cuve, si tanks() ne donne pas + -- la capacite + SEUIL_ALERTE = 0.15, -- alerte sous ce ratio de reserve + PERIODE = 5, -- periode d'echantillonnage (s) + LISSAGE = 0.1, -- apprentissage glissant de la conso + -- (EMA: 0.1 = lent et stable, 0.5 = reactif) + }, + + ------------------------------------------------------------------ + -- Moteur (sortie redstone bottom: 15=arret, 14=mini, 0=maxi) + -- Le moteur est dimensionne pour suffire; en cas de surcharge + -- malgre tout, les PROPULSEURS sont reduits en priorite + alerte. + ------------------------------------------------------------------ + MOTEUR = { + STRESS_HAUT = 0.90, -- au dela: augmenter le moteur + STRESS_BAS = 0.60, -- en deca: reduire le moteur (economie) + SURCHARGE = 0.98, -- au dela: reduire les propulseurs + alerte + PERIODE = 1.0, -- periode de l'asservissement (s) + }, + + ------------------------------------------------------------------ + -- Navigation (modes auto / position, points enregistres) + ------------------------------------------------------------------ + NAV = { + SEUIL_ARRIVEE = 2.0, -- distance a la cible consideree atteinte + VITESSE_GPS_MIN = 0.5, -- vitesse mini pour estimer le cap (b/s) + MAX_POINTS = 10, -- points nommes enregistrables + }, + + ------------------------------------------------------------------ + -- Calibration (voir README) + ------------------------------------------------------------------ + CALIB = { + AMPLITUDE = 4, -- pid: hauteur des echelons de test (blocs) + TOL_DEPASSEMENT = 0.0, -- pid: depassement tolere (0 = strict) + BANDE = 0.3, -- pid: bande de convergence (blocs) + DELAI_STABLE = 2.0, -- pid: duree dans la bande = converge (s) + TIMEOUT = 25, -- pid: duree max d'un echelon (s) + MAX_ITER = 6, -- pid: essais par direction + IMPULSION_PAS = 16, -- rsc: increment de vitesse (rpm) + IMPULSION_SEUIL = 0.8, -- rsc/gimbal: variation d'angle declarante (deg) + JOYSTICK_SEUIL = 8, -- joystick: niveau declarant une face (0..15) + }, +} +]] + +function Conf.charger(journal) + if not fs.exists(CHEMIN) then + local f = fs.open(CHEMIN, "w") + f.write(MODELE) + f.close() + journal.alerte(CHEMIN .. " cree avec les valeurs par defaut: A PERSONNALISER") + end + + local chargeur, err = loadfile(CHEMIN) + if not chargeur then + error("drone.conf invalide: " .. tostring(err), 0) + end + local conf = chargeur() + if type(conf) ~= "table" then + error("drone.conf doit retourner une table", 0) + end + + if conf.verifie ~= true then + journal.alerte("drone.conf non verifie: relire les parametres puis passer verifie a true") + end + return conf +end + +-- Empreinte des parametres dont dependent les gains PID calibres. +-- Si elle change, la calibration PID est invalidee. +function Conf.empreintePid(conf) + return textutils.serialize({ + vmax = conf.VITESSE_RSC_MAX, + rampe = conf.VITESSE_RAMPE, + angle = conf.ANGLE_MAX, + }) +end + +return Conf diff --git a/lib/etat.lua b/lib/etat.lua new file mode 100644 index 0000000..b2b7484 --- /dev/null +++ b/lib/etat.lua @@ -0,0 +1,61 @@ +-------------------------------------------------------------------- +-- lib/etat.lua : persistance de l'etat du drone (drone.etat) +-- Restaure apres un redemarrage: mode, consigne, et tous les +-- MAPPAGES CALIBRES (propres a chaque vaisseau): +-- roles nom RSC -> lb|rb|lf|rf|prop_l|prop_r +-- gimbal { indexTangage, signeTangage, indexRoulis, signeRoulis } +-- convention normalisee: tangage negatif = nez bas, +-- roulis negatif = penche a gauche +-- joystick direction (devant|derriere|gauche|droite) -> face relay +-- typewriter code de touche -> symbole ("0".."9", "valider", "effacer") +-- velocite nom capteur -> { axe = avant|lateral|vertical, +-- signe = 1|-1 } (positif = avant/gauche/haut) +-- points nom -> { x, y, z } (max NAV.MAX_POINTS) +-- empreintePid parametres de conf au moment de la calib PID +-------------------------------------------------------------------- +local Etat = {} + +local CHEMIN = "drone.etat" + +local DEFAUT = { + mode = "off", + consigneY = nil, + roles = {}, + gimbal = nil, + joystick = nil, + typewriter = nil, + velocite = {}, + points = {}, + pid = nil, + empreintePid = nil, + decollage = nil, +} + +function Etat.charger(journal) + local etat = {} + for k, v in pairs(DEFAUT) do etat[k] = v end + + if fs.exists(CHEMIN) then + local f = fs.open(CHEMIN, "r") + local contenu = f.readAll() + f.close() + local lu = textutils.unserialize(contenu) + if type(lu) == "table" then + for k, v in pairs(lu) do etat[k] = v end + journal.info(("etat restaure (mode %s)"):format(etat.mode)) + else + journal.alerte("drone.etat illisible, etat par defaut") + end + end + return etat +end + +function Etat.sauver(etat) + local f = fs.open(CHEMIN, "w") + if f then + f.write(textutils.serialize(etat)) + f.close() + end +end + +return Etat diff --git a/lib/ihm.lua b/lib/ihm.lua new file mode 100644 index 0000000..76a7bef --- /dev/null +++ b/lib/ihm.lua @@ -0,0 +1,176 @@ +-------------------------------------------------------------------- +-- lib/ihm.lua : interfaces homme-machine. +-- Display Link "conduite" : altitudes, vitesse, invites, alertes +-- Display Link "carburant" : reserve + autonomie estimee (minutes) +-- Advanced monitor tactile : modes (verrouilles si calibration +-- manquante), consigne, calibrations, informations +-- Typewriter : la saisie est geree dans drone.lua (scrutation) +-------------------------------------------------------------------- +local Ihm = {} + +function Ihm.nouveau(conf, materiel, journal) + local ihm = {} + local moniteur = materiel.moniteur + moniteur.setTextScale(0.5) + + local zones = {} + local messageConduite, messageFin = nil, 0 + + -- message temporaire sur la ligne 1 de la conduite (invites, + -- alertes); duree en secondes (nil = 5) + function ihm.message(texte, duree) + messageConduite = texte + messageFin = os.clock() + (duree or 5) + end + + ------------------------------------------------------------------ + -- ECRAN DE CONDUITE + ------------------------------------------------------------------ + -- ctx: { mode, consigneY, saisie } + function ihm.rafraichirConduite(ctx) + local e = materiel.ecranConduite + e.clear() + e.setCursorPos(1, 1) + if messageConduite and os.clock() < messageFin then + e.write(messageConduite:sub(1, 22)) + elseif ctx.saisie and #ctx.saisie > 0 then + e.write("SAISIE: " .. ctx.saisie) + elseif ctx.mode == "vol" or ctx.mode == "drone" + or ctx.mode == "atterrissage" or ctx.mode == "auto" + or ctx.mode == "position" then + e.write(("ALT %.1f / %.0f"):format( + materiel.lireAltitude(), ctx.consigneY or 0)) + else + e.write("MODE " .. ctx.mode:upper()) + end + e.setCursorPos(1, 2) + e.write(("VIT %.2f b/s"):format(materiel.vitesseHorizontale())) + e.update() + end + + ------------------------------------------------------------------ + -- ECRAN CARBURANT (peut etre tres etroit: format compact) + ------------------------------------------------------------------ + -- pct 0..1 ou nil ; minutes ou nil + function ihm.rafraichirCarburant(pct, minutes) + local e = materiel.ecranCarburant + local largeur = e.getSize() + e.clear() + e.setCursorPos(1, 1) + if pct == nil then + e.write(("n/a"):sub(1, largeur)) + else + e.write(("%d%%"):format(math.floor(pct * 100 + 0.5)):sub(1, largeur)) + e.setCursorPos(1, 2) + if minutes then + e.write(("%dm"):format(math.floor(minutes)):sub(1, largeur)) + else + e.write(("--m"):sub(1, largeur)) + end + end + e.update() + end + + ------------------------------------------------------------------ + -- MONITEUR TACTILE + ------------------------------------------------------------------ + local function bouton(x, y, texte, action, couleur) + moniteur.setCursorPos(x, y) + moniteur.setBackgroundColor(couleur or colors.gray) + moniteur.write(" " .. texte .. " ") + moniteur.setBackgroundColor(colors.black) + table.insert(zones, { + x1 = x, y1 = y, x2 = x + #texte + 1, y2 = y, action = action, + }) + end + + local function texte(x, y, contenu, couleur) + moniteur.setTextColor(couleur or colors.white) + moniteur.setCursorPos(x, y) + moniteur.write(contenu) + moniteur.setTextColor(colors.white) + end + + -- ctx: { mode, consigneY, stress, position, carburantPct, minutes, + -- moteurOk, verrous = { mode -> raison|nil }, points = n } + function ihm.rafraichirMoniteur(ctx) + zones = {} + moniteur.setBackgroundColor(colors.black) + moniteur.clear() + + texte(1, 1, "== MODES == actuel: " .. ctx.mode) + local modes = { "off", "inactif", "stationnement", "vol", "drone", + "atterrissage", "auto", "position" } + local x, y = 1, 2 + for _, m in ipairs(modes) do + local verrou = ctx.verrous[m] + local couleur = colors.gray + if m == ctx.mode then couleur = colors.green + elseif verrou then couleur = colors.red end + if x + #m + 2 > 50 then x, y = 1, y + 1 end + bouton(x, y, m, { type = "mode", valeur = m, verrou = verrou }, + couleur) + x = x + #m + 3 + end + + texte(1, y + 2, ("== CONSIGNE == %.0f"):format(ctx.consigneY or 0)) + bouton(1, y + 3, "-10", { type = "delta", valeur = -10 }) + bouton(7, y + 3, "-1", { type = "delta", valeur = -1 }) + bouton(12, y + 3, "+1", { type = "delta", valeur = 1 }) + bouton(17, y + 3, "+10", { type = "delta", valeur = 10 }) + + texte(1, y + 5, "== CALIBRATIONS ==") + local cx = 1 + for _, c in ipairs({ "gimbal", "joystick", "typew", "rsc", + "capteurs", "pid" }) do + bouton(cx, y + 6, c, { type = "calib", valeur = c }) + cx = cx + #c + 3 + end + + local ly = y + 8 + texte(1, ly, "== INFOS ==") + local t, r = materiel.lireAssiette() + texte(1, ly + 1, ("assiette T %+.2f R %+.2f%s"):format(t, r, + ctx.gimbalOk and "" or " (NON CALIBRE)")) + texte(1, ly + 2, ("sol %.1f (%s%s)"):format( + materiel.lireDistanceSol(), + materiel.auSol() and "au sol" or "en vol", + materiel.solPosable() and "" or ", NON POSABLE")) + texte(1, ly + 3, ("stress %s moteur %s"):format( + ctx.stress and ("%.0f%%"):format(ctx.stress * 100) or "n/a", + ctx.moteurOk and "ok" or "ARRETE"), + ctx.moteurOk and colors.white or colors.red) + texte(1, ly + 4, ("carburant %s autonomie %s"):format( + ctx.carburantPct + and ("%.0f%%"):format(ctx.carburantPct * 100) or "n/a", + ctx.minutes and ("%dmin"):format(math.floor(ctx.minutes)) or "--")) + if ctx.position then + texte(1, ly + 5, ("gps %.0f %.0f %.0f points: %d") + :format(ctx.position.x, ctx.position.y, ctx.position.z, + ctx.points or 0)) + else + texte(1, ly + 5, "gps n/a", colors.red) + end + end + + function ihm.traiterToucher(tx, ty) + for _, z in ipairs(zones) do + if tx >= z.x1 and tx <= z.x2 and ty >= z.y1 and ty <= z.y2 then + return z.action + end + end + return nil + end + + -- attend un toucher du moniteur (utilise par les calibrations) + function ihm.attendreToucher() + while true do + local ev = os.pullEvent("monitor_touch") + if ev then return end + end + end + + return ihm +end + +return Ihm diff --git a/lib/journal.lua b/lib/journal.lua new file mode 100644 index 0000000..477c3c6 --- /dev/null +++ b/lib/journal.lua @@ -0,0 +1,42 @@ +-------------------------------------------------------------------- +-- lib/journal.lua : journal de vol (console + fichier rotatif) +-------------------------------------------------------------------- +local Journal = {} + +local FICHIER, MAX_TAILLE = "drone.log", 64 * 1024 + +local function horodater(texte) + return ("[%s] %s"):format(textutils.formatTime(os.time("local"), true), texte) +end + +local function ecrireFichier(ligne) + if fs.exists(FICHIER) and fs.getSize(FICHIER) > MAX_TAILLE then + fs.delete(FICHIER .. ".old") + fs.move(FICHIER, FICHIER .. ".old") + end + local f = fs.open(FICHIER, "a") + if f then + f.writeLine(ligne) + f.close() + end +end + +function Journal.info(texte) + local ligne = horodater(texte) + print(ligne) + ecrireFichier(ligne) +end + +function Journal.erreur(texte) + local ligne = horodater("ERREUR " .. texte) + printError(ligne) + ecrireFichier(ligne) +end + +function Journal.alerte(texte) + local ligne = horodater("ALERTE " .. texte) + printError(ligne) + ecrireFichier(ligne) +end + +return Journal diff --git a/lib/materiel.lua b/lib/materiel.lua new file mode 100644 index 0000000..94a6057 --- /dev/null +++ b/lib/materiel.lua @@ -0,0 +1,254 @@ +-------------------------------------------------------------------- +-- lib/materiel.lua : resolution des peripheriques et adaptateurs. +-- Tous les formats sont VERIFIES en jeu (inventaire du 17/07/2026). +-- +-- Directs : altitude_sensor (top), 2 Create_DisplayLink (faces dans +-- conf.ECRANS), ender modem (gps), moteur diesel en bottom pilote +-- par redstone analogique (15=arret, 14=mini, 0=maxi). +-- Reseau : RSC (roles calibres), velocity_sensor x3 (axes/directions +-- calibres), gimbal_sensor (axes calibres), optical_sensor, +-- Create_Stressometer, redstone_relay (joystick calibre), +-- linked_typewriter, monitor, 2 cuves create:fluid_tank. +-------------------------------------------------------------------- +local Materiel = {} + +local function trouver(type_, obligatoire, journal, note) + local p = peripheral.find(type_) + if not p and obligatoire then + error("peripherique manquant: " .. type_, 0) + end + if not p and journal then + journal.alerte(type_ .. " introuvable" .. (note and (": " .. note) or "")) + end + return p +end + +function Materiel.initialiser(conf, etat, journal) + local m = {} + + m.altitude = trouver("altitude_sensor", true) + m.gimbal = trouver("gimbal_sensor", true) + m.optical = trouver("optical_sensor", true) + m.moniteur = trouver("monitor", true) + m.relay = trouver("redstone_relay", false, journal, "joystick inactif") + m.stresso = trouver("Create_Stressometer", false, journal, + "detection moteur et surcharge inactives") + m.typewriter = trouver("linked_typewriter", false, journal, + "consigne au moniteur uniquement") + + -- les deux display links, identifies par leur face (conf.ECRANS) + local function ecran(face, usage) + local p = peripheral.wrap(face) + if not p or peripheral.getType(face) ~= "Create_DisplayLink" then + error(("Create_DisplayLink attendu en '%s' (%s), voir conf.ECRANS") + :format(face, usage), 0) + end + return p + end + m.ecranConduite = ecran(conf.ECRANS.conduite, "conduite") + m.ecranCarburant = ecran(conf.ECRANS.carburant, "carburant") + + -- rotation speed controllers + m.rsc, m.rscParRole = {}, {} + for _, nom in ipairs(peripheral.getNames()) do + if peripheral.getType(nom) == "Create_RotationSpeedController" then + m.rsc[nom] = peripheral.wrap(nom) + local role = etat.roles[nom] + if role then m.rscParRole[role] = m.rsc[nom] end + end + end + + -- velocity sensors + m.velocite, m.veloParAxe = {}, {} + for _, nom in ipairs(peripheral.getNames()) do + if peripheral.getType(nom) == "velocity_sensor" then + m.velocite[nom] = peripheral.wrap(nom) + local aff = etat.velocite[nom] + if aff then + m.veloParAxe[aff.axe] = { p = m.velocite[nom], signe = aff.signe } + end + end + end + + -- cuves de carburant (create:fluid_tank; exclut le moteur diesel, + -- qui est fluid_storage mais pas fluid_tank) + m.cuves = {} + for _, nom in ipairs(peripheral.getNames()) do + if peripheral.hasType(nom, "create:fluid_tank") then + table.insert(m.cuves, peripheral.wrap(nom)) + end + end + if #m.cuves == 0 then + journal.alerte("aucune cuve create:fluid_tank: suivi carburant inactif") + end + + ------------------------------------------------------------------ + -- LECTURES + ------------------------------------------------------------------ + function m.lireAltitude() + return m.altitude.getHeight() + end + + -- angles bruts du gimbal (table de 2) + function m.lireAnglesBruts() + local a = m.gimbal.getAngles() + return a[1] or 0, a[2] or 0 + end + + -- assiette NORMALISEE via la calibration gimbal : + -- tangage negatif = nez bas, roulis negatif = penche a gauche + function m.lireAssiette() + local g = etat.gimbal + if not g then return m.lireAnglesBruts() end + local a = m.gimbal.getAngles() + return (a[g.indexTangage] or 0) * g.signeTangage, + (a[g.indexRoulis] or 0) * g.signeRoulis + end + + function m.lireDistanceSol() + return m.optical.getDistance() + end + + function m.auSol() + return m.lireDistanceSol() <= conf.DIST_SOL + end + + -- sol detecte ET posable (pas d'eau/lave: garde-fou ocean) + function m.solPosable() + if not m.optical.hasHit() then return false end + local bloc = m.optical.getBlock() + return not conf.BLOCS_INTERDITS[bloc] + end + + function m.horizontal() + local t, r = m.lireAssiette() + return math.abs(t) <= conf.ANGLE_MAX and math.abs(r) <= conf.ANGLE_MAX + end + + -- vitesse le long d'un axe calibre ("avant"|"lateral"|"vertical") + -- signe: positif = avant / gauche / haut + function m.lireVitesseAxe(axe) + local c = m.veloParAxe[axe] + if not c then return 0 end + return (c.p.getVelocity() or 0) * c.signe + end + + function m.vitesseHorizontale() + local va = m.lireVitesseAxe("avant") + local vl = m.lireVitesseAxe("lateral") + return math.sqrt(va * va + vl * vl) + end + + -- joystick calibre: avance (devant-derriere), virage (gauche-droite) + -- dans -1..1; pas de zone morte (joystick de mod, zero franc) + function m.lireJoystick() + local j = etat.joystick + if not (m.relay and j) then return 0, 0 end + local function lire(direction) + local face = j[direction] + if not face then return 0 end + return m.relay.getAnalogInput(face) / 15 + end + return lire("devant") - lire("derriere"), + lire("gauche") - lire("droite") + end + + function m.lireFacesRelay() + if not m.relay then return {} end + local lectures = {} + for _, face in ipairs({ "top", "bottom", "left", "right", + "front", "back" }) do + lectures[face] = m.relay.getAnalogInput(face) + end + return lectures + end + + -- typewriter: table des codes de touches actuellement pressees + function m.lireTouches() + if not m.typewriter then return {} end + return m.typewriter.getPressedKeyCodes() or {} + end + + -- stressometer (formats verifies): ratio 0..1, ou nil + function m.lireStress() + if not m.stresso then return nil end + local cap = m.stresso.getStressCapacity() + if not cap or cap <= 0 then return nil end + return m.stresso.getStress() / cap + end + + -- capacite > 0 <=> le moteur tourne (verifie: capacite 0 a l'arret) + function m.moteurTourne() + if not m.stresso then return true end -- indetectable: on suppose oui + return (m.stresso.getStressCapacity() or 0) > 0 + end + + -- carburant: quantite totale (mB) et capacite totale + function m.lireCarburant() + if #m.cuves == 0 then return nil end + local quantite, capacite = 0, 0 + for _, cuve in ipairs(m.cuves) do + for _, t in ipairs(cuve.tanks() or {}) do + quantite = quantite + (t.amount or 0) + capacite = capacite + (t.capacity or conf.CARBURANT.CAPACITE_TANK) + end + end + if capacite <= 0 then capacite = conf.CARBURANT.CAPACITE_TANK * #m.cuves end + return quantite, capacite + end + + -- position GPS corrigee des offsets, ou nil + function m.lirePosition() + local x, y, z = gps.locate(2) + if not x then return nil end + return x + conf.OFFSET_X, y + conf.OFFSET_Y, z + conf.OFFSET_Z + end + + ------------------------------------------------------------------ + -- SORTIES + ------------------------------------------------------------------ + function m.reglerMoteur(niveau) -- 0 (arret) .. 15 (maxi) + niveau = math.max(0, math.min(15, math.floor(niveau + 0.5))) + redstone.setAnalogOutput("bottom", 15 - niveau) + end + + function m.reglerRsc(role, vitesse) + local p = m.rscParRole[role] + if p then p.setTargetSpeed(math.floor(vitesse + 0.5)) end + end + + function m.toutArreter() + for _, p in pairs(m.rsc) do p.setTargetSpeed(0) end + end + + ------------------------------------------------------------------ + -- DIAGNOSTIC DES CALIBRATIONS + ------------------------------------------------------------------ + function m.helicesManquantes() + local manquantes = {} + for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do + if not m.rscParRole[role] then table.insert(manquantes, role) end + end + return manquantes + end + + function m.propulseursManquants() + local manquants = {} + for _, role in ipairs({ "prop_l", "prop_r" }) do + if not m.rscParRole[role] then table.insert(manquants, role) end + end + return manquants + end + + function m.axesManquants() + local manquants = {} + for _, axe in ipairs({ "avant", "lateral", "vertical" }) do + if not m.veloParAxe[axe] then table.insert(manquants, axe) end + end + return manquants + end + + return m +end + +return Materiel diff --git a/lib/navigation.lua b/lib/navigation.lua new file mode 100644 index 0000000..669006c --- /dev/null +++ b/lib/navigation.lua @@ -0,0 +1,77 @@ +-------------------------------------------------------------------- +-- lib/navigation.lua : position GPS, estimation du cap, ralliement. +-- +-- Le cap n'est pas mesurable directement (pas de boussole): il est +-- estime a partir du deplacement GPS quand la vitesse est +-- suffisante. Le ralliement avance donc "en aveugle" au depart puis +-- corrige le cap des que le drone bouge. +-------------------------------------------------------------------- +local Navigation = {} + +function Navigation.nouveau(conf, materiel, journal) + local n = {} + local posPrec, tPrec = nil, nil + local cap = nil -- radians, atan2(dx, dz) du deplacement + + function n.position() + return materiel.lirePosition() + end + + -- A appeler periodiquement: met a jour l'estimation du cap + function n.actualiser() + local x, y, z = materiel.lirePosition() + if not x then return nil end + local t = os.clock() + if posPrec and t > tPrec then + local dx, dz = x - posPrec.x, z - posPrec.z + local dist = math.sqrt(dx * dx + dz * dz) + local vitesse = dist / (t - tPrec) + if vitesse >= conf.NAV.VITESSE_GPS_MIN then + cap = math.atan2(dx, dz) + end + end + posPrec, tPrec = { x = x, y = y, z = z }, t + return x, y, z + end + + -- Commandes propulseurs pour rallier (cibleX, cibleZ). + -- Retourne avance, virage, distance (ou nil si pas de GPS). + function n.rallier(cibleX, cibleZ) + local x, y, z = n.actualiser() + if not x then return nil end + + local dx, dz = cibleX - x, cibleZ - z + local distance = math.sqrt(dx * dx + dz * dz) + if distance <= conf.NAV.SEUIL_ARRIVEE then + return 0, 0, distance + end + + local capCible = math.atan2(dx, dz) + local virage = 0 + if cap then + local ecart = capCible - cap + while ecart > math.pi do ecart = ecart - 2 * math.pi end + while ecart < -math.pi do ecart = ecart + 2 * math.pi end + -- proportionnel borne: plein virage au dela de 45 degres + virage = math.max(-1, math.min(1, ecart / (math.pi / 4))) + end + + -- avance reduite tant que le cap est incertain ou tres desaxe + local avance = 0.8 + if cap == nil then + avance = 0.4 -- demarrage: avancer pour estimer le cap + elseif math.abs(virage) > 0.7 then + avance = 0.2 -- virage serre: privilegier la rotation + end + + return avance, virage, distance + end + + function n.razCap() + cap, posPrec, tPrec = nil, nil, nil + end + + return n +end + +return Navigation diff --git a/lib/pid.lua b/lib/pid.lua new file mode 100644 index 0000000..d8986ea --- /dev/null +++ b/lib/pid.lua @@ -0,0 +1,61 @@ +-------------------------------------------------------------------- +-- lib/pid.lua : regulateur PID concu pour le zero-depassement +-- - derivee calculee sur la MESURE (pas de coup de derivee au +-- changement de consigne) +-- - integrale conditionnelle: gelee quand la sortie sature dans le +-- sens de l'erreur (anti-emballement) +-------------------------------------------------------------------- +local Pid = {} +Pid.__index = Pid + +-- gains: { kp, ki, kd } ; bornes de sortie ; borne d'integrale +function Pid.nouveau(gains, borneMin, borneMax, iMax) + return setmetatable({ + gains = gains, + borneMin = borneMin, + borneMax = borneMax, + iMax = iMax or 40, + integrale = 0, + mesurePrec = nil, + }, Pid) +end + +function Pid:raz() + self.integrale = 0 + self.mesurePrec = nil +end + +function Pid:reglerGains(gains) + self.gains = gains +end + +function Pid:calculer(consigne, mesure, dt) + local g = self.gains + local erreur = consigne - mesure + + local derivee = 0 + if self.mesurePrec and dt > 0 then + derivee = (mesure - self.mesurePrec) / dt + end + self.mesurePrec = mesure + + -- tentative d'integration bornee + local integrale = self.integrale + erreur * dt + if integrale > self.iMax then integrale = self.iMax end + if integrale < -self.iMax then integrale = -self.iMax end + + local sortie = g.kp * erreur + g.ki * integrale - g.kd * derivee + + if sortie > self.borneMax then + -- sature haut: on n'integre que si l'erreur tire vers le bas + if erreur < 0 then self.integrale = integrale end + return self.borneMax + elseif sortie < self.borneMin then + if erreur > 0 then self.integrale = integrale end + return self.borneMin + end + self.integrale = integrale + return sortie +end + +return Pid diff --git a/lib/pilotage.lua b/lib/pilotage.lua new file mode 100644 index 0000000..cfd2892 --- /dev/null +++ b/lib/pilotage.lua @@ -0,0 +1,135 @@ +-------------------------------------------------------------------- +-- lib/pilotage.lua : regulation et mixage. +-- +-- base = PID d'altitude (double regime montee / descente) +-- assiette = PID tangage + PID roulis vers des angles cibles +-- (0 sauf en mode drone ou le joystick les commande) +-- helice(coin) = base + signeBF*corrTangage + signeLR*corrRoulis +-- propulseurs = mixage differentiel avance / virage +-- +-- La consigne d'altitude passe par une rampe (VITESSE_RAMPE) pour +-- garantir des transitions douces et sans depassement. +-------------------------------------------------------------------- +local Pilotage = {} + +local SIGNES = { + lb = { bf = -1, lr = 1 }, + rb = { bf = -1, lr = -1 }, + lf = { bf = 1, lr = 1 }, + rf = { bf = 1, lr = -1 }, +} + +-- materiel.lireAssiette() est NORMALISE par la calibration gimbal: +-- tangage negatif = nez bas, roulis negatif = penche a gauche. +-- Une helice AVANT pousse plus quand le tangage est negatif, une +-- helice GAUCHE pousse plus quand le roulis est negatif => signes -1 +local SIGNE_TANGAGE, SIGNE_ROULIS = -1, -1 + +function Pilotage.nouveau(conf, etat, materiel, Pid, journal) + local p = {} + local VMAX = conf.VITESSE_RSC_MAX + local gains = etat.pid or conf.PID + + local pidMontee = Pid.nouveau(gains.montee, 0, VMAX) + local pidDescente = Pid.nouveau(gains.descente, 0, VMAX) + local pidTangage = Pid.nouveau(gains.tangage, -VMAX / 2, VMAX / 2) + local pidRoulis = Pid.nouveau(gains.roulis, -VMAX / 2, VMAX / 2) + + local consigneRampe = nil + local regimePrec = nil + local facteurProp = 1.0 -- reduit en cas de surcharge du moteur + + function p.gains() + return gains + end + + -- 0..1 : applique aux propulseurs (priorite de delestage en + -- cas de surcharge du reseau de stress) + function p.reglerFacteurProp(f) + facteurProp = math.max(0, math.min(1, f)) + end + + function p.reglerGains(nouveaux) + gains = nouveaux + etat.pid = nouveaux + pidMontee:reglerGains(gains.montee) + pidDescente:reglerGains(gains.descente) + pidTangage:reglerGains(gains.tangage) + pidRoulis:reglerGains(gains.roulis) + end + + function p.raz() + pidMontee:raz() + pidDescente:raz() + pidTangage:raz() + pidRoulis:raz() + consigneRampe = nil + regimePrec = nil + end + + -- Un pas de regulation en vol. + -- consigneY : altitude cible + -- angles : { tangage = deg, roulis = deg } cibles + -- avance, virage : commande propulseurs dans -1..1 + function p.reguler(consigneY, angles, avance, virage, dt) + local altitude = materiel.lireAltitude() + local tangage, roulis = materiel.lireAssiette() + + -- rampe de consigne + if consigneRampe == nil then consigneRampe = altitude end + local pas = conf.VITESSE_RAMPE * dt + local ecart = consigneY - consigneRampe + if ecart > pas then ecart = pas elseif ecart < -pas then ecart = -pas end + consigneRampe = consigneRampe + ecart + + -- double regime : montee ou descente + local montee = (consigneRampe - altitude) >= 0 + local pid = montee and pidMontee or pidDescente + if regimePrec ~= nil and regimePrec ~= montee then + pid:raz() + end + regimePrec = montee + local base = pid:calculer(consigneRampe, altitude, dt) + + -- assiette + local corrTangage = SIGNE_TANGAGE + * pidTangage:calculer(angles.tangage or 0, tangage, dt) + local corrRoulis = SIGNE_ROULIS + * pidRoulis:calculer(angles.roulis or 0, roulis, dt) + + for role, s in pairs(SIGNES) do + local v = base + s.bf * corrTangage + s.lr * corrRoulis + if v < 0 then v = 0 elseif v > VMAX then v = VMAX end + materiel.reglerRsc(role, v) + end + + -- propulseurs (differentiel) + local function borner(v) + if v > 1 then return 1 elseif v < -1 then return -1 end + return v + end + materiel.reglerRsc("prop_r", borner(avance + virage) * VMAX * facteurProp) + materiel.reglerRsc("prop_l", borner(avance - virage) * VMAX * facteurProp) + + return altitude, consigneRampe + end + + -- Stationnement: helices a pleine puissance inversee (plaquage), + -- propulseurs a 0 + function p.plaquer() + for role in pairs(SIGNES) do + materiel.reglerRsc(role, -VMAX) + end + materiel.reglerRsc("prop_l", 0) + materiel.reglerRsc("prop_r", 0) + end + + function p.arreter() + materiel.toutArreter() + p.raz() + end + + return p +end + +return Pilotage