From 39cce4bbde717a9039c63c3c360af857ea61b308 Mon Sep 17 00:00:00 2001 From: Jonathan Roth Date: Tue, 10 Mar 2026 16:02:53 +0100 Subject: [PATCH] dev --- README.md | 75 ++++++++++++--- includes/arm.h | 59 ++++++++++++ includes/eeprom.h | 23 +++++ includes/macros.h | 10 ++ includes/rtu.h | 4 +- includes/spool.h | 46 +++++++++ src/arm.c | 235 ++++++++++++++++++++++++++++++++++++++++++++++ src/eeprom.c | 53 +++++++++++ src/main.c | 14 +-- src/rtu.c | 155 +++++++++++++++++++++++++++--- src/setup.c | 4 + src/spool.c | 100 ++++++++++++++++++++ 12 files changed, 742 insertions(+), 36 deletions(-) create mode 100644 includes/arm.h create mode 100644 includes/eeprom.h create mode 100644 includes/spool.h create mode 100644 src/arm.c create mode 100644 src/eeprom.c create mode 100644 src/spool.c diff --git a/README.md b/README.md index f5529b8..912fb5c 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ # fdr1-modbus-slave -Firmware Modbus RTU esclave pour encodeur incrémental, conçu pour tourner sur la carte **FDR1 v1.00**. +Firmware Modbus RTU esclave pour trancanneur industriel, conçu pour tourner sur la carte **FDR1 v1.00**. -Le PIC lit la position du codeur QEI et la met à disposition d'un maître Modbus RTU (OpenPLC ou autre) via liaison série RS232/RS485. +Le PIC lit la vitesse du fil via le périphérique QEI, pilote le moteur de bobine (SPOOL) et synchronise le guide-fil (ARM) via un virtual gear configurable. La communication avec le maître (OpenPLC ou autre) se fait en Modbus RTU RS232/RS485. --- @@ -12,24 +12,73 @@ Le PIC lit la position du codeur QEI et la met à disposition d'un maître Modbu - Microcontrôleur **PIC18F4431** - Quartz **8 MHz** (HSPLL x4 → Fosc = 32 MHz) - Encodeur incrémental raccordé au périphérique QEI +- 2 drivers pas à pas Step/Dir (SPOOL + ARM) - Liaison série RS232 (RS485 à venir) --- ## Configuration Modbus RTU -| Paramètre | Valeur | -|------------------|------------| -| Adresse esclave | 183 | -| Vitesse | 115200 bps | -| Format | 8N1 | -| Function code | FC03 | +| Paramètre | Valeur | +|-----------------|------------| +| Adresse esclave | 183 | +| Vitesse | 115200 bps | +| Format | 8N1 | +| Function codes | FC03, FC06, FC16 | -### Registres +--- -| Adresse | Nom | Type | Accès | Description | -|----------|-----------|--------|-------|-----------------| -| `0x1700` | HR0 | UINT16 | R | Position QEI | +## Registres Modbus + +### Lecture (FC03) + +| Adresse | Nom | Type | Description | +|----------|-------------|--------|--------------------------| +| `0x1700` | QEI_POS | UINT16 | Vitesse fil (position QEI) | + +### Écriture (FC06 / FC16) + +| Adresse | Nom | Type | Accès | Description | +|----------|-------------|--------|-------|----------------------------------------------------| +| `0x0800` | REG_CTRL | UINT16 | R/W | Registre de contrôle (voir bits ci-dessous) | +| `0x0801` | SPOOL_FREQ | UINT16 | R/W | Fréquence moteur SPOOL (Hz, 0=arrêt) | +| `0x0802` | ARM_GEAR | UINT16 | R/W | Virtual gear ARM (µm par tour SPOOL, 0=arrêt) | +| `0x0803` | ARM_OFFSET | UINT16 | R/W | Distance capteur home → origine (µm) — EEPROM | +| `0x0804` | ARM_LEFT | UINT16 | R/W | Position limite gauche (1/10 mm, défaut: 500) | +| `0x0805` | ARM_RIGHT | UINT16 | R/W | Position limite droite (1/10 mm, défaut: 4500) | +| `0x0806` | SPOOL_RATIO | UINT16 | R/W | Démultiplication SPOOL (millièmes, 1000=1:1) — EEPROM | + +### Bits REG_CTRL (0x0800) + +| Bit | Nom | Description | +|------|-------------------|------------------------------------------------------| +| 15 | SPOOL_ENABLE | Active le moteur SPOOL | +| 14 | SPOOL_DIR | Sens de rotation SPOOL | +| 13 | ARM_HOMING | Lance la procédure de prise d'origine (one-shot) | +| 12 | ARM_GO_TO_ZERO | Retour ARM à LEFT, démarre nouveau cycle (one-shot) | +| 1 | ARM_FREE | Libère le moteur ARM (désactive vgear + ENABLE=0) | + +### EEPROM interne + +| Adresse | Contenu | +|---------|---------------------| +| `0x10` | ARM_OFFSET high | +| `0x11` | ARM_OFFSET low | +| `0x12` | SPOOL_RATIO high | +| `0x13` | SPOOL_RATIO low | + +--- + +## Fonctionnement normal + +En production, seuls deux registres sont utilisés en continu : + +``` +FC03 0x1700 → lecture vitesse fil +FC06 0x0801 → consigne vitesse SPOOL +``` + +Les registres de configuration (0x0802~0x0806) sont écrits une fois à la mise en service. --- @@ -73,4 +122,4 @@ Redistribution and use in source and binary forms, with or without modification, 3. All advertising materials mentioning features or use of this software must display the following acknowledgement: *This product includes software developed by the FDR1 Project.* 4. Neither the name of the FDR1 Project nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. -THIS SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED. +THIS SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED. \ No newline at end of file diff --git a/includes/arm.h b/includes/arm.h new file mode 100644 index 0000000..465f710 --- /dev/null +++ b/includes/arm.h @@ -0,0 +1,59 @@ +#ifndef ARM_H +#define ARM_H + +#include +#include +#include + +// ───────────────────────────────────────────── +// Registres Modbus +// ───────────────────────────────────────────── + +#define ARM_REG_GEAR 0x0802 // déplacement en µm par tour SPOOL +#define ARM_REG_OFFSET 0x0803 // distance capteur home → origine (µm) +#define ARM_REG_LEFT 0x0804 // position limite gauche (1/10 mm) +#define ARM_REG_RIGHT 0x0805 // position limite droite (1/10 mm) + +// ───────────────────────────────────────────── +// Mécanique +// 1 tour ARM = 40mm = 1600 micropas +// 1 micropas ARM = 25µm +// ───────────────────────────────────────────── + +#define ARM_UM_PER_USTEP 25U // µm par micropas +#define ARM_USTEP_PER_REV 1600U // micropas par tour +#define ARM_UM_PER_REV ((uint32_t)ARM_UM_PER_USTEP * ARM_USTEP_PER_REV) // 40000µm + +// ───────────────────────────────────────────── +// Limites gear +// ───────────────────────────────────────────── + +#define ARM_GEAR_MIN 0 // µm (arrêt vgear) +#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 +// ───────────────────────────────────────────── + +void ARM_Init(void); +void ARM_Step(void); // appelé depuis l'ISR SPOOL à chaque step +void ARM_Enable(uint8_t state); // ENABLE moteur 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_GoToZero(void); // retour à LEFT, nouvelle bobine (REG_CTRL bit12) +void ARM_SetGear(uint16_t um); // écriture registre 0x0802 +void ARM_SetOffset(uint16_t um); // écriture registre 0x0803 + sauvegarde EEPROM +uint16_t ARM_GetOffset(void); // lecture offset courant +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_GetRight(void); + +#endif // ARM_H \ No newline at end of file diff --git a/includes/eeprom.h b/includes/eeprom.h new file mode 100644 index 0000000..e9cb364 --- /dev/null +++ b/includes/eeprom.h @@ -0,0 +1,23 @@ +#ifndef EEPROM_H +#define EEPROM_H + +#include +#include + +// ───────────────────────────────────────────── +// Adresses EEPROM +// ───────────────────────────────────────────── + +#define EE_ARM_OFFSET_H 0x10 // ARM offset high byte +#define EE_ARM_OFFSET_L 0x11 // ARM offset low byte + +// ───────────────────────────────────────────── +// API publique +// ───────────────────────────────────────────── + +uint8_t EE_ReadByte(uint8_t addr); +void EE_WriteByte(uint8_t addr, uint8_t data); +uint16_t EE_ReadWord(uint8_t addr_h, uint8_t addr_l); +void EE_WriteWord(uint8_t addr_h, uint8_t addr_l, uint16_t data); + +#endif // EEPROM_H diff --git a/includes/macros.h b/includes/macros.h index 1c07f40..126ab8c 100644 --- a/includes/macros.h +++ b/includes/macros.h @@ -3,6 +3,16 @@ #ifndef __MACROS_H__ #define __MACROS_H__ +#define MOTOR_EN(STATE) {LATBbits.LB4 = !STATE;} +#define MOTOR_STATE() (!LATBbits.LB4) +#define MOTOR_REV(STATE) {LATBbits.LB2 = STATE;} +#define MOTOR_HSTEP() {LATBbits.LB3 = !LATBbits.LB3;} + +#define ARM_EN(STATE) {LATEbits.LE0 = !STATE;} +#define ARM_STATE() (!LATEbits.LE0) +#define ARM_REV(STATE) {LATEbits.LE2 = STATE;} +#define ARM_HSTEP() {LATEbits.LE1 = !LATEbits.LE1;} + #define RED(STATE) {LATDbits.LD0 = STATE;} #define GREEN(STATE) {LATDbits.LD2 = STATE;} #define BLUE(STATE) {LATDbits.LD1 = STATE;} diff --git a/includes/rtu.h b/includes/rtu.h index 1e7ffe8..a6be8f2 100644 --- a/includes/rtu.h +++ b/includes/rtu.h @@ -10,6 +10,8 @@ #define RTU_SLAVE_ADDR 0xB7 #define RTU_FC03 0x03 +#define RTU_FC06 0x06 +#define RTU_FC16 0x10 #define RTU_HR0_ADDR 0x1700 // Position QEI @@ -35,6 +37,6 @@ void RTU_Init(void); void RTU_RxISR(void); // ISR UART RX haute priorité void RTU_TimerISR(void); // ISR Timer2 basse priorité -void RTU_Task(void); // boucle principale +void RTU_Task(void); // boucle principale #endif // RTU_H \ No newline at end of file diff --git a/includes/spool.h b/includes/spool.h new file mode 100644 index 0000000..5508ba0 --- /dev/null +++ b/includes/spool.h @@ -0,0 +1,46 @@ +#ifndef SPOOL_H +#define SPOOL_H + +#include +#include +#include + +// ───────────────────────────────────────────── +// Registres Modbus +// ───────────────────────────────────────────── + +#define REG_CTRL 0x0800 // registre de contrôle commun +#define SPOOL_REG_FREQ 0x0801 // fréquence Step (Hz) + +// bits REG_CTRL +#define CTRL_SPOOL_ENABLE ((uint16_t)(1U << 15)) // bit15 : SPOOL enable +#define CTRL_SPOOL_DIR ((uint16_t)(1U << 14)) // bit14 : SPOOL dir +#define CTRL_ARM_HOMING ((uint16_t)(1U << 13)) // bit13 : ARM homing start +#define CTRL_ARM_GO_TO_ZERO ((uint16_t)(1U << 12)) // bit12 : ARM retour à LEFT +#define CTRL_ARM_FREE ((uint16_t)(1U << 1)) // bit1 : ARM libre + +// ───────────────────────────────────────────── +// Limites +// ───────────────────────────────────────────── + +#define SPOOL_FREQ_MIN 1 // Hz +#define SPOOL_FREQ_MAX 16000 // Hz (600tr/min, 200pas, x8 microstepping) + +// ───────────────────────────────────────────── +// Timer5 +// Fosc/4 = 8MHz, prescaler 1:1 +// reload = 65536 - (8000000 / (freq × 2)) +// ───────────────────────────────────────────── + +#define SPOOL_TIMER_PRESCALER 1 + +// ───────────────────────────────────────────── +// API publique +// ───────────────────────────────────────────── + +void SPOOL_Init(void); +void SPOOL_TimerISR(void); // ISR Timer5 basse priorité +void SPOOL_SetCtrl(uint16_t ctrl); // écriture registre 0x0800 +void SPOOL_SetFreq(uint16_t hz); // écriture registre 0x0801 + +#endif // SPOOL_H \ No newline at end of file diff --git a/src/arm.c b/src/arm.c new file mode 100644 index 0000000..150b702 --- /dev/null +++ b/src/arm.c @@ -0,0 +1,235 @@ +#include +#include + +// ───────────────────────────────────────────── +// Machine d'état traverse +// ───────────────────────────────────────────── + +typedef enum { + ARM_IDLE, + ARM_GO_TO_ZERO, // retour vers LEFT + ARM_WAIT_LEFT, // attente 3/4 tour à LEFT + ARM_TRAVERSE, // déplacement vers RIGHT + ARM_WAIT_RIGHT, // attente 3/4 tour à RIGHT + ARM_RETURN // déplacement vers LEFT +} arm_state_t; + +// ───────────────────────────────────────────── +// Constantes +// ───────────────────────────────────────────── + +#define ARM_WAIT_STEPS 1200U // 3/4 tour SPOOL (200pas × 8µstep × 3/4) +#define ARM_DIR_RIGHT 1 +#define ARM_DIR_LEFT 0 + +// ───────────────────────────────────────────── +// État interne +// ───────────────────────────────────────────── + +static volatile uint16_t gear_um; // déplacement µm par tour SPOOL +static volatile uint32_t accumulator; // accumulateur µm +static uint16_t arm_offset; // distance capteur home → origine (µm) +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_right; // limite droite (1/10 mm) +static volatile arm_state_t state; // état courant de la machine +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 uint16_t travel_steps; // micropas pour aller de LEFT à RIGHT + +// ───────────────────────────────────────────── +// Calcul du trajet LEFT→RIGHT en micropas +// (pos_right - pos_left) en 1/10mm → µm → micropas +// ───────────────────────────────────────────── + +static uint16_t ARM_CalcTravelSteps(void) +{ + uint32_t um = (uint32_t)(pos_right - pos_left) * 100UL; // 1/10mm → µm + return (uint16_t)(um / ARM_UM_PER_USTEP); +} + +// ───────────────────────────────────────────── +// Init — lecture offset EEPROM au démarrage +// ───────────────────────────────────────────── + +void ARM_Init(void) +{ + gear_um = 0; + accumulator = 0; + free_arm = 0; + pos_left = ARM_LEFT_DEFAULT; + pos_right = ARM_RIGHT_DEFAULT; + state = ARM_IDLE; + arm_pos = 0; + wait_ctr = 0; + travel_steps = ARM_CalcTravelSteps(); + + ARM_EN(0); + ARM_REV(0); + + arm_offset = EE_ReadWord(EE_ARM_OFFSET_H, EE_ARM_OFFSET_L); +} + +// ───────────────────────────────────────────── +// Enable / Dir — appelés depuis homing ou PLC +// ───────────────────────────────────────────── + +void ARM_Enable(uint8_t state_en) +{ + ARM_EN(state_en); +} + +void ARM_SetDir(uint8_t dir) +{ + ARM_REV(dir); +} + +// ───────────────────────────────────────────── +// Step — appelé depuis l'ISR SPOOL +// ───────────────────────────────────────────── + +void ARM_Step(void) +{ + if (free_arm || gear_um == 0) + return; + + switch (state) { + + case ARM_IDLE: + break; + + case ARM_GO_TO_ZERO: + // Déplacement vers LEFT — on décrémente arm_pos + accumulator += gear_um; + while (accumulator >= ARM_UM_PER_REV) { + accumulator -= ARM_UM_PER_REV; + if (arm_pos > 0) { + ARM_HSTEP(); + arm_pos--; + } else { + // Arrivée à LEFT + accumulator = 0; + wait_ctr = 0; + state = ARM_WAIT_LEFT; + ARM_REV(ARM_DIR_RIGHT); + } + } + break; + + case ARM_WAIT_LEFT: + wait_ctr++; + if (wait_ctr >= ARM_WAIT_STEPS) { + wait_ctr = 0; + state = ARM_TRAVERSE; + } + break; + + case ARM_TRAVERSE: + // Déplacement vers RIGHT + accumulator += gear_um; + while (accumulator >= ARM_UM_PER_REV) { + accumulator -= ARM_UM_PER_REV; + if (arm_pos < travel_steps) { + ARM_HSTEP(); + arm_pos++; + } else { + // Arrivée à RIGHT + accumulator = 0; + wait_ctr = 0; + state = ARM_WAIT_RIGHT; + ARM_REV(ARM_DIR_LEFT); + } + } + break; + + case ARM_WAIT_RIGHT: + wait_ctr++; + if (wait_ctr >= ARM_WAIT_STEPS) { + wait_ctr = 0; + state = ARM_RETURN; + } + break; + + case ARM_RETURN: + // Déplacement vers LEFT + accumulator += gear_um; + while (accumulator >= ARM_UM_PER_REV) { + accumulator -= ARM_UM_PER_REV; + if (arm_pos > 0) { + ARM_HSTEP(); + arm_pos--; + } else { + // Arrivée à LEFT + accumulator = 0; + wait_ctr = 0; + state = ARM_WAIT_LEFT; + ARM_REV(ARM_DIR_RIGHT); + } + } + break; + } +} + +// ───────────────────────────────────────────── +// GO_TO_ZERO — retour à LEFT, nouvelle bobine +// appelé depuis REG_CTRL bit12 +// ───────────────────────────────────────────── + +void ARM_GoToZero(void) +{ + accumulator = 0; + wait_ctr = 0; + ARM_REV(ARM_DIR_LEFT); + state = ARM_GO_TO_ZERO; +} + +// ───────────────────────────────────────────── +// Écriture registre GEAR (0x0802) +// ───────────────────────────────────────────── + +void ARM_SetGear(uint16_t um) +{ + gear_um = um; + accumulator = 0; +} + +// ───────────────────────────────────────────── +// Écriture registre OFFSET (0x0803) + EEPROM +// ───────────────────────────────────────────── + +void ARM_SetOffset(uint16_t um) +{ + arm_offset = um; + EE_WriteWord(EE_ARM_OFFSET_H, EE_ARM_OFFSET_L, um); +} + +uint16_t ARM_GetOffset(void) +{ + return arm_offset; +} + +// ───────────────────────────────────────────── +// Free ARM — appelé depuis REG_CTRL bit1 +// ───────────────────────────────────────────── + +void ARM_SetFree(uint8_t s) +{ + free_arm = s; + ARM_EN(s ? 0 : 1); +} + +// ───────────────────────────────────────────── +// Positions limites gauche/droite (1/10 mm) +// ───────────────────────────────────────────── + +void ARM_SetLeft(uint16_t dmm) +{ + pos_left = dmm; + travel_steps = ARM_CalcTravelSteps(); +} + +void ARM_SetRight(uint16_t dmm) +{ + pos_right = dmm; + travel_steps = ARM_CalcTravelSteps(); +} \ No newline at end of file diff --git a/src/eeprom.c b/src/eeprom.c new file mode 100644 index 0000000..eeccf34 --- /dev/null +++ b/src/eeprom.c @@ -0,0 +1,53 @@ +#include + +// ───────────────────────────────────────────── +// Lecture d'un octet +// ───────────────────────────────────────────── + +uint8_t EE_ReadByte(uint8_t addr) +{ + EEADR = addr; + EECON1bits.EEPGD = 0; + EECON1bits.CFGS = 0; + EECON1bits.RD = 1; + return EEDATA; +} + +// ───────────────────────────────────────────── +// Écriture d'un octet +// ───────────────────────────────────────────── + +void EE_WriteByte(uint8_t addr, uint8_t data) +{ + EEADR = addr; + EEDATA = data; + EECON1bits.EEPGD = 0; + EECON1bits.CFGS = 0; + EECON1bits.WREN = 1; + INTCONbits.GIE = 0; + EECON2 = 0x55; + EECON2 = 0xAA; + EECON1bits.WR = 1; + while (EECON1bits.WR); + EECON1bits.WREN = 0; + INTCONbits.GIE = 1; +} + +// ───────────────────────────────────────────── +// Lecture d'un mot 16 bits (big endian) +// ───────────────────────────────────────────── + +uint16_t EE_ReadWord(uint8_t addr_h, uint8_t addr_l) +{ + return ((uint16_t)EE_ReadByte(addr_h) << 8) | EE_ReadByte(addr_l); +} + +// ───────────────────────────────────────────── +// Écriture d'un mot 16 bits (big endian) +// ───────────────────────────────────────────── + +void EE_WriteWord(uint8_t addr_h, uint8_t addr_l, uint16_t data) +{ + EE_WriteByte(addr_h, data >> 8); + EE_WriteByte(addr_l, data & 0xFF); +} diff --git a/src/main.c b/src/main.c index 5fd3667..0000a49 100644 --- a/src/main.c +++ b/src/main.c @@ -5,6 +5,7 @@ #include #include #include +#include void main(void) { setup(); @@ -15,16 +16,15 @@ void main(void) { } void __interrupt(low_priority) LOWprio_interrupt(void) { - if( PIR1bits.TMR2IF ) - RTU_TimerISR(); - return; + if (PIR1bits.TMR2IF) + RTU_TimerISR(); + if (PIR3bits.TMR5IF) + SPOOL_TimerISR(); } void __interrupt(high_priority) HIGHprio_interrupt(void) { - if( PIR1bits.RCIF ) - RTU_RxISR(); - - return; + if (PIR1bits.RCIF) + RTU_RxISR(); } diff --git a/src/rtu.c b/src/rtu.c index 15aa48a..c24166a 100644 --- a/src/rtu.c +++ b/src/rtu.c @@ -1,6 +1,7 @@ -#include "rtu.h" -#include "qei.h" -#include +#include +#include +#include +#include // ───────────────────────────────────────────── // Buffers & état interne @@ -68,9 +69,65 @@ static void RTU_SendFC03(uint16_t value) } // ───────────────────────────────────────────── -// Init +// Réponse FC06 — Write Single Register +// Echo de la requête (adresse + valeur) // ───────────────────────────────────────────── +static void RTU_SendFC06(uint16_t reg_addr, uint16_t value) +{ + uint8_t resp[6]; + uint16_t crc; + + resp[0] = RTU_SLAVE_ADDR; + resp[1] = RTU_FC06; + resp[2] = reg_addr >> 8; + resp[3] = reg_addr & 0xFF; + resp[4] = value >> 8; + resp[5] = value & 0xFF; + + crc = RTU_CRC16(resp, 6); + + RTU_SendByte(resp[0]); + RTU_SendByte(resp[1]); + RTU_SendByte(resp[2]); + RTU_SendByte(resp[3]); + RTU_SendByte(resp[4]); + RTU_SendByte(resp[5]); + RTU_SendByte(crc & 0xFF); + RTU_SendByte(crc >> 8); +} + +// ───────────────────────────────────────────── +// Réponse FC16 — Write Multiple Registers +// Echo adresse + nombre de registres écrits +// ───────────────────────────────────────────── + +static void RTU_SendFC16(uint16_t reg_addr, uint16_t reg_count) +{ + uint8_t resp[6]; + uint16_t crc; + + resp[0] = RTU_SLAVE_ADDR; + resp[1] = RTU_FC16; + resp[2] = reg_addr >> 8; + resp[3] = reg_addr & 0xFF; + resp[4] = reg_count >> 8; + resp[5] = reg_count & 0xFF; + + crc = RTU_CRC16(resp, 6); + + RTU_SendByte(resp[0]); + RTU_SendByte(resp[1]); + RTU_SendByte(resp[2]); + RTU_SendByte(resp[3]); + RTU_SendByte(resp[4]); + RTU_SendByte(resp[5]); + RTU_SendByte(crc & 0xFF); + RTU_SendByte(crc >> 8); +} + + + void RTU_Init(void) { rx_len = 0; @@ -138,13 +195,14 @@ void RTU_Task(void) { uint16_t crc_rx, crc_calc; uint16_t reg_addr, reg_count; + uint16_t value; if (!frame_ready) return; frame_ready = 0; - // Trame FC03 minimum : 8 octets + // Trame minimum : 8 octets if (rx_len < 8) goto done; @@ -152,10 +210,6 @@ void RTU_Task(void) if (rx_buf[0] != RTU_SLAVE_ADDR) goto done; - // Function code - if (rx_buf[1] != RTU_FC03) - goto done; - // Vérification CRC crc_rx = (uint16_t)rx_buf[rx_len - 1] << 8 | rx_buf[rx_len - 2]; crc_calc = RTU_CRC16(rx_buf, rx_len - 2); @@ -163,16 +217,87 @@ void RTU_Task(void) if (crc_rx != crc_calc) goto done; - // Adresse et nombre de registres reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3]; reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5]; - // On ne supporte que HR0, 1 registre - if (reg_addr != RTU_HR0_ADDR || reg_count != 1) - goto done; - GREEN(1); - RTU_SendFC03(QEI_ReadPosition()); + + switch (rx_buf[1]) { + + // ── FC03 — Read Holding Registers ──────── + case RTU_FC03: + if (reg_addr != RTU_HR0_ADDR || reg_count != 1) + goto done; + RTU_SendFC03(QEI_ReadPosition()); + break; + + // ── FC06 — Write Single Register ───────── + case RTU_FC06: + value = reg_count; // dans FC06, octets 4-5 = valeur + if (reg_addr == REG_CTRL) { + SPOOL_SetCtrl(value); + if (value & CTRL_ARM_HOMING) { + // TODO: lancer homing + } + if (value & CTRL_ARM_GO_TO_ZERO) + ARM_GoToZero(); + } else if (reg_addr == SPOOL_REG_FREQ) + SPOOL_SetFreq(value); + else if (reg_addr == ARM_REG_GEAR) + ARM_SetGear(value); + else if (reg_addr == ARM_REG_OFFSET) + ARM_SetOffset(value); + else if (reg_addr == ARM_REG_LEFT) + ARM_SetLeft(value); + else if (reg_addr == ARM_REG_RIGHT) + ARM_SetRight(value); + else + goto done; + RTU_SendFC06(reg_addr, value); + break; + + // ── FC16 — Write Multiple Registers ────── + case RTU_FC16: { + uint8_t byte_count = rx_buf[6]; + uint8_t i; + + // On supporte 0x0800~0x0805, registres contigus + if (reg_addr < REG_CTRL || reg_addr + reg_count - 1 > ARM_REG_RIGHT) + goto done; + if (byte_count != reg_count * 2) + goto done; + + // Vérification longueur trame : 9 + byte_count + 2 CRC + if (rx_len < (uint8_t)(9 + byte_count)) + goto done; + + for (i = 0; i < reg_count; i++) { + value = (uint16_t)rx_buf[7 + i * 2] << 8 | rx_buf[8 + i * 2]; + switch (reg_addr + i) { + case REG_CTRL: + SPOOL_SetCtrl(value); + if (value & CTRL_ARM_HOMING) { + // TODO: lancer homing + } + if (value & CTRL_ARM_GO_TO_ZERO) + ARM_GoToZero(); + break; + case SPOOL_REG_FREQ: SPOOL_SetFreq(value); break; + case ARM_REG_GEAR: ARM_SetGear(value); break; + case ARM_REG_OFFSET: ARM_SetOffset(value); break; + case ARM_REG_LEFT: ARM_SetLeft(value); break; + case ARM_REG_RIGHT: ARM_SetRight(value); break; + default: goto done; + } + } + RTU_SendFC16(reg_addr, reg_count); + break; + } + + default: + goto done; + } + GREEN(0); done: diff --git a/src/setup.c b/src/setup.c index 9652950..d8f2183 100644 --- a/src/setup.c +++ b/src/setup.c @@ -6,6 +6,8 @@ #include #include #include +#include +#include void setup(void) { setup_gpio(); @@ -15,6 +17,8 @@ void setup(void) { QEI_Init(); UART_Init(); RTU_Init(); + SPOOL_Init(); + ARM_Init(); INTCON = 0b01010000; // GIE disabled for now, PEIE enable, INT0 enable (always highprio), not int on portB, TMR0 INTCON2 = 0b00000101; // port B pull-up enabled, INT0/1/2 on falling edge diff --git a/src/spool.c b/src/spool.c new file mode 100644 index 0000000..c78733c --- /dev/null +++ b/src/spool.c @@ -0,0 +1,100 @@ +#include +#include + +// ───────────────────────────────────────────── +// État interne +// ───────────────────────────────────────────── + +static volatile uint16_t reload; +static volatile uint8_t running; + +// ───────────────────────────────────────────── +// Calcul du reload Timer5 +// reload = 65536 - (Fosc/4 / (freq × 2)) +// Fosc/4 = 8 000 000, prescaler 1:1 +// ───────────────────────────────────────────── + +static uint16_t SPOOL_CalcReload(uint16_t hz) +{ + return (uint16_t)(65536UL - (8000000UL / ((uint32_t)hz * 2))); +} + +// ───────────────────────────────────────────── +// Init +// ───────────────────────────────────────────── + +void SPOOL_Init(void) +{ + reload = 0; + running = 0; + + MOTOR_EN(0); + MOTOR_REV(0); + + // Timer5 off, prescaler 1:1, 16bit, Fosc/4 + T5CON = 0b00000000; + TMR5H = 0; + TMR5L = 0; + PIR3bits.TMR5IF = 0; + IPR3bits.TMR5IP = 0; // basse priorité + PIE3bits.TMR5IE = 1; +} + +// ───────────────────────────────────────────── +// ISR Timer5 — basse priorité +// ───────────────────────────────────────────── + +void SPOOL_TimerISR(void) +{ + PIR3bits.TMR5IF = 0; + TMR5H = reload >> 8; + TMR5L = reload & 0xFF; + + MOTOR_HSTEP(); + ARM_Step(); +} + +// ───────────────────────────────────────────── +// Écriture registre CTRL (0x0800) +// bit15 = ENABLE, bit14 = DIR +// ───────────────────────────────────────────── + +void SPOOL_SetCtrl(uint16_t ctrl) +{ + MOTOR_EN((ctrl & CTRL_SPOOL_ENABLE) ? 1 : 0); + MOTOR_REV((ctrl & CTRL_SPOOL_DIR) ? 1 : 0); + ARM_SetFree((ctrl & CTRL_ARM_FREE) ? 1 : 0); + + // CTRL_ARM_HOMING : one-shot, traité dans RTU_Task +} + +// ───────────────────────────────────────────── +// Écriture registre FREQ (0x0801) +// ───────────────────────────────────────────── + +void SPOOL_SetFreq(uint16_t hz) +{ + if (hz == 0) { + // Arrêt — timer off, position maintenue par ENABLE + T5CONbits.TMR5ON = 0; + running = 0; + return; + } + + // Clamping + if (hz < SPOOL_FREQ_MIN) hz = SPOOL_FREQ_MIN; + if (hz > SPOOL_FREQ_MAX) hz = SPOOL_FREQ_MAX; + + reload = SPOOL_CalcReload(hz); + + if (!running) { + // Premier démarrage — charger le timer et démarrer + TMR5H = reload >> 8; + TMR5L = reload & 0xFF; + PIR3bits.TMR5IF = 0; + T5CONbits.TMR5ON = 1; + running = 1; + } + // Si déjà running, le nouveau reload sera pris en compte + // au prochain débordement dans l'ISR +} \ No newline at end of file