Refactor servo control into modular architecture with documentation
Code Structure: - servo.h/c: Low-level servo control (init, PWM, position) - gestures.h/c: High-level finger movements and hand gestures - main.c: Clean application entry point New Features: - servo_degrees_to_duty(): Convert angle (0-180) to duty cycle - servo_set_finger_degrees(): Set finger position using degrees - finger_t enum for type-safe finger identification Documentation: - Added Doxygen-style comments (@file, @brief, @param, etc.) - Professional header blocks for all modules - Inline comments explaining hardware configuration API Functions: - Individual finger control: flex_thumb(), unflex_index(), etc. - Composite gestures: gesture_make_fist(), gesture_open_hand() - Demo sequences: demo_individual_fingers(), demo_close_open()
This commit is contained in:
Surya Balaji
5
.gitignore
vendored
5
.gitignore
vendored
@@ -9,9 +9,8 @@ venv/
|
||||
EMG_Arm/.pio/
|
||||
EMG_Arm/.vscode/
|
||||
|
||||
# Data (uncomment if you want to exclude)
|
||||
# collected_data/
|
||||
# models/
|
||||
# Large data files (too big for GitHub)
|
||||
assets/*.hdf5
|
||||
|
||||
# OS
|
||||
.DS_Store
|
||||
|
||||
140
EMG_Arm/src/gestures.c
Normal file
140
EMG_Arm/src/gestures.c
Normal file
@@ -0,0 +1,140 @@
|
||||
/**
|
||||
* @file gestures.c
|
||||
* @brief Gesture control implementation for the EMG-controlled robotic hand.
|
||||
*
|
||||
* This module implements high-level gesture functions using the low-level
|
||||
* servo control interface. Each gesture function translates intuitive
|
||||
* commands into appropriate servo positions.
|
||||
*/
|
||||
|
||||
#include "gestures.h"
|
||||
#include "servo.h"
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/task.h>
|
||||
|
||||
/*******************************************************************************
|
||||
* Individual Finger Control - Flex Functions
|
||||
******************************************************************************/
|
||||
|
||||
void flex_thumb(void)
|
||||
{
|
||||
servo_set_finger(FINGER_THUMB, SERVO_POS_MAX);
|
||||
}
|
||||
|
||||
void flex_index(void)
|
||||
{
|
||||
servo_set_finger(FINGER_INDEX, SERVO_POS_MAX);
|
||||
}
|
||||
|
||||
void flex_middle(void)
|
||||
{
|
||||
servo_set_finger(FINGER_MIDDLE, SERVO_POS_MAX);
|
||||
}
|
||||
|
||||
void flex_ring(void)
|
||||
{
|
||||
servo_set_finger(FINGER_RING, SERVO_POS_MAX);
|
||||
}
|
||||
|
||||
void flex_pinky(void)
|
||||
{
|
||||
servo_set_finger(FINGER_PINKY, SERVO_POS_MAX);
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Individual Finger Control - Unflex Functions
|
||||
******************************************************************************/
|
||||
|
||||
void unflex_thumb(void)
|
||||
{
|
||||
servo_set_finger(FINGER_THUMB, SERVO_POS_MIN);
|
||||
}
|
||||
|
||||
void unflex_index(void)
|
||||
{
|
||||
servo_set_finger(FINGER_INDEX, SERVO_POS_MIN);
|
||||
}
|
||||
|
||||
void unflex_middle(void)
|
||||
{
|
||||
servo_set_finger(FINGER_MIDDLE, SERVO_POS_MIN);
|
||||
}
|
||||
|
||||
void unflex_ring(void)
|
||||
{
|
||||
servo_set_finger(FINGER_RING, SERVO_POS_MIN);
|
||||
}
|
||||
|
||||
void unflex_pinky(void)
|
||||
{
|
||||
servo_set_finger(FINGER_PINKY, SERVO_POS_MIN);
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Composite Gestures
|
||||
******************************************************************************/
|
||||
|
||||
void gesture_make_fist(void)
|
||||
{
|
||||
/* Flex all fingers simultaneously */
|
||||
flex_thumb();
|
||||
flex_index();
|
||||
flex_middle();
|
||||
flex_ring();
|
||||
flex_pinky();
|
||||
}
|
||||
|
||||
void gesture_open_hand(void)
|
||||
{
|
||||
/* Extend all fingers simultaneously */
|
||||
unflex_thumb();
|
||||
unflex_index();
|
||||
unflex_middle();
|
||||
unflex_ring();
|
||||
unflex_pinky();
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Demo/Test Sequences
|
||||
******************************************************************************/
|
||||
|
||||
void demo_individual_fingers(uint32_t delay_ms)
|
||||
{
|
||||
/* Thumb */
|
||||
flex_thumb();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflex_thumb();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
/* Index */
|
||||
flex_index();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflex_index();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
/* Middle */
|
||||
flex_middle();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflex_middle();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
/* Ring */
|
||||
flex_ring();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflex_ring();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
/* Pinky */
|
||||
flex_pinky();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflex_pinky();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
}
|
||||
|
||||
void demo_close_open(uint32_t delay_ms)
|
||||
{
|
||||
gesture_make_fist();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
gesture_open_hand();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
}
|
||||
128
EMG_Arm/src/gestures.h
Normal file
128
EMG_Arm/src/gestures.h
Normal file
@@ -0,0 +1,128 @@
|
||||
/**
|
||||
* @file gestures.h
|
||||
* @brief Gesture control interface for the EMG-controlled robotic hand.
|
||||
*
|
||||
* This module provides high-level gesture functions for controlling the
|
||||
* robotic hand. It builds upon the low-level servo control module to
|
||||
* implement intuitive finger movements and hand gestures.
|
||||
*
|
||||
* Gesture Categories:
|
||||
* - Individual finger control (flex/unflex each finger)
|
||||
* - Composite gestures (fist, open hand, etc.)
|
||||
* - Demo/test sequences
|
||||
*/
|
||||
|
||||
#ifndef GESTURES_H
|
||||
#define GESTURES_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/*******************************************************************************
|
||||
* Individual Finger Control - Flex Functions
|
||||
*
|
||||
* Flex functions move a finger to the closed (180 degree) position.
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Flex the thumb (move to closed position).
|
||||
*/
|
||||
void flex_thumb(void);
|
||||
|
||||
/**
|
||||
* @brief Flex the index finger (move to closed position).
|
||||
*/
|
||||
void flex_index(void);
|
||||
|
||||
/**
|
||||
* @brief Flex the middle finger (move to closed position).
|
||||
*/
|
||||
void flex_middle(void);
|
||||
|
||||
/**
|
||||
* @brief Flex the ring finger (move to closed position).
|
||||
*/
|
||||
void flex_ring(void);
|
||||
|
||||
/**
|
||||
* @brief Flex the pinky finger (move to closed position).
|
||||
*/
|
||||
void flex_pinky(void);
|
||||
|
||||
/*******************************************************************************
|
||||
* Individual Finger Control - Unflex Functions
|
||||
*
|
||||
* Unflex functions move a finger to the extended (0 degree) position.
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Unflex the thumb (move to extended position).
|
||||
*/
|
||||
void unflex_thumb(void);
|
||||
|
||||
/**
|
||||
* @brief Unflex the index finger (move to extended position).
|
||||
*/
|
||||
void unflex_index(void);
|
||||
|
||||
/**
|
||||
* @brief Unflex the middle finger (move to extended position).
|
||||
*/
|
||||
void unflex_middle(void);
|
||||
|
||||
/**
|
||||
* @brief Unflex the ring finger (move to extended position).
|
||||
*/
|
||||
void unflex_ring(void);
|
||||
|
||||
/**
|
||||
* @brief Unflex the pinky finger (move to extended position).
|
||||
*/
|
||||
void unflex_pinky(void);
|
||||
|
||||
/*******************************************************************************
|
||||
* Composite Gestures
|
||||
*
|
||||
* These functions control multiple fingers simultaneously to form gestures.
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Close all fingers to form a fist.
|
||||
*
|
||||
* All five fingers move to the flexed position simultaneously.
|
||||
*/
|
||||
void gesture_make_fist(void);
|
||||
|
||||
/**
|
||||
* @brief Open the hand fully.
|
||||
*
|
||||
* All five fingers move to the extended position simultaneously.
|
||||
*/
|
||||
void gesture_open_hand(void);
|
||||
|
||||
/*******************************************************************************
|
||||
* Demo/Test Sequences
|
||||
*
|
||||
* These functions provide demonstration sequences for testing servo operation.
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Demo sequence: flex and unflex each finger individually.
|
||||
*
|
||||
* Cycles through each finger, flexing and unflexing with a delay
|
||||
* between each movement. Useful for testing individual servo operation.
|
||||
*
|
||||
* @param delay_ms Delay in milliseconds between each movement.
|
||||
*/
|
||||
void demo_individual_fingers(uint32_t delay_ms);
|
||||
|
||||
/**
|
||||
* @brief Demo sequence: repeatedly close and open the hand.
|
||||
*
|
||||
* Alternates between making a fist and opening the hand.
|
||||
* Useful for testing simultaneous servo operation.
|
||||
*
|
||||
* @param delay_ms Delay in milliseconds between fist and open positions.
|
||||
*/
|
||||
void demo_close_open(uint32_t delay_ms);
|
||||
|
||||
#endif /* GESTURES_H */
|
||||
@@ -1,180 +1,50 @@
|
||||
/**
|
||||
* @file main.c
|
||||
* @brief Main application entry point for the EMG-controlled robotic hand.
|
||||
*
|
||||
* This application controls a 5-finger robotic hand using servo motors.
|
||||
* The servos are driven by PWM signals generated through the ESP32's LEDC
|
||||
* peripheral. Future versions will integrate EMG signal processing to
|
||||
* translate muscle activity into hand gestures.
|
||||
*
|
||||
* Hardware Platform: ESP32-S3-DevKitC-1
|
||||
* Framework: ESP-IDF with FreeRTOS
|
||||
*/
|
||||
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/ledc.h"
|
||||
#include <freertos/task.h>
|
||||
#include "servo.h"
|
||||
#include "gestures.h"
|
||||
|
||||
// Finger servo pin mappings
|
||||
#define thumbServoPin GPIO_NUM_1
|
||||
#define indexServoPin GPIO_NUM_4
|
||||
#define middleServoPin GPIO_NUM_5
|
||||
#define ringServoPin GPIO_NUM_6
|
||||
#define pinkyServoPin GPIO_NUM_7
|
||||
/*******************************************************************************
|
||||
* Configuration
|
||||
******************************************************************************/
|
||||
|
||||
// LEDC channels (one per servo)
|
||||
#define thumbChannel LEDC_CHANNEL_0
|
||||
#define indexChannel LEDC_CHANNEL_1
|
||||
#define middleChannel LEDC_CHANNEL_2
|
||||
#define ringChannel LEDC_CHANNEL_3
|
||||
#define pinkyChannel LEDC_CHANNEL_4
|
||||
#define deg180 2048
|
||||
#define deg0 430
|
||||
/** @brief Delay between demo movements in milliseconds. */
|
||||
#define DEMO_DELAY_MS 1000
|
||||
|
||||
// Arrays for cleaner initialization
|
||||
const int servoPins[] = {thumbServoPin, indexServoPin, middleServoPin, ringServoPin, pinkyServoPin};
|
||||
const int servoChannels[] = {thumbChannel, indexChannel, middleChannel, ringChannel, pinkyChannel};
|
||||
const int numServos = 5;
|
||||
/*******************************************************************************
|
||||
* Application Entry Point
|
||||
******************************************************************************/
|
||||
|
||||
void servoInit() {
|
||||
// LEDC timer configuration (shared by all servos)
|
||||
ledc_timer_config_t ledc_timer = {};
|
||||
ledc_timer.speed_mode = LEDC_LOW_SPEED_MODE;
|
||||
ledc_timer.timer_num = LEDC_TIMER_0;
|
||||
ledc_timer.duty_resolution = LEDC_TIMER_14_BIT;
|
||||
ledc_timer.freq_hz = 50;
|
||||
ledc_timer.clk_cfg = LEDC_AUTO_CLK;
|
||||
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
|
||||
/**
|
||||
* @brief Main application entry point.
|
||||
*
|
||||
* Initializes the servo hardware and runs a demo sequence.
|
||||
* The demo can be configured to test individual finger control
|
||||
* or simultaneous hand gestures.
|
||||
*/
|
||||
void app_main(void)
|
||||
{
|
||||
/* Initialize servo motors */
|
||||
servo_init();
|
||||
|
||||
// Initialize each finger servo channel
|
||||
for (int i = 0; i < numServos; i++) {
|
||||
ledc_channel_config_t ledc_channel = {};
|
||||
ledc_channel.speed_mode = LEDC_LOW_SPEED_MODE;
|
||||
ledc_channel.channel = servoChannels[i];
|
||||
ledc_channel.timer_sel = LEDC_TIMER_0;
|
||||
ledc_channel.intr_type = LEDC_INTR_DISABLE;
|
||||
ledc_channel.gpio_num = servoPins[i];
|
||||
ledc_channel.duty = deg0; // Start with fingers open
|
||||
ledc_channel.hpoint = 0;
|
||||
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
|
||||
}
|
||||
/* Run demo sequence */
|
||||
while (1) {
|
||||
/* Option 1: Test individual finger control */
|
||||
// demo_individual_fingers(DEMO_DELAY_MS);
|
||||
|
||||
/* Option 2: Test simultaneous hand gestures */
|
||||
demo_close_open(DEMO_DELAY_MS);
|
||||
}
|
||||
}
|
||||
|
||||
// Flex functions (move to 180 degrees - finger closed)
|
||||
void flexThumb() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, thumbChannel, deg180);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, thumbChannel);
|
||||
}
|
||||
|
||||
void flexIndex() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, indexChannel, deg180);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, indexChannel);
|
||||
}
|
||||
|
||||
void flexMiddle() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, middleChannel, deg180);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, middleChannel);
|
||||
}
|
||||
|
||||
void flexRing() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, ringChannel, deg180);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, ringChannel);
|
||||
}
|
||||
|
||||
void flexPinky() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, pinkyChannel, deg180);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, pinkyChannel);
|
||||
}
|
||||
|
||||
// Unflex functions (move to 0 degrees - finger open)
|
||||
void unflexThumb() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, thumbChannel, deg0);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, thumbChannel);
|
||||
}
|
||||
|
||||
void unflexIndex() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, indexChannel, deg0);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, indexChannel);
|
||||
}
|
||||
|
||||
void unflexMiddle() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, middleChannel, deg0);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, middleChannel);
|
||||
}
|
||||
|
||||
void unflexRing() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, ringChannel, deg0);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, ringChannel);
|
||||
}
|
||||
|
||||
void unflexPinky() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, pinkyChannel, deg0);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, pinkyChannel);
|
||||
}
|
||||
|
||||
// Combo functions
|
||||
void makeFist() {
|
||||
// Set all duties first
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, thumbChannel, deg180);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, indexChannel, deg180);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, middleChannel, deg180);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, ringChannel, deg180);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, pinkyChannel, deg180);
|
||||
// Update all at once
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, thumbChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, indexChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, middleChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, ringChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, pinkyChannel);
|
||||
}
|
||||
|
||||
void openHand() {
|
||||
// Set all duties first
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, thumbChannel, deg0);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, indexChannel, deg0);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, middleChannel, deg0);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, ringChannel, deg0);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, pinkyChannel, deg0);
|
||||
// Update all at once
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, thumbChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, indexChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, middleChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, ringChannel);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, pinkyChannel);
|
||||
}
|
||||
|
||||
void individualFingerDemo(int delay_ms){
|
||||
flexThumb();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflexThumb();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
flexIndex();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflexIndex();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
flexMiddle();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflexMiddle();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
flexRing();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflexRing();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
|
||||
flexPinky();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
unflexPinky();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
}
|
||||
|
||||
void closeOpenDemo(int delay_ms){
|
||||
makeFist();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
openHand();
|
||||
vTaskDelay(pdMS_TO_TICKS(delay_ms));
|
||||
}
|
||||
|
||||
void app_main() {
|
||||
servoInit();
|
||||
|
||||
// Demo: flex and unflex each finger in sequence
|
||||
// while(1) {
|
||||
// individualFingerDemo(1000);
|
||||
// }
|
||||
|
||||
// Demo: close and open hand
|
||||
while(1) {
|
||||
closeOpenDemo(1000);
|
||||
}
|
||||
}
|
||||
125
EMG_Arm/src/servo.c
Normal file
125
EMG_Arm/src/servo.c
Normal file
@@ -0,0 +1,125 @@
|
||||
/**
|
||||
* @file servo.c
|
||||
* @brief Servo motor control implementation for the EMG-controlled robotic hand.
|
||||
*
|
||||
* This module implements low-level servo control using the ESP32's LEDC peripheral.
|
||||
* The LEDC is configured to generate 50Hz PWM signals suitable for standard hobby servos.
|
||||
*/
|
||||
|
||||
#include "servo.h"
|
||||
#include "esp_err.h"
|
||||
|
||||
/*******************************************************************************
|
||||
* Private Constants
|
||||
******************************************************************************/
|
||||
|
||||
/** @brief PWM frequency for servo control (standard is 50Hz). */
|
||||
#define SERVO_PWM_FREQ_HZ 50
|
||||
|
||||
/** @brief PWM resolution in bits (14-bit = 16384 levels). */
|
||||
#define SERVO_PWM_RESOLUTION LEDC_TIMER_14_BIT
|
||||
|
||||
/** @brief LEDC speed mode (ESP32-S3 only supports low-speed mode). */
|
||||
#define SERVO_SPEED_MODE LEDC_LOW_SPEED_MODE
|
||||
|
||||
/** @brief LEDC timer used for all servos. */
|
||||
#define SERVO_TIMER LEDC_TIMER_0
|
||||
|
||||
/*******************************************************************************
|
||||
* Private Data
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Mapping of finger indices to GPIO pins.
|
||||
*/
|
||||
static const int servo_pins[FINGER_COUNT] = {
|
||||
THUMB_SERVO_PIN,
|
||||
INDEX_SERVO_PIN,
|
||||
MIDDLE_SERVO_PIN,
|
||||
RING_SERVO_PIN,
|
||||
PINKY_SERVO_PIN
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Mapping of finger indices to LEDC channels.
|
||||
*/
|
||||
static const ledc_channel_t servo_channels[FINGER_COUNT] = {
|
||||
THUMB_CHANNEL,
|
||||
INDEX_CHANNEL,
|
||||
MIDDLE_CHANNEL,
|
||||
RING_CHANNEL,
|
||||
PINKY_CHANNEL
|
||||
};
|
||||
|
||||
/*******************************************************************************
|
||||
* Public Function Implementations
|
||||
******************************************************************************/
|
||||
|
||||
void servo_init(void)
|
||||
{
|
||||
/* Configure LEDC timer (shared by all servo channels) */
|
||||
ledc_timer_config_t timer_config = {
|
||||
.speed_mode = SERVO_SPEED_MODE,
|
||||
.timer_num = SERVO_TIMER,
|
||||
.duty_resolution = SERVO_PWM_RESOLUTION,
|
||||
.freq_hz = SERVO_PWM_FREQ_HZ,
|
||||
.clk_cfg = LEDC_AUTO_CLK
|
||||
};
|
||||
ESP_ERROR_CHECK(ledc_timer_config(&timer_config));
|
||||
|
||||
/* Configure LEDC channel for each finger servo */
|
||||
for (int i = 0; i < FINGER_COUNT; i++) {
|
||||
ledc_channel_config_t channel_config = {
|
||||
.speed_mode = SERVO_SPEED_MODE,
|
||||
.channel = servo_channels[i],
|
||||
.timer_sel = SERVO_TIMER,
|
||||
.intr_type = LEDC_INTR_DISABLE,
|
||||
.gpio_num = servo_pins[i],
|
||||
.duty = SERVO_POS_MIN, /* Start with fingers extended */
|
||||
.hpoint = 0
|
||||
};
|
||||
ESP_ERROR_CHECK(ledc_channel_config(&channel_config));
|
||||
}
|
||||
}
|
||||
|
||||
void servo_set_position(ledc_channel_t channel, uint32_t duty)
|
||||
{
|
||||
ledc_set_duty(SERVO_SPEED_MODE, channel, duty);
|
||||
ledc_update_duty(SERVO_SPEED_MODE, channel);
|
||||
}
|
||||
|
||||
void servo_set_finger(finger_t finger, uint32_t duty)
|
||||
{
|
||||
if (finger >= FINGER_COUNT) {
|
||||
return; /* Invalid finger index */
|
||||
}
|
||||
servo_set_position(servo_channels[finger], duty);
|
||||
}
|
||||
|
||||
uint32_t servo_degrees_to_duty(float degrees)
|
||||
{
|
||||
/* Clamp input to valid range */
|
||||
if (degrees < 0.0f) {
|
||||
degrees = 0.0f;
|
||||
} else if (degrees > 180.0f) {
|
||||
degrees = 180.0f;
|
||||
}
|
||||
|
||||
/*
|
||||
* Linear interpolation formula:
|
||||
* duty = min + (degrees / 180) * (max - min)
|
||||
*
|
||||
* Where:
|
||||
* - min = SERVO_POS_MIN (duty at 0 degrees)
|
||||
* - max = SERVO_POS_MAX (duty at 180 degrees)
|
||||
*/
|
||||
float duty = SERVO_POS_MIN + (degrees / 180.0f) * (SERVO_POS_MAX - SERVO_POS_MIN);
|
||||
|
||||
return (uint32_t)duty;
|
||||
}
|
||||
|
||||
void servo_set_finger_degrees(finger_t finger, float degrees)
|
||||
{
|
||||
uint32_t duty = servo_degrees_to_duty(degrees);
|
||||
servo_set_finger(finger, duty);
|
||||
}
|
||||
135
EMG_Arm/src/servo.h
Normal file
135
EMG_Arm/src/servo.h
Normal file
@@ -0,0 +1,135 @@
|
||||
/**
|
||||
* @file servo.h
|
||||
* @brief Servo motor control interface for the EMG-controlled robotic hand.
|
||||
*
|
||||
* This module provides low-level servo motor control using the ESP32's LEDC
|
||||
* (LED Controller) peripheral for PWM generation. It handles initialization
|
||||
* and position control for all five finger servos.
|
||||
*
|
||||
* Hardware Configuration:
|
||||
* - Thumb: GPIO 1, LEDC Channel 0
|
||||
* - Index: GPIO 4, LEDC Channel 1
|
||||
* - Middle: GPIO 5, LEDC Channel 2
|
||||
* - Ring: GPIO 6, LEDC Channel 3
|
||||
* - Pinky: GPIO 7, LEDC Channel 4
|
||||
*
|
||||
* @note All servos share a single LEDC timer configured for 50Hz (standard servo frequency).
|
||||
*/
|
||||
|
||||
#ifndef SERVO_H
|
||||
#define SERVO_H
|
||||
|
||||
#include "driver/ledc.h"
|
||||
#include "driver/gpio.h"
|
||||
|
||||
/*******************************************************************************
|
||||
* GPIO Pin Definitions
|
||||
******************************************************************************/
|
||||
|
||||
#define THUMB_SERVO_PIN GPIO_NUM_1
|
||||
#define INDEX_SERVO_PIN GPIO_NUM_4
|
||||
#define MIDDLE_SERVO_PIN GPIO_NUM_5
|
||||
#define RING_SERVO_PIN GPIO_NUM_6
|
||||
#define PINKY_SERVO_PIN GPIO_NUM_7
|
||||
|
||||
/*******************************************************************************
|
||||
* LEDC Channel Definitions
|
||||
******************************************************************************/
|
||||
|
||||
#define THUMB_CHANNEL LEDC_CHANNEL_0
|
||||
#define INDEX_CHANNEL LEDC_CHANNEL_1
|
||||
#define MIDDLE_CHANNEL LEDC_CHANNEL_2
|
||||
#define RING_CHANNEL LEDC_CHANNEL_3
|
||||
#define PINKY_CHANNEL LEDC_CHANNEL_4
|
||||
|
||||
/*******************************************************************************
|
||||
* Servo Position Definitions
|
||||
*
|
||||
* These duty cycle values correspond to servo positions at 14-bit resolution.
|
||||
* At 50Hz with 14-bit resolution (16384 counts per 20ms period):
|
||||
* - 1ms pulse (~0 degrees) = ~819 counts
|
||||
* - 2ms pulse (~180 degrees) = ~1638 counts
|
||||
*
|
||||
* Actual values may vary based on specific servo characteristics.
|
||||
******************************************************************************/
|
||||
|
||||
#define SERVO_POS_MIN 430 /**< Duty cycle for 0 degrees (finger extended) */
|
||||
#define SERVO_POS_MAX 2048 /**< Duty cycle for 180 degrees (finger flexed) */
|
||||
|
||||
/*******************************************************************************
|
||||
* Finger Index Enumeration
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Enumeration for finger identification.
|
||||
*/
|
||||
typedef enum {
|
||||
FINGER_THUMB = 0,
|
||||
FINGER_INDEX,
|
||||
FINGER_MIDDLE,
|
||||
FINGER_RING,
|
||||
FINGER_PINKY,
|
||||
FINGER_COUNT /**< Total number of fingers (5) */
|
||||
} finger_t;
|
||||
|
||||
/*******************************************************************************
|
||||
* Public Function Declarations
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief Initialize all servo motors.
|
||||
*
|
||||
* Configures the LEDC timer and channels for all five finger servos.
|
||||
* All servos are initialized to the extended (open) position.
|
||||
*
|
||||
* @note Must be called before any other servo functions.
|
||||
*/
|
||||
void servo_init(void);
|
||||
|
||||
/**
|
||||
* @brief Set a specific servo to a given duty cycle position.
|
||||
*
|
||||
* @param channel The LEDC channel corresponding to the servo.
|
||||
* @param duty The duty cycle value (use SERVO_POS_MIN to SERVO_POS_MAX).
|
||||
*/
|
||||
void servo_set_position(ledc_channel_t channel, uint32_t duty);
|
||||
|
||||
/**
|
||||
* @brief Set a finger servo to a specific position.
|
||||
*
|
||||
* @param finger The finger to control (use finger_t enumeration).
|
||||
* @param duty The duty cycle value (use SERVO_POS_MIN to SERVO_POS_MAX).
|
||||
*/
|
||||
void servo_set_finger(finger_t finger, uint32_t duty);
|
||||
|
||||
/**
|
||||
* @brief Convert an angle in degrees to the corresponding duty cycle value.
|
||||
*
|
||||
* Performs linear interpolation between SERVO_POS_MIN (0 degrees) and
|
||||
* SERVO_POS_MAX (180 degrees). Input values are clamped to the valid range.
|
||||
*
|
||||
* @param degrees The desired angle in degrees (0 to 180).
|
||||
* @return The corresponding duty cycle value for the LEDC peripheral.
|
||||
*
|
||||
* @example
|
||||
* uint32_t duty = servo_degrees_to_duty(90); // Get duty for 90 degrees
|
||||
* servo_set_finger(FINGER_INDEX, duty);
|
||||
*/
|
||||
uint32_t servo_degrees_to_duty(float degrees);
|
||||
|
||||
/**
|
||||
* @brief Set a finger servo to a specific angle in degrees.
|
||||
*
|
||||
* Convenience function that combines degree-to-duty conversion with
|
||||
* finger positioning. This is the recommended function for most use cases.
|
||||
*
|
||||
* @param finger The finger to control (use finger_t enumeration).
|
||||
* @param degrees The desired angle in degrees (0 = extended, 180 = flexed).
|
||||
*
|
||||
* @example
|
||||
* servo_set_finger_degrees(FINGER_THUMB, 90); // Move thumb to 90 degrees
|
||||
* servo_set_finger_degrees(FINGER_INDEX, 45); // Move index to 45 degrees
|
||||
*/
|
||||
void servo_set_finger_degrees(finger_t finger, float degrees);
|
||||
|
||||
#endif /* SERVO_H */
|
||||
Reference in New Issue
Block a user