mirror of
https://github.com/m1ngsama/robot_arm.git
synced 2026-03-25 19:53:49 +00:00
m1ngsama
2d36633dcf
All Chinese comments, Serial.println strings, and inline notes translated to English per the project's English-first codebase rule. No logic changes. Closes #11
85 lines
2.7 KiB
C++
85 lines
2.7 KiB
C++
/*
|
||
* ESP32 native hardware PWM servo controller (ESP32 Arduino Core 3.x)
|
||
* Uses the updated ledcAttach/ledcWrite API (replaces deprecated ledcSetup).
|
||
*/
|
||
|
||
// --- 1. Pin assignments ---
|
||
const int PIN_X = 14; // X-axis servo
|
||
const int PIN_Y = 4; // Y-axis servo
|
||
const int PIN_Z = 5; // Z-axis servo
|
||
const int PIN_B = 18; // Base rotation servo
|
||
const int PIN_G = 23; // Gripper servo
|
||
|
||
// --- 2. PWM parameters (standard servo) ---
|
||
const int freq = 50; // 50 Hz (20 ms period)
|
||
const int resolution = 12; // 12-bit resolution (0–4095)
|
||
|
||
// Duty-cycle values for servo angles (12-bit, 50 Hz):
|
||
// 0.5 ms ( 0°) → ~102
|
||
// 1.5 ms ( 90°) → ~307
|
||
// 2.5 ms (180°) → ~512
|
||
|
||
void setup() {
|
||
Serial.begin(115200);
|
||
delay(1000);
|
||
|
||
Serial.println("\n--- ESP32 Core 3.x LEDC driver initializing ---");
|
||
|
||
// Core 3.x API: ledcAttach(pin, frequency, resolution)
|
||
ledcAttach(PIN_X, freq, resolution);
|
||
ledcAttach(PIN_Y, freq, resolution);
|
||
ledcAttach(PIN_Z, freq, resolution);
|
||
ledcAttach(PIN_B, freq, resolution);
|
||
ledcAttach(PIN_G, freq, resolution);
|
||
|
||
// Center all servos at startup
|
||
writeAngle(PIN_X, 90); delay(500);
|
||
writeAngle(PIN_Y, 90); delay(500);
|
||
writeAngle(PIN_Z, 90); delay(500);
|
||
writeAngle(PIN_B, 90); delay(500);
|
||
writeAngle(PIN_G, 90); delay(500);
|
||
|
||
Serial.println("--- 5-axis hardware PWM ready ---");
|
||
}
|
||
|
||
void loop() {
|
||
if (Serial.available() > 0) {
|
||
String cmd = Serial.readStringUntil('\n');
|
||
cmd.trim();
|
||
|
||
if (cmd.startsWith("Servo_ArmX")) writeAngle(PIN_X, extractAngle(cmd, "Servo_ArmX"));
|
||
else if (cmd.startsWith("Servo_ArmY")) writeAngle(PIN_Y, extractAngle(cmd, "Servo_ArmY"));
|
||
else if (cmd.startsWith("Servo_ArmZ")) writeAngle(PIN_Z, extractAngle(cmd, "Servo_ArmZ"));
|
||
else if (cmd.startsWith("Servo_ArmB")) writeAngle(PIN_B, extractAngle(cmd, "Servo_ArmB"));
|
||
else if (cmd.startsWith("Servo_Gripper")) {
|
||
int a = extractAngle(cmd, "Servo_Gripper");
|
||
if (a != -1) {
|
||
writeAngle(PIN_G, a);
|
||
Serial.print("Gripper moved to: "); Serial.println(a);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Write a target angle to a servo pin.
|
||
* pin : GPIO pin number
|
||
* angle: target angle in degrees (0–180, clamped)
|
||
*/
|
||
void writeAngle(int pin, int angle) {
|
||
if (angle < 0) angle = 0;
|
||
if (angle > 180) angle = 180;
|
||
|
||
// Linear map: 0° → duty 102, 180° → duty 512
|
||
int duty = (angle * (512 - 102) / 180) + 102;
|
||
|
||
// Core 3.x API: ledcWrite takes pin number directly
|
||
ledcWrite(pin, duty);
|
||
}
|
||
|
||
/** Extract the numeric argument after a known command prefix. Returns -1 on failure. */
|
||
int extractAngle(String cmd, String prefix) {
|
||
String valPart = cmd.substring(prefix.length());
|
||
if (valPart.length() > 0) return valPart.toInt();
|
||
return -1;
|
||
}
|