ghassan/arduinomega-servo-driver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add code.ino

Browse Source
This commit is contained in:
Ghassan Yusuf 2025-11-03 12:30:09 +03:00
parent 078a5b4902
commit 3070770cf7
1 changed files with 58 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
code.ino Normal file
View File

@ -0,0 +1,58 @@
#define ENVELOPE 0xAA
#define MOTOR1_PWM_PIN 16
#define MOTOR2_PWM_PIN 17
#define PWM_CHANNEL_1 0
#define PWM_CHANNEL_2 1
#define PWM_FREQ 1000
#define PWM_RESOLUTION 8 // 8 bits: 0-255
unsigned long lastReceiveTime = 0;
uint8_t prevMotor1 = 0, prevMotor2 = 0;
void setup() {
Serial.begin(115200);
ledcSetup(PWM_CHANNEL_1, PWM_FREQ, PWM_RESOLUTION);
ledcSetup(PWM_CHANNEL_2, PWM_FREQ, PWM_RESOLUTION);
ledcAttachPin(MOTOR1_PWM_PIN, PWM_CHANNEL_1);
ledcAttachPin(MOTOR2_PWM_PIN, PWM_CHANNEL_2);
}
void loop() {
static enum { WAIT_HEADER, WAIT_M1, WAIT_M2 } state = WAIT_HEADER;
static uint8_t m1_val, m2_val;
while (Serial.available()) {
uint8_t b = Serial.read();
if (state == WAIT_HEADER) {
if (b == ENVELOPE) state = WAIT_M1;
} else if (state == WAIT_M1) {
m1_val = b;
state = WAIT_M2;
} else if (state == WAIT_M2) {
m2_val = b;
// Only update if changed
if (m1_val != prevMotor1) {
ledcWrite(PWM_CHANNEL_1, m1_val);
prevMotor1 = m1_val;
}
if (m2_val != prevMotor2) {
ledcWrite(PWM_CHANNEL_2, m2_val);
prevMotor2 = m2_val;
}
lastReceiveTime = millis();
state = WAIT_HEADER;
}
}
// Motor safety: Stop if no update in 200 ms
if (millis() - lastReceiveTime > 200) {
if (prevMotor1 != 0) {
ledcWrite(PWM_CHANNEL_1, 0);
prevMotor1 = 0;
}
if (prevMotor2 != 0) {
ledcWrite(PWM_CHANNEL_2, 0);
prevMotor2 = 0;
}
}
}