j/fdr1-modbus-slave
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

dev

Browse Source
This commit is contained in:
Jonathan Roth 2026-03-10 16:46:46 +01:00
parent 38b0853c32
commit c0c8cb91c7
3 changed files with 82 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
includes/arm.h
View File

@ -14,14 +14,25 @@
#define ARM_REG_LEFT 0x0804 // position limite gauche (1/10 mm) #define ARM_REG_LEFT 0x0804 // position limite gauche (1/10 mm)
#define ARM_REG_RIGHT 0x0805 // position limite droite (1/10 mm) #define ARM_REG_RIGHT 0x0805 // position limite droite (1/10 mm)
// ─────────────────────────────────────────────
// Valeurs par défaut LEFT/RIGHT
// ─────────────────────────────────────────────
#define ARM_LEFT_DEFAULT 500U // 50.0 mm en dixièmes
#define ARM_RIGHT_DEFAULT 4500U // 450.0 mm en dixièmes
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// Mécanique // Mécanique
// 1 tour ARM = 40mm = 1600 micropas // 1 tour ARM = 40mm = 1600 micropas
// 1 micropas ARM = 25µm // 1 micropas ARM = 25µm
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
#define ARM_UM_PER_USTEP 25U // µm par micropas // 8 microsteps
#define ARM_USTEP_PER_REV 1600U // micropas par tour // #define ARM_UM_PER_USTEP 25U // µm par micropas
// #define ARM_USTEP_PER_REV 1600U // micropas par tour
// 4 microsteps
#define ARM_UM_PER_USTEP 50U // µm par micropas
#define ARM_USTEP_PER_REV 800U // micropas par tour
#define ARM_UM_PER_REV ((uint32_t)ARM_UM_PER_USTEP * ARM_USTEP_PER_REV) // 40000µm #define ARM_UM_PER_REV ((uint32_t)ARM_UM_PER_USTEP * ARM_USTEP_PER_REV) // 40000µm
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
@ -31,19 +42,12 @@
#define ARM_GEAR_MIN 0 // µm (arrêt vgear) #define ARM_GEAR_MIN 0 // µm (arrêt vgear)
#define ARM_GEAR_MAX 65535 // µm = 65.535mm par tour SPOOL #define ARM_GEAR_MAX 65535 // µm = 65.535mm par tour SPOOL
// ─────────────────────────────────────────────
// Valeurs par défaut positions (1/10 mm)
// ─────────────────────────────────────────────
#define ARM_LEFT_DEFAULT 500 // 50.0mm
#define ARM_RIGHT_DEFAULT 4500 // 450.0mm
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// API publique // API publique
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
void ARM_Init(void); void ARM_Init(void);
void ARM_Step(void); // appelé depuis l'ISR SPOOL à chaque step void ARM_UpdateTarget(void); // appelé depuis l'ISR SPOOL — met à jour arm_target_position
void ARM_Enable(uint8_t state); // ENABLE moteur ARM void ARM_Enable(uint8_t state); // ENABLE moteur ARM
void ARM_SetDir(uint8_t dir); // sens de rotation ARM void ARM_SetDir(uint8_t dir); // sens de rotation ARM
void ARM_SetFree(uint8_t state); // free ARM + désactive vgear (REG_CTRL bit1) void ARM_SetFree(uint8_t state); // free ARM + désactive vgear (REG_CTRL bit1)
@ -53,6 +57,12 @@ void ARM_SetOffset(uint16_t um); // écriture registre 0x0803 + sauvegard
uint16_t ARM_GetOffset(void); // lecture offset courant uint16_t ARM_GetOffset(void); // lecture offset courant
void ARM_SetLeft(uint16_t dmm); // écriture registre 0x0804 (1/10 mm) void ARM_SetLeft(uint16_t dmm); // écriture registre 0x0804 (1/10 mm)
void ARM_SetRight(uint16_t dmm); // écriture registre 0x0805 (1/10 mm) void ARM_SetRight(uint16_t dmm); // écriture registre 0x0805 (1/10 mm)
void ARM_SetLeft(uint16_t pos); // écriture registre 0x0804 (1/10 mm)
void ARM_SetRight(uint16_t pos); // écriture registre 0x0805 (1/10 mm)
uint16_t ARM_GetLeft(void); // lecture position gauche courante
uint16_t ARM_GetRight(void); // lecture position droite courante
void ARM_SetLeft(uint16_t dmm); // écriture registre 0x0804 (1/10 mm)
void ARM_SetRight(uint16_t dmm); // écriture registre 0x0805 (1/10 mm)
uint16_t ARM_GetLeft(void); uint16_t ARM_GetLeft(void);
uint16_t ARM_GetRight(void); uint16_t ARM_GetRight(void);

68
src/arm.c
View File

@ -1,5 +1,6 @@
#include <arm.h> #include <arm.h>
#include <eeprom.h> #include <eeprom.h>
#include <spool.h>
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// Machine d'état traverse // Machine d'état traverse
@ -18,7 +19,7 @@ typedef enum {
// Constantes // Constantes
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
#define ARM_WAIT_STEPS 1200U // 3/4 tour SPOOL (200pas × 8µstep × 3/4) #define ARM_WAIT_STEPS 600U // 3/4 tour SPOOL (200pas × 4µstep × 3/4)
#define ARM_DIR_RIGHT 1 #define ARM_DIR_RIGHT 1
#define ARM_DIR_LEFT 0 #define ARM_DIR_LEFT 0
@ -26,20 +27,18 @@ typedef enum {
// État interne // État interne
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
static volatile uint16_t gear_um; // déplacement µm par tour SPOOL static volatile uint16_t gear_um; // déplacement µm par tour SPOOL
static volatile uint32_t accumulator; // accumulateur µm static volatile uint32_t accumulator; // accumulateur µm
static uint16_t arm_offset; // distance capteur home → origine (µm) static uint16_t arm_offset; // distance capteur home → origine (µm)
static volatile uint8_t free_arm; // 1 = vgear désactivé, moteur libre static volatile uint8_t free_arm; // 1 = vgear désactivé, moteur libre
static uint16_t pos_left; // limite gauche (1/10 mm) static uint16_t pos_left; // limite gauche (1/10 mm)
static uint16_t pos_right; // limite droite (1/10 mm) static uint16_t pos_right; // limite droite (1/10 mm)
static volatile arm_state_t state; // état courant de la machine static volatile arm_state_t state; // état courant
static volatile uint16_t arm_pos; // position courante en micropas depuis LEFT static volatile uint16_t wait_ctr; // compteur steps SPOOL pour les pauses
static volatile uint16_t wait_ctr; // compteur steps SPOOL pour les pauses static uint16_t travel_steps; // micropas pour aller de LEFT à RIGHT
static uint16_t travel_steps; // micropas pour aller de LEFT à RIGHT
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// Calcul du trajet LEFT→RIGHT en micropas // Calcul du trajet LEFT→RIGHT en micropas
// (pos_right - pos_left) en 1/10mm → µm → micropas
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
static uint16_t ARM_CalcTravelSteps(void) static uint16_t ARM_CalcTravelSteps(void)
@ -60,7 +59,6 @@ void ARM_Init(void)
pos_left = ARM_LEFT_DEFAULT; pos_left = ARM_LEFT_DEFAULT;
pos_right = ARM_RIGHT_DEFAULT; pos_right = ARM_RIGHT_DEFAULT;
state = ARM_IDLE; state = ARM_IDLE;
arm_pos = 0;
wait_ctr = 0; wait_ctr = 0;
travel_steps = ARM_CalcTravelSteps(); travel_steps = ARM_CalcTravelSteps();
@ -71,7 +69,7 @@ void ARM_Init(void)
} }
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// Enable / Dir — appelés depuis homing ou PLC // Enable / Dir
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
void ARM_Enable(uint8_t state_en) void ARM_Enable(uint8_t state_en)
@ -85,10 +83,12 @@ void ARM_SetDir(uint8_t dir)
} }
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// Step — appelé depuis l'ISR SPOOL // UpdateTarget — appelée depuis l'ISR SPOOL
// Machine d'état légère : accumule µm et met
// à jour arm_target_position (pas de steps ici)
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
void ARM_Step(void) void ARM_UpdateTarget(void)
{ {
if (free_arm || gear_um == 0) if (free_arm || gear_um == 0)
return; return;
@ -99,19 +99,15 @@ void ARM_Step(void)
break; break;
case ARM_GO_TO_ZERO: case ARM_GO_TO_ZERO:
// Déplacement vers LEFT — on décrémente arm_pos
accumulator += gear_um; accumulator += gear_um;
while (accumulator >= ARM_UM_PER_REV) { while (accumulator >= ARM_UM_PER_REV) {
accumulator -= ARM_UM_PER_REV; accumulator -= ARM_UM_PER_REV;
if (arm_pos > 0) { if (arm_target_position > 0)
ARM_HSTEP(); arm_target_position--;
arm_pos--; else {
} else {
// Arrivée à LEFT
accumulator = 0; accumulator = 0;
wait_ctr = 0; wait_ctr = 0;
state = ARM_WAIT_LEFT; state = ARM_WAIT_LEFT;
ARM_REV(ARM_DIR_RIGHT);
} }
} }
break; break;
@ -120,24 +116,22 @@ void ARM_Step(void)
wait_ctr++; wait_ctr++;
if (wait_ctr >= ARM_WAIT_STEPS) { if (wait_ctr >= ARM_WAIT_STEPS) {
wait_ctr = 0; wait_ctr = 0;
state = ARM_TRAVERSE; ARM_REV(ARM_DIR_RIGHT);
state = ARM_TRAVERSE;
} }
break; break;
case ARM_TRAVERSE: case ARM_TRAVERSE:
// Déplacement vers RIGHT
accumulator += gear_um; accumulator += gear_um;
while (accumulator >= ARM_UM_PER_REV) { while (accumulator >= ARM_UM_PER_REV) {
accumulator -= ARM_UM_PER_REV; accumulator -= ARM_UM_PER_REV;
if (arm_pos < travel_steps) { if (arm_target_position < travel_steps)
ARM_HSTEP(); arm_target_position++;
arm_pos++; else {
} else {
// Arrivée à RIGHT
accumulator = 0; accumulator = 0;
wait_ctr = 0; wait_ctr = 0;
state = ARM_WAIT_RIGHT;
ARM_REV(ARM_DIR_LEFT); ARM_REV(ARM_DIR_LEFT);
state = ARM_WAIT_RIGHT;
} }
} }
break; break;
@ -151,19 +145,16 @@ void ARM_Step(void)
break; break;
case ARM_RETURN: case ARM_RETURN:
// Déplacement vers LEFT
accumulator += gear_um; accumulator += gear_um;
while (accumulator >= ARM_UM_PER_REV) { while (accumulator >= ARM_UM_PER_REV) {
accumulator -= ARM_UM_PER_REV; accumulator -= ARM_UM_PER_REV;
if (arm_pos > 0) { if (arm_target_position > 0)
ARM_HSTEP(); arm_target_position--;
arm_pos--; else {
} else {
// Arrivée à LEFT
accumulator = 0; accumulator = 0;
wait_ctr = 0; wait_ctr = 0;
state = ARM_WAIT_LEFT;
ARM_REV(ARM_DIR_RIGHT); ARM_REV(ARM_DIR_RIGHT);
state = ARM_WAIT_LEFT;
} }
} }
break; break;
@ -172,7 +163,6 @@ void ARM_Step(void)
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// GO_TO_ZERO — retour à LEFT, nouvelle bobine // GO_TO_ZERO — retour à LEFT, nouvelle bobine
// appelé depuis REG_CTRL bit12
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
void ARM_GoToZero(void) void ARM_GoToZero(void)

48
src/spool.c
View File

@ -1,5 +1,6 @@
#include <spool.h> #include <spool.h>
#include <arm.h> #include <arm.h>
#include <eeprom.h>
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// État interne // État interne
@ -7,6 +8,10 @@
static volatile uint16_t reload; static volatile uint16_t reload;
static volatile uint8_t running; static volatile uint8_t running;
static uint16_t ratio;
static uint16_t current_freq;
volatile uint16_t arm_target_position; // position cible ARM en micropas, mise à jour par ISR
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// Calcul du reload Timer5 // Calcul du reload Timer5
@ -16,7 +21,7 @@ static volatile uint8_t running;
static uint16_t SPOOL_CalcReload(uint16_t hz) static uint16_t SPOOL_CalcReload(uint16_t hz)
{ {
return (uint16_t)(65536UL - (8000000UL / ((uint32_t)hz * 2))); return (uint16_t)(65536UL - (8000000UL / ((uint32_t)hz * 2UL)));
} }
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
@ -25,23 +30,26 @@ static uint16_t SPOOL_CalcReload(uint16_t hz)
void SPOOL_Init(void) void SPOOL_Init(void)
{ {
reload = 0; reload = 0;
running = 0; running = 0;
arm_target_position = 0;
ratio = EE_ReadWord(EE_SPOOL_RATIO_H, EE_SPOOL_RATIO_L);
if (ratio == 0) ratio = 1000;
MOTOR_EN(0); MOTOR_EN(0);
MOTOR_REV(0); MOTOR_REV(0);
// Timer5 off, prescaler 1:1, 16bit, Fosc/4
T5CON = 0b00000000; T5CON = 0b00000000;
TMR5H = 0; TMR5H = 0;
TMR5L = 0; TMR5L = 0;
PIR3bits.TMR5IF = 0; PIR3bits.TMR5IF = 0;
IPR3bits.TMR5IP = 0; // basse priorité IPR3bits.TMR5IP = 1; // haute priorité
PIE3bits.TMR5IE = 1; PIE3bits.TMR5IE = 1;
} }
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// ISR Timer5 — basse priorité // ISR Timer5 — haute priorité
// Génère le step SPOOL, délègue ARM au main
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
void SPOOL_TimerISR(void) void SPOOL_TimerISR(void)
@ -51,12 +59,11 @@ void SPOOL_TimerISR(void)
TMR5L = reload & 0xFF; TMR5L = reload & 0xFF;
MOTOR_HSTEP(); MOTOR_HSTEP();
ARM_Step(); ARM_UpdateTarget(); // machine d'état ARM — met à jour arm_target_position
} }
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
// Écriture registre CTRL (0x0800) // Écriture registre CTRL (0x0800)
// bit15 = ENABLE, bit14 = DIR
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
void SPOOL_SetCtrl(uint16_t ctrl) void SPOOL_SetCtrl(uint16_t ctrl)
@ -64,8 +71,6 @@ void SPOOL_SetCtrl(uint16_t ctrl)
MOTOR_EN((ctrl & CTRL_SPOOL_ENABLE) ? 1 : 0); MOTOR_EN((ctrl & CTRL_SPOOL_ENABLE) ? 1 : 0);
MOTOR_REV((ctrl & CTRL_SPOOL_DIR) ? 1 : 0); MOTOR_REV((ctrl & CTRL_SPOOL_DIR) ? 1 : 0);
ARM_SetFree((ctrl & CTRL_ARM_FREE) ? 1 : 0); ARM_SetFree((ctrl & CTRL_ARM_FREE) ? 1 : 0);
// CTRL_ARM_HOMING : one-shot, traité dans RTU_Task
} }
// ───────────────────────────────────────────── // ─────────────────────────────────────────────
@ -74,27 +79,40 @@ void SPOOL_SetCtrl(uint16_t ctrl)
void SPOOL_SetFreq(uint16_t hz) void SPOOL_SetFreq(uint16_t hz)
{ {
if (hz == current_freq)
return;
current_freq = hz;
if (hz == 0) { if (hz == 0) {
// Arrêt — timer off, position maintenue par ENABLE
T5CONbits.TMR5ON = 0; T5CONbits.TMR5ON = 0;
running = 0; running = 0;
return; return;
} }
// Clamping
if (hz < SPOOL_FREQ_MIN) hz = SPOOL_FREQ_MIN; if (hz < SPOOL_FREQ_MIN) hz = SPOOL_FREQ_MIN;
if (hz > SPOOL_FREQ_MAX) hz = SPOOL_FREQ_MAX; if (hz > SPOOL_FREQ_MAX) hz = SPOOL_FREQ_MAX;
reload = SPOOL_CalcReload(hz); reload = SPOOL_CalcReload(hz);
if (!running) { if (!running) {
// Premier démarrage — charger le timer et démarrer
TMR5H = reload >> 8; TMR5H = reload >> 8;
TMR5L = reload & 0xFF; TMR5L = reload & 0xFF;
PIR3bits.TMR5IF = 0; PIR3bits.TMR5IF = 0;
T5CONbits.TMR5ON = 1; T5CONbits.TMR5ON = 1;
running = 1; running = 1;
} }
// Si déjà running, le nouveau reload sera pris en compte }
// au prochain débordement dans l'ISR
// ─────────────────────────────────────────────
// Écriture registre RATIO (0x0806) + EEPROM
// ─────────────────────────────────────────────
void SPOOL_SetRatio(uint16_t milli)
{
if (milli == 0) milli = 1;
ratio = milli;
EE_WriteWord(EE_SPOOL_RATIO_H, EE_SPOOL_RATIO_L, milli);
if (running && current_freq > 0)
reload = SPOOL_CalcReload(current_freq);
} }