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/.pio/
|
||||||
EMG_Arm/.vscode/
|
EMG_Arm/.vscode/
|
||||||
|
|
||||||
# Data (uncomment if you want to exclude)
|
# Large data files (too big for GitHub)
|
||||||
# collected_data/
|
assets/*.hdf5
|
||||||
# models/
|
|
||||||
|
|
||||||
# OS
|
# OS
|
||||||
.DS_Store
|
.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 <freertos/FreeRTOS.h>
|
||||||
#include "driver/gpio.h"
|
#include <freertos/task.h>
|
||||||
#include "driver/ledc.h"
|
#include "servo.h"
|
||||||
|
#include "gestures.h"
|
||||||
|
|
||||||
// Finger servo pin mappings
|
/*******************************************************************************
|
||||||
#define thumbServoPin GPIO_NUM_1
|
* Configuration
|
||||||
#define indexServoPin GPIO_NUM_4
|
******************************************************************************/
|
||||||
#define middleServoPin GPIO_NUM_5
|
|
||||||
#define ringServoPin GPIO_NUM_6
|
|
||||||
#define pinkyServoPin GPIO_NUM_7
|
|
||||||
|
|
||||||
// LEDC channels (one per servo)
|
/** @brief Delay between demo movements in milliseconds. */
|
||||||
#define thumbChannel LEDC_CHANNEL_0
|
#define DEMO_DELAY_MS 1000
|
||||||
#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
|
|
||||||
|
|
||||||
// Arrays for cleaner initialization
|
/*******************************************************************************
|
||||||
const int servoPins[] = {thumbServoPin, indexServoPin, middleServoPin, ringServoPin, pinkyServoPin};
|
* Application Entry Point
|
||||||
const int servoChannels[] = {thumbChannel, indexChannel, middleChannel, ringChannel, pinkyChannel};
|
******************************************************************************/
|
||||||
const int numServos = 5;
|
|
||||||
|
|
||||||
void servoInit() {
|
/**
|
||||||
// LEDC timer configuration (shared by all servos)
|
* @brief Main application entry point.
|
||||||
ledc_timer_config_t ledc_timer = {};
|
*
|
||||||
ledc_timer.speed_mode = LEDC_LOW_SPEED_MODE;
|
* Initializes the servo hardware and runs a demo sequence.
|
||||||
ledc_timer.timer_num = LEDC_TIMER_0;
|
* The demo can be configured to test individual finger control
|
||||||
ledc_timer.duty_resolution = LEDC_TIMER_14_BIT;
|
* or simultaneous hand gestures.
|
||||||
ledc_timer.freq_hz = 50;
|
*/
|
||||||
ledc_timer.clk_cfg = LEDC_AUTO_CLK;
|
void app_main(void)
|
||||||
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
|
{
|
||||||
|
/* Initialize servo motors */
|
||||||
|
servo_init();
|
||||||
|
|
||||||
// Initialize each finger servo channel
|
/* Run demo sequence */
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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) {
|
while (1) {
|
||||||
closeOpenDemo(1000);
|
/* Option 1: Test individual finger control */
|
||||||
|
// demo_individual_fingers(DEMO_DELAY_MS);
|
||||||
|
|
||||||
|
/* Option 2: Test simultaneous hand gestures */
|
||||||
|
demo_close_open(DEMO_DELAY_MS);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
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