Új hozzászólás Aktív témák

• #23751 Postas99 őstag Aryes #23750

  Új Válasz 2025-09-01 17:31:55 #23751

  Új hozzászólás

  Összes hozzászólása itt Válaszok az összes hozzászólására itt Válaszok erre a hozzászólásra

  Privát üzenet küldése

  

  Postas99

  őstag

  válasz Aryes #23750 üzenetére

  A legtöbb szervo 360-as amin van a maradék 180-as. Itt a kód:

  #include <Arduino.h>
#include "driver/gpio.h"
#include "esp_timer.h"

// ---- SZERVÓ
static const int SERVO_L_PIN = 18; // bal
static const int SERVO_R_PIN = 19; // jobb

// ---- TRIGGER
static const int TRIG_L = 32; // 1x bal: előre->stop->vissza->stop
static const int TRIG_R = 33; // 1x jobb: hátra->stop->előre->stop
static const int TRIG_BOTH = 25; // 1x mindkettő (bal előre, jobb hátra) - belső PULLUP!


static const uint32_t FRAME_US = 20000; // 20 ms, 50 Hz

// ---- BAL/JOBB PULSE (µs) + állapot ----
volatile uint32_t pulse_us_L = 1500; // 1000..2000
volatile uint32_t pulse_us_R = 1500;
volatile bool highPhase_L = false;
volatile bool highPhase_R = false;
portMUX_TYPE muxServo = portMUX_INITIALIZER_UNLOCKED;

// ---- TIMER HANDLE-K ----
esp_timer_handle_t tmrL;
esp_timer_handle_t tmrR;


inline void setLeftPulseUs(uint32_t us) {
if (us < 1000) us = 1000; if (us > 2000) us = 2000;
portENTER_CRITICAL(&muxServo); pulse_us_L = us; portEXIT_CRITICAL(&muxServo);
}
inline void setRightPulseUs(uint32_t us) {
if (us < 1000) us = 1000; if (us > 2000) us = 2000;
portENTER_CRITICAL(&muxServo); pulse_us_R = us; portEXIT_CRITICAL(&muxServo);
}

// ---- PWM
void IRAM_ATTR cbLeft(void*) {
uint32_t p; portENTER_CRITICAL_ISR(&muxServo); p = pulse_us_L; portEXIT_CRITICAL_ISR(&muxServo);
if (!highPhase_L) { gpio_set_level((gpio_num_t)SERVO_L_PIN, 1); highPhase_L = true; esp_timer_start_once(tmrL, p); }
else { gpio_set_level((gpio_num_t)SERVO_L_PIN, 0); highPhase_L = false; esp_timer_start_once(tmrL, (p<FRAME_US)?(FRAME_US-p):1000); }
}
void IRAM_ATTR cbRight(void*) {
uint32_t p; portENTER_CRITICAL_ISR(&muxServo); p = pulse_us_R; portEXIT_CRITICAL_ISR(&muxServo);
if (!highPhase_R) { gpio_set_level((gpio_num_t)SERVO_R_PIN, 1); highPhase_R = true; esp_timer_start_once(tmrR, p); }
else { gpio_set_level((gpio_num_t)SERVO_R_PIN, 0); highPhase_R = false; esp_timer_start_once(tmrR, (p<FRAME_US)?(FRAME_US-p):1000); }
}

// ---------- MOZGÁS
const int NEUTRAL_L = 1500, NEUTRAL_R = 1500;
const int L_FWD_US = 1600, L_BACK_US = 1400;
const int R_FWD_US = 1600, R_BACK_US = 1400;

const unsigned long L_FWD_MS = 190;
const unsigned long L_BACK_MS = 186;
const unsigned long R_FWD_MS = 205;
const unsigned long R_BACK_MS = 181;
const unsigned long STOP1_MS = 1000;
const unsigned long STOP2_MS = 1000;


enum SPhase { S_IDLE, S_FWD, S_STOP1, S_BACK, S_STOP2 };
SPhase L_phase = S_IDLE, R_phase = S_IDLE;
unsigned long L_nextUs = 0, R_nextUs = 0;

// Indítások (csak ha IDLE)
void startLeftCycle() {
if (L_phase != S_IDLE) return;
setLeftPulseUs(L_FWD_US);
L_phase = S_FWD;
L_nextUs = micros() + L_FWD_MS * 1000UL;
}
void startRightCycle_BackThenFwd() {
if (R_phase != S_IDLE) return;
setRightPulseUs(R_BACK_US);
R_phase = S_BACK;
R_nextUs = micros() + R_BACK_MS * 1000UL;
}
void startBothCycleMirrored() {
if (L_phase != S_IDLE || R_phase != S_IDLE) return; // egyszerre csak ha mindkettő szabad
unsigned long now = micros();
setLeftPulseUs(L_FWD_US); L_phase = S_FWD; L_nextUs = now + L_FWD_MS * 1000UL;
setRightPulseUs(R_BACK_US); R_phase = S_BACK; R_nextUs = now + R_BACK_MS * 1000UL;
}


inline void serviceLeft() {
if (L_phase == S_IDLE) return;
unsigned long now = micros();
if ((long)(now - L_nextUs) < 0) return;

switch (L_phase) {
case S_FWD: setLeftPulseUs(NEUTRAL_L); L_phase = S_STOP1; L_nextUs = now + STOP1_MS * 1000UL; break;
case S_STOP1: setLeftPulseUs(L_BACK_US); L_phase = S_BACK; L_nextUs = now + L_BACK_MS * 1000UL; break;
case S_BACK: setLeftPulseUs(NEUTRAL_L); L_phase = S_STOP2; L_nextUs = now + STOP2_MS * 1000UL; break;
case S_STOP2: setLeftPulseUs(NEUTRAL_L); L_phase = S_IDLE; break;
default: break;
}
}
inline void serviceRight() {
if (R_phase == S_IDLE) return;
unsigned long now = micros();
if ((long)(now - R_nextUs) < 0) return;

switch (R_phase) {
case S_BACK: setRightPulseUs(NEUTRAL_R); R_phase = S_STOP1; R_nextUs = now + STOP1_MS * 1000UL; break;
case S_STOP1: setRightPulseUs(R_FWD_US); R_phase = S_FWD; R_nextUs = now + R_FWD_MS * 1000UL; break;
case S_FWD: setRightPulseUs(NEUTRAL_R); R_phase = S_STOP2; R_nextUs = now + STOP2_MS * 1000UL; break;
case S_STOP2: setRightPulseUs(NEUTRAL_R); R_phase = S_IDLE; break;
default: break;
}
}

// ---- GOMBOLVASÁS
const unsigned long DEBOUNCE_MS = 30;
int prevL = HIGH, prevR = HIGH, prevB = HIGH;
unsigned long lastEdgeL = 0, lastEdgeR = 0, lastEdgeB = 0;

void setup() {
// Szervó kimenetek
gpio_set_direction((gpio_num_t)SERVO_L_PIN, GPIO_MODE_OUTPUT);
gpio_set_direction((gpio_num_t)SERVO_R_PIN, GPIO_MODE_OUTPUT);
gpio_set_level((gpio_num_t)SERVO_L_PIN, 0);
gpio_set_level((gpio_num_t)SERVO_R_PIN, 0);

// Két független esp_timer
esp_timer_create_args_t aL = {}; aL.callback = &cbLeft; aL.name = "servoL";
esp_timer_create_args_t aR = {}; aR.callback = &cbRight; aR.name = "servoR";
ESP_ERROR_CHECK(esp_timer_create(&aL, &tmrL));
ESP_ERROR_CHECK(esp_timer_create(&aR, &tmrR));
ESP_ERROR_CHECK(esp_timer_start_once(tmrL, 100));
ESP_ERROR_CHECK(esp_timer_start_once(tmrR, 100));

// Kezdő állás (IDLE, tartó jel 1500 µs)
setLeftPulseUs(NEUTRAL_L);
setRightPulseUs(NEUTRAL_R);


pinMode(TRIG_L, INPUT_PULLUP);
pinMode(TRIG_R, INPUT_PULLUP);
pinMode(TRIG_BOTH, INPUT_PULLUP); // 25-ön VAN belső PULLUP

prevL = digitalRead(TRIG_L);
prevR = digitalRead(TRIG_R);
prevB = digitalRead(TRIG_BOTH);
}

void loop() {
unsigned long nowMs = millis();

// 32: bal
int rl = digitalRead(TRIG_L);
if (rl != prevL) { prevL = rl; lastEdgeL = nowMs; }
if (rl == LOW && (nowMs - lastEdgeL) > DEBOUNCE_MS) {
startLeftCycle();
lastEdgeL = nowMs + 300; // primitív ismétlésgátló
}

// 33: jobb
int rr = digitalRead(TRIG_R);
if (rr != prevR) { prevR = rr; lastEdgeR = nowMs; }
if (rr == LOW && (nowMs - lastEdgeR) > DEBOUNCE_MS) {
startRightCycle_BackThenFwd();
lastEdgeR = nowMs + 300;
}

// 25: egyszerre mindkettő
int rb = digitalRead(TRIG_BOTH);
if (rb != prevB) { prevB = rb; lastEdgeB = nowMs; }
if (rb == LOW && (nowMs - lastEdgeB) > DEBOUNCE_MS) {
startBothCycleMirrored();
lastEdgeB = nowMs + 300;
}


serviceLeft();
serviceRight();

delay(1); // kis yield
}[/M][/M]

Új hozzászólás Aktív témák