/* ALMC STM32 Robot firmware Version: v3.1.5 (2026-01-25) ALMC_STM32_ROBOT_V3 Version: 2026-01-16_v3.1 (adds: separate failsafe, joystick deadzone/min-start, smooth-start (slew) for M5 + PB8 tool) Pin map (Variant #3): --------------------- USB: PA11 USB_DM (FREE) PA12 USB_DP (FREE) Motors (A4950T) - FULL reverse with PWM on either IN: M1: TIM2 group pins (but using analogWriteFrequency, no direct TIM usage) PA0 M1_AIN1 PA1 M1_AIN2 M2: PA2 M2_AIN1 PA3 M2_AIN2 M3: PB0 M3_AIN1 PB1 M3_AIN2 M4: PA6 M4_AIN1 PA7 M4_AIN2 M5 (chassis speed, closed-loop by Hall): PB6 M5_AIN1 PB7 M5_AIN2 MOSFET PWM (closed-loop by Hall): PB8 MOSFET_PWM Video switch (servo-pulse style): PA8 VIDEO_PWM (50Hz, 1000/1500/2000us) RS485 (USART1): PA9 USART1_TX -> RS485_DI PA10 USART1_RX -> RS485_RO PB12 RS485_DE/RE (HIGH=TX, LOW=RX) Hall sensors: PB3 HALL_M5 PB4 HALL_MOSFET NOTE: PB3/PB4 are JTAG pins -> we disable JTAG, keep SWD. ADC: PA4 Angle1 PA5 Angle2 DS18B20: PC14 1-wire data (single device, minimal driver) Requirements implemented: - Coast in off-phase: when motor=0 or direction change -> both AIN=0 - Full reverse: PWM goes to IN1 or IN2 (one active at a time) - M5 speed set by panel ENC1 in percent (-100..+100) (encoder ticks accumulate percent; 1 tick = +/-SPEED_PER_TICK_PCT%) - 100% maps to M5_RPM_MAX (default 3000 rpm) - Print RS485 frames to Serial when ctrl command changes - Send status packet back over RS485 with sensors + rpm + angles + etc. */ #include #include #include #include "robot_app.h" #include "almc_rs485.h" // ---------------- PIN DEFINES (Variant #3) ---------------- #define M1_AIN1 PA0 #define M1_AIN2 PA1 #define M2_AIN1 PA2 #define M2_AIN2 PA3 #define M3_AIN1 PB0 #define M3_AIN2 PB1 #define M4_AIN1 PA6 #define M4_AIN2 PA7 #define M5_AIN1 PB6 #define M5_AIN2 PB7 #define MOSFET_PWM PB8 #define VIDEO_PWM PA8 #define RS485_TX PA9 #define RS485_RX PA10 #define RS485_DE PB12 #define HALL_M5 PB3 #define HALL_MOS PB4 #define ANG1_ADC PA4 #define ANG2_ADC PA5 #define DS18_PIN PC14 // ---------------- USER TUNABLES ---------------- static const uint32_t RS485_BAUD = 115200; // Motor/MOSFET PWM frequency static const uint32_t PWM_HZ = 20000; // Мы держим команду в шкале 0..1000 (как и раньше) static const uint16_t PWM_CMD_MAX = 1000; // Реальный max зависит от analogWriteResolution() static uint16_t PWM_HW_MAX = 1023; // будет установлен в setup static inline uint16_t pwmScaleCmdToHw(uint16_t cmd0_1000){ if (cmd0_1000 > PWM_CMD_MAX) cmd0_1000 = PWM_CMD_MAX; // scale 0..1000 -> 0..PWM_HW_MAX return (uint16_t)((uint32_t)cmd0_1000 * (uint32_t)PWM_HW_MAX / (uint32_t)PWM_CMD_MAX); } // Video pulse parameters (servo-style) static const uint32_t VIDEO_HZ = 50; static const uint32_t VIDEO_PERIOD_US = 1000000UL / VIDEO_HZ; static const uint16_t VIDEO_US_CH1 = 1000; static const uint16_t VIDEO_US_CH2 = 1500; static const uint16_t VIDEO_US_CH3 = 2000; // Closed-loop timing static const uint32_t RPM_UPDATE_MS = 100; // how often we compute RPM static const uint32_t CTRL_UPDATE_MS = 20; // PI update period ~50Hz // Fail-safe timing: // - M5 + tool (MOSFET) should stop faster for safety // - M1..M4 can keep last command longer (prevents annoying "3 sec on/off" when packets are sparse) static const uint32_t CTRL_STALE_WARN_MS = 1500; // warn if no CTRL for this long static const uint32_t FAILSAFE_SOFT_MS = 30000; // keep running last cmd; mark CTRL as stale after 30s static const uint32_t FAILSAFE_HARD_MS = 600000; // hard stop after 10 min without CTRL (safety) // Joystick shaping (M1..M4) static const int16_t JOY_DEAD = 40; // dead-zone in joystick units (-1000..1000 from panel) static const int16_t CMD_MIN_START = 120; // minimal command to break static friction (0..1000) // Soft-start / slew limits (command units per second, where 1000 == full) static const float SLEW_M5_CMD_PER_S = 500.0f; // M5 duty change speed static const float SLEW_MOS_CMD_PER_S = 500.0f; // tool (MOSFET) duty change speed // Hall: 1 pulse per revolution static const float HALL_PPR_M5 = 1.0f; static const float HALL_PPR_MOS = 1.0f; // Encoder->Speed% step (ENC1 = Speed) int16_t SPEED_PER_TICK_PCT = 1; // 1 tick = +/-1% (editable) int16_t M5_RPM_MAX = 3000; // 100% -> this RPM (editable) int16_t M5_RPM_MIN_NONZERO = 200; // if |speed|>0 but rpm target too small, force this RPM (editable) // IDs for protocol (adjust if your panel expects other) static const uint8_t ROBOT_ID = 3; static const uint8_t PANEL_ID = 1; static const uint8_t PROTO_VER = 1; // if (h->dst != ROBOT_ID) return; // ---------------- Buttons bit mapping (PCF8575 -> pressed mask) ---------------- // IMPORTANT: In panel firmware, buttons mask is inverted: 1 == pressed. // These bit numbers must match the panel mapping. static const uint8_t BIT_JOY2_SW = 0; static const uint8_t BIT_CAM3 = 1; static const uint8_t BIT_CAM2 = 2; static const uint8_t BIT_CAM1 = 3; static const uint8_t BIT_BTN_C = 4; static const uint8_t BIT_BTN_B = 5; static const uint8_t BIT_BTN_A = 6; static const uint8_t BIT_JOY1_SW = 9; static const uint8_t BIT_ENC2_SW = 10; static const uint8_t BIT_ENC2_DT = 11; static const uint8_t BIT_ENC2_CLK = 12; static const uint8_t BIT_ENC1_SW = 13; static const uint8_t BIT_ENC1_DT = 14; static const uint8_t BIT_ENC1_CLK = 15; static inline bool btnPressed(uint16_t buttonsMask, uint8_t bit){ return ((buttonsMask >> bit) & 0x1u) != 0; } static void printPressedList(uint16_t buttonsMask){ // buttonsMask is "pressed mask" (1 = pressed), see panel buildButtonsPressedMask(): buttons = ~pcfRaw. // IMPORTANT: encoder CLK/DT bits represent encoder signal levels and may toggle even when you do nothing. // We intentionally DO NOT print ENC*_CLK/ENC*_DT here to keep logs readable. Serial.print(" pressed:"); bool any = false; #define ADD_BIT(_bit,_name) do { \ if (btnPressed(buttonsMask, (_bit))) { Serial.print(' '); Serial.print((_name)); any = true; } \ } while(0) ADD_BIT(BIT_BTN_A, "BTN_A"); ADD_BIT(BIT_BTN_B, "BTN_B"); ADD_BIT(BIT_BTN_C, "BTN_C"); ADD_BIT(BIT_CAM1, "CAM1"); ADD_BIT(BIT_CAM2, "CAM2"); ADD_BIT(BIT_CAM3, "CAM3"); ADD_BIT(BIT_JOY1_SW, "JOY1_SW"); ADD_BIT(BIT_JOY2_SW, "JOY2_SW"); ADD_BIT(BIT_ENC1_SW, "ENC1_SW"); ADD_BIT(BIT_ENC2_SW, "ENC2_SW"); #undef ADD_BIT if (!any) Serial.print(" (none)"); } // ---------------- RS485/ALMC ---------------- HardwareSerial &RS485 = Serial1; // USART1 static ALMC_Parser parser; static PayloadCtrlState lastCtrl {}; static bool haveCtrl = false; static uint32_t lastCtrlMs = 0; static uint16_t tx_seq = 1; // ---------------- RX stats / diagnostics ---------------- static uint32_t rx_frames_total = 0; static uint32_t rx_ctrl_state_frames = 0; static uint16_t rx_last_seq_ctrl = 0; static uint16_t rx_last_seq_any = 0; static uint32_t rx_last_ms = 0; static bool ctrl_stale = true; // ---------------- HALL / RPM ---------------- static volatile uint32_t hall_m5_pulses = 0; static volatile uint32_t hall_mos_pulses = 0; static float rpm5 = 0.0f; static float rpmm = 0.0f; static uint32_t lastRpmMs = 0; // ---------------- PI Controller ---------------- struct PIReg { float kp; float ki; float integ; float outMin; float outMax; }; static PIReg pi_m5 {0.8f, 3.0f, 0.0f, 0.0f, (float)PWM_CMD_MAX}; static PIReg pi_mos {0.8f, 3.0f, 0.0f, 0.0f, (float)PWM_CMD_MAX}; static inline float clampf(float v, float lo, float hi){ if (v < lo) return lo; if (v > hi) return hi; return v; } static inline int16_t clampi16(int32_t v, int16_t lo, int16_t hi){ if (v < lo) return lo; if (v > hi) return hi; return (int16_t)v; } static inline uint16_t clampu16(int32_t v, uint16_t lo, uint16_t hi){ if (v < (int32_t)lo) return lo; if (v > (int32_t)hi) return hi; return (uint16_t)v; } static float piUpdate(PIReg &c, float err, float dt){ c.integ += err * c.ki * dt; c.integ = clampf(c.integ, c.outMin, c.outMax); float out = err * c.kp + c.integ; return clampf(out, c.outMin, c.outMax); } // Targets // Speed command for M5 in percent (-100..+100). We convert it to signed RPM target. static int16_t speed_pct_m5 = 0; static int16_t target_rpm_m5 = 0; static int16_t target_rpm_mos = 300; // fixed for now (editable if you want) // ---------------- DS18B20 minimal (single device) ---------------- static inline void odLow(uint8_t pin){ pinMode(pin, OUTPUT_OPEN_DRAIN); digitalWrite(pin, LOW); } static inline void odRelease(uint8_t pin){ pinMode(pin, INPUT_PULLUP); } static bool ds18_reset(uint8_t pin){ odLow(pin); delayMicroseconds(480); odRelease(pin); delayMicroseconds(70); bool presence = (digitalRead(pin) == LOW); delayMicroseconds(410); return presence; } static void ds18_writeBit(uint8_t pin, uint8_t b){ odLow(pin); if (b){ delayMicroseconds(6); odRelease(pin); delayMicroseconds(64); } else { delayMicroseconds(60); odRelease(pin); delayMicroseconds(10); } } static uint8_t ds18_readBit(uint8_t pin){ odLow(pin); delayMicroseconds(6); odRelease(pin); delayMicroseconds(9); uint8_t b = digitalRead(pin); delayMicroseconds(55); return b; } static void ds18_writeByte(uint8_t pin, uint8_t v){ for (int i=0;i<8;i++) ds18_writeBit(pin, (v>>i)&1); } static uint8_t ds18_readByte(uint8_t pin){ uint8_t v=0; for (int i=0;i<8;i++) v |= (ds18_readBit(pin)< 0){ analogWrite(m.in2, 0); digitalWrite(m.in2, LOW); analogWrite(m.in1, dutyHw); } else { analogWrite(m.in1, 0); digitalWrite(m.in1, LOW); analogWrite(m.in2, dutyHw); } } static void mosfetWrite(uint16_t dutyCmd){ dutyCmd = clampu16(dutyCmd, 0, PWM_CMD_MAX); analogWrite(MOSFET_PWM, pwmScaleCmdToHw(dutyCmd)); } // ---------------- Video pulse generator on PA8 ---------------- static uint32_t video_last_period_us = 0; static bool video_high = false; static uint32_t video_high_until_us = 0; static uint16_t video_pulse_us = VIDEO_US_CH2; static void videoSetChannel(uint8_t ch){ // expected ch: 1..3 (panel may send 0..2 or 1..3; we normalize) if (ch == 0) ch = 1; if (ch > 3) ch = 3; video_pulse_us = (ch==1)?VIDEO_US_CH1 : (ch==2)?VIDEO_US_CH2 : VIDEO_US_CH3; } static void videoTask(){ uint32_t now = micros(); if (!video_high){ if ((uint32_t)(now - video_last_period_us) >= VIDEO_PERIOD_US){ video_last_period_us = now; digitalWrite(VIDEO_PWM, HIGH); video_high = true; video_high_until_us = now + (uint32_t)video_pulse_us; } } else { if ((int32_t)(now - video_high_until_us) >= 0){ digitalWrite(VIDEO_PWM, LOW); video_high = false; } } } // ---------------- RS485 send helpers ---------------- static inline void rs485Tx(bool en){ digitalWrite(RS485_DE, en ? HIGH : LOW); } static void rs485Send(const uint8_t* data, uint16_t len){ rs485Tx(true); delayMicroseconds(40); RS485.write(data, len); RS485.flush(); delayMicroseconds(40); rs485Tx(false); } // ---------------- Status payload to panel ---------------- #pragma pack(push,1) typedef struct { int16_t rpm_m5; // rpm * 1 int16_t rpm_mos; // rpm * 1 int16_t tgt_m5; // target rpm int16_t tgt_mos; // target rpm uint16_t ang1_adc; // 0..4095 uint16_t ang2_adc; // 0..4095 int16_t temp_x10; // degC*10 (e.g. 253 => 25.3C), -32768 if NA uint32_t hall5_pulses; // total pulses since boot uint32_t hallm_pulses; // total pulses since boot uint8_t cam_ch; // 1..3 uint8_t flags; // future } PayloadStatusRobotV3; #pragma pack(pop) static void sendStatusToPanel(){ PayloadStatusRobotV3 st{}; st.rpm_m5 = (int16_t)clampi16((int32_t)lroundf(rpm5), -32768, 32767); st.rpm_mos = (int16_t)clampi16((int32_t)lroundf(rpmm), -32768, 32767); st.tgt_m5 = target_rpm_m5; st.tgt_mos = target_rpm_mos; st.ang1_adc = (uint16_t)analogRead(ANG1_ADC); st.ang2_adc = (uint16_t)analogRead(ANG2_ADC); int16_t t_x10 = -32768; (void)ds18_readTempC_x10(t_x10); st.temp_x10 = t_x10; st.hall5_pulses = hall_m5_pulses; st.hallm_pulses = hall_mos_pulses; st.cam_ch = (uint8_t)((video_pulse_us==VIDEO_US_CH1)?1 : (video_pulse_us==VIDEO_US_CH2)?2 : 3); st.flags = 0; // Build ALMC frame uint8_t frame[256]; PktHeader h{}; h.magic[0]='A'; h.magic[1]='L'; h.magic[2]='M'; h.magic[3]='C'; h.ver = PROTO_VER; h.type = PKT_STATUS_ROBOT; h.src = ROBOT_ID; h.dst = PANEL_ID; h.seq = tx_seq++; h.len = (uint16_t)sizeof(st); h.crc = 0; uint16_t need = (uint16_t)(sizeof(PktHeader) + sizeof(st)); memcpy(frame, &h, sizeof(h)); memcpy(frame + sizeof(h), &st, sizeof(st)); // CRC16 over header(with crc=0) + payload uint16_t crc = 0xFFFF; crc = crc16_ccitt(crc, frame, sizeof(PktHeader)); crc = crc16_ccitt(crc, frame + sizeof(PktHeader), sizeof(st)); // write CRC into header ((PktHeader*)frame)->crc = crc; rs485Send(frame, need); } // ---------------- Debug printing (only on changes) ---------------- static void printHexFrame(const uint8_t* data, uint16_t len){ for (uint16_t i=0;i= 1000){ uint32_t dtms = now - rx_prev_ms; uint32_t df = rx_frames_total - rx_prev_total; uint32_t dc = rx_ctrl_state_frames - rx_prev_ctrl; rx_prev_total = rx_frames_total; rx_prev_ctrl = rx_ctrl_state_frames; rx_prev_ms = now; Serial.print("[RX] frames/s="); uint32_t fps = (dtms>0) ? (uint32_t)((df*1000 + dtms/2)/dtms) : 0; // rounded uint32_t cps = (dtms>0) ? (uint32_t)((dc*1000 + dtms/2)/dtms) : 0; // rounded Serial.print(fps); Serial.print(" ctrl/s="); Serial.print(cps); Serial.print(" seqAny="); Serial.print(rx_last_seq_any); Serial.print(" seqCtrl="); Serial.print(rx_last_seq_ctrl); Serial.print(" ctrlAge="); Serial.print(haveCtrl ? (now - lastCtrlMs) : 9999); Serial.print(" stale="); Serial.println(ctrl_stale ? 1 : 0); } if ((now - lastRpmMs) < RPM_UPDATE_MS) return; uint32_t dt_ms = now - lastRpmMs; lastRpmMs = now; static uint32_t prev5 = 0, prevm = 0; uint32_t p5 = hall_m5_pulses; uint32_t pm = hall_mos_pulses; uint32_t d5 = p5 - prev5; uint32_t dm = pm - prevm; prev5 = p5; prevm = pm; float dt = (dt_ms > 0) ? ((float)dt_ms / 1000.0f) : 0.1f; rpm5 = ((float)d5 / HALL_PPR_M5) / dt * 60.0f; rpmm = ((float)dm / HALL_PPR_MOS) / dt * 60.0f; } // ---------------- Control loop task ---------------- static uint32_t lastCtrlUpdate = 0; // Slew-limited outputs for smooth start/stop static uint16_t duty5_slew = 0; // 0..1000 static uint16_t dutym_slew = 0; // 0..1000 static inline uint16_t slewToward(uint16_t cur, uint16_t tgt, float maxPerSec, float dt){ if (cur == tgt) return cur; float stepF = maxPerSec * dt; uint16_t step = (uint16_t)lroundf(stepF); if (step < 1) step = 1; if (cur < tgt){ uint32_t nv = (uint32_t)cur + step; return (nv > tgt) ? tgt : (uint16_t)nv; } else { int32_t nv = (int32_t)cur - (int32_t)step; return (nv < (int32_t)tgt) ? tgt : (uint16_t)nv; } } static void controlLoopTask(){ uint32_t now = millis(); if ((now - lastCtrlUpdate) < CTRL_UPDATE_MS) return; float dt = (now - lastCtrlUpdate) / 1000.0f; lastCtrlUpdate = now; // Fail-safe / link-loss behavior // Goal: if packets from the panel drop out, KEEP running the last received command // (prevents annoying 1-2s on/off). For safety, we still do a HARD stop after FAILSAFE_HARD_MS. uint32_t age = haveCtrl ? (now - lastCtrlMs) : 999999UL; bool stale_now = (age > FAILSAFE_SOFT_MS); if (stale_now != ctrl_stale){ ctrl_stale = stale_now; Serial.print("[FAILSAFE] CTRL "); Serial.print(ctrl_stale ? "STALE" : "OK"); Serial.print(" age_ms="); Serial.println(age); } if (age > FAILSAFE_HARD_MS){ // HARD stop (safety) target_rpm_m5 = 0; target_rpm_mos = 0; motorCoast(M1); motorCoast(M2); motorCoast(M3); motorCoast(M4); } // Closed-loop M5 speed float err5 = (float)target_rpm_m5 - rpm5; float out5 = piUpdate(pi_m5, err5, dt); uint16_t duty5_tgt = clampu16((int32_t)lroundf(out5), 0, PWM_CMD_MAX); // Smooth start/stop for M5 duty5_slew = slewToward(duty5_slew, duty5_tgt, SLEW_M5_CMD_PER_S, dt); // (for now: forward only; to add reverse, set target_rpm_m5 negative and map sign) int16_t cmd5 = (target_rpm_m5 >= 0) ? (int16_t)duty5_slew : (int16_t)(- (int16_t)duty5_slew); // soft coast on direction change (protect gearbox/H-bridge) if ((last_cmd_m5 > 0 && cmd5 < 0) || (last_cmd_m5 < 0 && cmd5 > 0)){ motorCoast(M5); delay(2); } motorWrite(M5, cmd5); last_cmd_m5 = cmd5; // Closed-loop MOSFET "speed" float errm = (float)target_rpm_mos - rpmm; float outm = piUpdate(pi_mos, errm, dt); uint16_t dutym_tgt = clampu16((int32_t)lroundf(outm), 0, PWM_CMD_MAX); // Smooth start/stop for tool (MOSFET on PB8) dutym_slew = slewToward(dutym_slew, dutym_tgt, SLEW_MOS_CMD_PER_S, dt); mosfetWrite(dutym_slew); static uint32_t last=0; if (millis()-last>300){ last=millis(); Serial.print("duty5_tgt="); Serial.print(duty5_tgt); Serial.print(" duty5="); Serial.print(duty5_slew); Serial.print(" | dutym_tgt="); Serial.print(dutym_tgt); Serial.print(" dutym="); Serial.println(dutym_slew); } } // ---------------- Control mapping from panel ---------------- // NOTE about ENC mapping: // Your panel firmware now uses **ENC1 = Speed** (for M5) // and **ENC1_SW** is the quick-stop (Speed=0). // In the ALMC PayloadCtrlState struct we still receive fields named // enc2_delta / enc2_sw (historical), but we interpret them as: // enc2_delta -> ENC1 delta (speed change) // enc2_sw -> ENC1 button (speed=0) // This keeps compatibility without changing the binary protocol. static inline int16_t joyToCmd(int16_t v){ // NOTE: panel sends already scaled joystick values in -1000..+1000 (see v0_94_4_fixed13: joyTo1000()). // So here we only apply deadzone + minimal start shaping, and clamp to [-1000..1000]. if (abs(v) < JOY_DEAD) return 0; int32_t x = v; if (x > 1000) x = 1000; if (x < -1000) x = -1000; // minimal start to break static friction if (x > 0 && x < CMD_MIN_START) x = CMD_MIN_START; if (x < 0 && x > -CMD_MIN_START) x = -CMD_MIN_START; return (int16_t)x; } static void onCtrlState(const PayloadCtrlState* s, const uint8_t* rawFrame, uint16_t rawLen){ static uint32_t prev=0; uint32_t now=millis(); Serial.print("CTRL dt="); Serial.println(now-prev); prev=now; bool isFirst = !haveCtrl; bool changed = isFirst || ctrlChanged(*s, lastCtrl); // update stored ctrl + timestamp lastCtrl = *s; haveCtrl = true; lastCtrlMs = now; // video // Panel (v0_94_4_fixed13) sends cam_sel as 0 (none) or 1..3. // Do NOT add +1 here (that would shift to 2..4). if (s->cam_sel >= 1 && s->cam_sel <= 3) { videoSetChannel(s->cam_sel); } // Motors M1..M4 from joysticks (example mapping, adjust as you like) motorWrite(M1, joyToCmd(s->joy1_y)); motorWrite(M2, joyToCmd(s->joy1_x)); motorWrite(M3, joyToCmd(s->joy2_y)); motorWrite(M4, joyToCmd(s->joy2_x)); // ENC1 = Speed (panel). We receive it in fields enc2_delta/enc2_sw for now. // - speed_pct_m5 accumulates encoder ticks in percent (-100..+100) // - then we convert percent -> signed RPM target for the closed-loop M5 control if (s->enc2_delta != 0){ int32_t np = (int32_t)speed_pct_m5 + (int32_t)s->enc2_delta * (int32_t)SPEED_PER_TICK_PCT; speed_pct_m5 = clampi16(np, -100, 100); } // ENC1_SW -> quick stop // Panel sends ENC1_SW both as enc2_sw (repurposed) and also inside buttons mask (active-high pressed mask). if (s->enc2_sw || btnPressed(s->buttons, BIT_ENC1_SW)){ speed_pct_m5 = 0; } // percent -> RPM target if (speed_pct_m5 == 0){ target_rpm_m5 = 0; } else { int32_t mag = (int32_t)lroundf((float)abs(speed_pct_m5) * ((float)M5_RPM_MAX / 100.0f)); if (mag < M5_RPM_MIN_NONZERO) mag = M5_RPM_MIN_NONZERO; target_rpm_m5 = (speed_pct_m5 > 0) ? (int16_t)mag : (int16_t)(-mag); } // Debug: print only on changes if (changed){ Serial.println(); Serial.println("=== RS485 CTRL_STATE CHANGED ==="); Serial.print("joy1_x="); Serial.print(s->joy1_x); Serial.print(" joy1_y="); Serial.print(s->joy1_y); Serial.print(" joy2_x="); Serial.print(s->joy2_x); Serial.print(" joy2_y="); Serial.println(s->joy2_y); Serial.print("buttons=0x"); Serial.print(s->buttons, HEX); printPressedList(s->buttons); Serial.println(); Serial.print(" enc_speed_delta="); Serial.print(s->enc2_delta); Serial.print(" enc_speed_sw="); Serial.print(s->enc2_sw); Serial.print(" cam_sel="); Serial.print(s->cam_sel); Serial.print(" flags=0x"); Serial.println(s->flags, HEX); Serial.print("speed_pct_m5="); Serial.print(speed_pct_m5); Serial.print("% (SPEED_PER_TICK_PCT="); Serial.print(SPEED_PER_TICK_PCT); Serial.println(")"); Serial.print("target_rpm_m5="); Serial.print(target_rpm_m5); Serial.print(" (M5_RPM_MAX="); Serial.print(M5_RPM_MAX); Serial.println(")"); Serial.print("Raw frame len="); Serial.println(rawLen); printHexFrame(rawFrame, rawLen); } } // ---------------- Setup PWM frequencies ---------------- static void setupPwmPins(){ // 10-bit PWM: 0..1023 analogWriteResolution(10); PWM_HW_MAX = 1023; // Частота PWM в этом core задаётся ГЛОБАЛЬНО для analogWrite() analogWriteFrequency(PWM_HZ); const uint8_t pins_pwm[] = { M1_AIN1, M1_AIN2, M2_AIN1, M2_AIN2, M3_AIN1, M3_AIN2, M4_AIN1, M4_AIN2, M5_AIN1, M5_AIN2, MOSFET_PWM }; for (uint8_t p : pins_pwm){ pinMode(p, OUTPUT); analogWrite(p, 0); } // Video pin - software pulses pinMode(VIDEO_PWM, OUTPUT); digitalWrite(VIDEO_PWM, LOW); motorCoast(M1); motorCoast(M2); motorCoast(M3); motorCoast(M4); motorCoast(M5); mosfetWrite(0); } // ---------------- Disable JTAG to free PB3/PB4 (keep SWD) ---------------- static void disableJtagKeepSwd(){ #if defined(STM32F1xx) // In STM32duino, this HAL call is usually available. // It keeps SWD on PA13/PA14 and disables JTAG on PB3/PB4/PA15. __HAL_AFIO_REMAP_SWJ_NOJTAG(); #endif } // ---------------- appSetup / appLoop ---------------- void appSetup(){ Serial.begin(115200); delay(150); Serial.println(); Serial.println("=== ALMC_STM32_ROBOT_V3 boot ==="); Serial.println("Build: 2026-01-25_v3.1.4"); // Free PB3/PB4 for Hall: disableJtagKeepSwd(); // RS485 pinMode(RS485_DE, OUTPUT); rs485Tx(false); // RX by default RS485.setTx(RS485_TX); RS485.setRx(RS485_RX); RS485.begin(RS485_BAUD); // Parser ALMC_Init(&parser); // Hall pinMode(HALL_M5, INPUT_PULLUP); pinMode(HALL_MOS, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(HALL_M5), isrHallM5, RISING); attachInterrupt(digitalPinToInterrupt(HALL_MOS), isrHallMos, RISING); // ADC pinMode(ANG1_ADC, INPUT_ANALOG); pinMode(ANG2_ADC, INPUT_ANALOG); analogReadResolution(12); // DS18B20 line idle pinMode(DS18_PIN, INPUT_PULLUP); // PWM setupPwmPins(); // initial targets // Start with motor stopped. Panel encoder will set speed. speed_pct_m5 = 0; target_rpm_m5 = 0; target_rpm_mos = 3000; // start times lastRpmMs = millis(); lastCtrlUpdate = millis(); // initial video videoSetChannel(2); Serial.println("Init done."); } void appLoop(){ // 1) RX bytes from RS485 -> ALMC parser while (RS485.available()){ uint8_t b = (uint8_t)RS485.read(); if (ALMC_Feed(&parser, b)){ // A full ALMC frame has been collected (CRC already checked in parser) rx_frames_total++; if (parser.frame_len >= sizeof(PktHeader)){ const PktHeader* h = (const PktHeader*)parser.frame; const uint8_t* payload = parser.frame + sizeof(PktHeader); rx_last_seq_any = h->seq; rx_last_ms = millis(); if (h->type == PKT_CTRL_STATE && h->len == sizeof(PayloadCtrlState)){ rx_ctrl_state_frames++; rx_last_seq_ctrl = h->seq; onCtrlState((const PayloadCtrlState*)payload, parser.frame, parser.frame_len); } } } } // 2) periodic tasks rpmTask(); controlLoopTask(); videoTask(); // 3) send status back to panel periodically (every 100ms) static uint32_t lastTx = 0; uint32_t now = millis(); if (now - lastTx >= 100){ lastTx = now; sendStatusToPanel(); } // 4) local debug output (sensors + rpm) every 500ms static uint32_t lastDbg = 0; if (now - lastDbg >= 500){ lastDbg = now; int16_t t_x10 = -32768; bool t_ok = ds18_readTempC_x10(t_x10); uint16_t a1 = (uint16_t)analogRead(ANG1_ADC); uint16_t a2 = (uint16_t)analogRead(ANG2_ADC); Serial.print("rpm5="); Serial.print(rpm5,1); Serial.print(" tgt5="); Serial.print(target_rpm_m5); Serial.print(" | rpmm="); Serial.print(rpmm,1); Serial.print(" tgtm="); Serial.print(target_rpm_mos); Serial.print(" | hall5="); Serial.print(hall_m5_pulses); Serial.print(" hallm="); Serial.print(hall_mos_pulses); Serial.print(" | ang1="); Serial.print(a1); Serial.print(" ang2="); Serial.print(a2); Serial.print(" | T="); if (t_ok){ Serial.print(t_x10 / 10); Serial.print('.'); Serial.print(abs(t_x10 % 10)); Serial.print("C"); } else { Serial.print("NA"); } Serial.print(" | ctrlAge="); Serial.println(haveCtrl ? (now - lastCtrlMs) : 9999); } }