Skip to content

Commit 9e76d46

Browse files
Jeyem-PauleJeyem-Paule
committed
not working conversion to motor driver
1 parent b4a89d6 commit 9e76d46

File tree

1 file changed

+218
-96
lines changed

1 file changed

+218
-96
lines changed

‎soccer-robots-esp32/src/main.c‎

Lines changed: 218 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include "wifi_connectivity.h"
2+
23
#include "esp_log.h"
34
#include "esp_err.h"
45

@@ -38,16 +39,25 @@
3839
#define BLINK_PERIOD 1000
3940

4041
#define LEDC_MODE LEDC_LOW_SPEED_MODE
41-
#define LEDC_DUTY_RES LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
42-
#define LEDC_TIMER LEDC_TIMER_0
43-
#define LEDC_FREQUENCY 350 // Frequency in Hertz. Set frequency at 4 kHz
42+
#define LEDC_DUTY_RES LEDC_TIMER_13_BIT // 13-bit duty (0–8191)
43+
#define LEDC_TIMER LEDC_TIMER_0
44+
#define LEDC_FREQUENCY 20000 // 20 kHz for DRV8833
45+
46+
//DRV8833 input pins
47+
//Motor A (left): IN1, IN2
48+
#define MOTOR_A_IN1_GPIO GPIO_NUM_16 // DRV8833 IN1 (forward)
49+
#define MOTOR_A_IN2_GPIO GPIO_NUM_18 // DRV8833 IN2 (reverse)
4450

45-
//YELLOW IS LEFT MOTOR, WHITE IS RIGHT MOTOR
46-
#define LEDC_OUTPUT_IO1 18// Define the output GPIO
47-
#define LEDC_OUTPUT_IO2 16 // Define the output GPIO
51+
//Motor B (right): IN3, IN4
52+
#define MOTOR_B_IN1_GPIO GPIO_NUM_12 // DRV8833 IN3 (forward)
53+
#define MOTOR_B_IN2_GPIO GPIO_NUM_11 // DRV8833 IN4 (reverse)
54+
55+
//LEDC channels – 2 per motor (fwd/rev)
56+
#define LEDC_CH_A_FWD LEDC_CHANNEL_0
57+
#define LEDC_CH_A_REV LEDC_CHANNEL_1
58+
#define LEDC_CH_B_FWD LEDC_CHANNEL_2
59+
#define LEDC_CH_B_REV LEDC_CHANNEL_3
4860

49-
#define LEDC_CHANNEL1 LEDC_CHANNEL_0
50-
#define LEDC_CHANNEL2 LEDC_CHANNEL_1
5161

5262
#define FADE_RESOLUTION 10
5363

@@ -77,11 +87,13 @@ static bool charging;
7787
static bool inGame;
7888
static bool resetting;
7989

80-
// used to decide when we start reversing or moving forward and such
90+
// used to decide when we start reversing or moving forward and such (THIS WAS FOR ESC)
91+
/*
8192
static uint8_t lowerReverseBound = 16;
8293
static uint8_t upperReverseBound = 49;
8394
static uint8_t lowerForwardBound = 56;
8495
static uint8_t upperForwardBound = 89;
96+
*/
8597

8698
TaskHandle_t doMovementHandle = NULL;
8799
bool finishedMoving = false;
@@ -563,111 +575,153 @@ float getPercentFromRawDuty(float duty)
563575
return (duty*100)/(pow(2, LEDC_DUTY_RES));
564576
}
565577

566-
//from the direction going from -100 to 100, gets actual duty value needed to send to the motors
567-
float getRawDutyFromBaseDirection(float duty)
578+
// Convert -100..100 "direction" into a raw duty magnitude for PWM
579+
// Sign (positive/negative) is handled separately in move()
580+
float getRawDutyFromBaseDirection(float dir)
568581
{
569-
//if duty is positive, remember valid values are from 86 - 100. If negative, from 36-50
570-
uint8_t range = upperReverseBound - lowerReverseBound;
571-
//if positive movement
572-
if(duty > 1)
573-
{
574-
//first get it between 0 and 1, then multiple to get in the forward range, then add by lower bound to get a range between loewr and upper bound.
575-
//this keeps it in the correct range 56 to 89
576-
duty /= 100.0;
577-
duty *= range;
578-
duty += lowerForwardBound;
579-
}
580-
// if negative movement
581-
else if(duty < -1)
582-
{
583-
//first limit it to the range -1 to 0 by dividing by (100 / range)
584-
duty /= 100.0;
585-
//then put it in the correct range from say -33 to 0 by multiplying by range
586-
duty *= range;
587-
//now, add 33 to put it in the range 0 and 33, and then add 16 to put it in the range 16 to 49
588-
duty += range + lowerReverseBound;
589-
}
590-
// meaning movement is roughly 0, so set it to 53 which makes it about 0.
591-
else
592-
{
593-
duty = 53;
594-
}
595-
//now that we have correct duty cycle numbers, get specifically the raw duty
596-
return getRawDutyFromPercent(duty);
582+
float mag = fabsf(dir); // 0..100
583+
if (mag < 1.0f) {
584+
return 0.0f; // treat tiny values as stop
585+
}
586+
if (mag > 100.0f) {
587+
mag = 100.0f; // clamp
588+
}
589+
return getRawDutyFromPercent(mag);
597590
}
598591

592+
599593
//converts pulse width (in ms) to the proper duty cycle RAW. Not sure if this is right, may have to check
600594
float convertPulseWidthToPercentDuty(int pulseWidth)
601595
{
602596
//get it with pulse width over period. Convert from microseconds to seconds too.
603597
return pulseWidth/(pow(10, 6)/LEDC_FREQUENCY) * 100;
604598
}
605599

606-
// sets up the PWM pins and timer
607600
static void ledc_setup(){
608-
// Timer Configuration
609-
gpio_reset_pin(LEDC_OUTPUT_IO1);
610-
gpio_set_direction(LEDC_OUTPUT_IO1, GPIO_MODE_OUTPUT);
611-
gpio_reset_pin(LEDC_OUTPUT_IO2);
612-
gpio_set_direction(LEDC_OUTPUT_IO2, GPIO_MODE_OUTPUT);
613-
614-
ledc_timer_config_t timer_conf = {
615-
.speed_mode = LEDC_MODE,
616-
.duty_resolution = LEDC_DUTY_RES,
617-
.timer_num = LEDC_TIMER,
618-
.freq_hz = LEDC_FREQUENCY,
619-
.clk_cfg = LEDC_AUTO_CLK
620-
};
621-
ESP_ERROR_CHECK(ledc_timer_config(&timer_conf));
622-
// Channel Configuration
623-
ledc_channel_config_t channel_conf1 = {
624-
.speed_mode = LEDC_MODE,
625-
.channel = LEDC_CHANNEL1,
626-
.timer_sel = LEDC_TIMER,
627-
.intr_type = LEDC_INTR_DISABLE,
628-
.gpio_num = LEDC_OUTPUT_IO1,
629-
.duty = 0,
630-
.hpoint = 0
631-
};
632-
ESP_ERROR_CHECK(ledc_channel_config(&channel_conf1));
633-
634-
//Channel Configuration
635-
ledc_channel_config_t channel_conf2 = {
636-
.speed_mode = LEDC_MODE,
637-
.channel = LEDC_CHANNEL2,
638-
.timer_sel = LEDC_TIMER,
639-
.intr_type = LEDC_INTR_DISABLE,
640-
.gpio_num = LEDC_OUTPUT_IO2,
641-
.duty = 0,
642-
.hpoint = 0
643-
};
644-
ESP_ERROR_CHECK(ledc_channel_config(&channel_conf2));
645-
646-
//50 means neither moves
647-
uint8_t startPos = 50;
648-
//Move the left motor to start position
649-
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CHANNEL1, startPos / 100.0 * pow(2, LEDC_DUTY_RES)));
650-
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CHANNEL1));
651-
652-
//Move the right motor
653-
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CHANNEL2, startPos / 100.0 * pow(2, LEDC_DUTY_RES)));
654-
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CHANNEL2));
601+
// Timer configuration – shared by all channels
602+
ledc_timer_config_t timer_conf = {
603+
.speed_mode = LEDC_MODE,
604+
.duty_resolution = LEDC_DUTY_RES,
605+
.timer_num = LEDC_TIMER,
606+
.freq_hz = LEDC_FREQUENCY,
607+
.clk_cfg = LEDC_AUTO_CLK
608+
};
609+
ESP_ERROR_CHECK(ledc_timer_config(&timer_conf));
610+
611+
// Motor A – forward (IN1)
612+
ledc_channel_config_t ch_a_fwd = {
613+
.speed_mode = LEDC_MODE,
614+
.channel = LEDC_CH_A_FWD,
615+
.timer_sel = LEDC_TIMER,
616+
.intr_type = LEDC_INTR_DISABLE,
617+
.gpio_num = MOTOR_A_IN1_GPIO,
618+
.duty = 0,
619+
.hpoint = 0
620+
};
621+
ESP_ERROR_CHECK(ledc_channel_config(&ch_a_fwd));
622+
623+
// Motor A – reverse (IN2)
624+
ledc_channel_config_t ch_a_rev = {
625+
.speed_mode = LEDC_MODE,
626+
.channel = LEDC_CH_A_REV,
627+
.timer_sel = LEDC_TIMER,
628+
.intr_type = LEDC_INTR_DISABLE,
629+
.gpio_num = MOTOR_A_IN2_GPIO,
630+
.duty = 0,
631+
.hpoint = 0
632+
};
633+
ESP_ERROR_CHECK(ledc_channel_config(&ch_a_rev));
634+
635+
// Motor B – forward (IN3)
636+
ledc_channel_config_t ch_b_fwd = {
637+
.speed_mode = LEDC_MODE,
638+
.channel = LEDC_CH_B_FWD,
639+
.timer_sel = LEDC_TIMER,
640+
.intr_type = LEDC_INTR_DISABLE,
641+
.gpio_num = MOTOR_B_IN1_GPIO,
642+
.duty = 0,
643+
.hpoint = 0
644+
};
645+
ESP_ERROR_CHECK(ledc_channel_config(&ch_b_fwd));
646+
647+
// Motor B – reverse (IN4)
648+
ledc_channel_config_t ch_b_rev = {
649+
.speed_mode = LEDC_MODE,
650+
.channel = LEDC_CH_B_REV,
651+
.timer_sel = LEDC_TIMER,
652+
.intr_type = LEDC_INTR_DISABLE,
653+
.gpio_num = MOTOR_B_IN2_GPIO,
654+
.duty = 0,
655+
.hpoint = 0
656+
};
657+
ESP_ERROR_CHECK(ledc_channel_config(&ch_b_rev));
658+
659+
// Start with everything stopped
660+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_FWD, 0));
661+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_FWD));
662+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_REV, 0));
663+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_REV));
664+
665+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_FWD, 0));
666+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_FWD));
667+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_REV, 0));
668+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_REV));
655669
}
656670

657-
void move(){
658-
//Move the left motor
659-
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CHANNEL1, getRawDutyFromBaseDirection(currentDirection[0])));
660-
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CHANNEL1));
661671

662-
//Move the right motor
663-
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CHANNEL2, getRawDutyFromBaseDirection(currentDirection[1])));
664-
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CHANNEL2));
672+
void move(){
673+
// Left motor = motor A
674+
float dirA = currentDirection[0]; // -100..100
675+
float dutyA = getRawDutyFromBaseDirection(dirA);
676+
677+
if (fabsf(dirA) < 1.0f) {
678+
// Stop: both inputs low -> coast
679+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_FWD, 0));
680+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_FWD));
681+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_REV, 0));
682+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_REV));
683+
} else if (dirA > 0) {
684+
// Forward: PWM on IN1, IN2 low
685+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_FWD, (uint32_t)dutyA));
686+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_FWD));
687+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_REV, 0));
688+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_REV));
689+
} else {
690+
// Reverse: PWM on IN2, IN1 low
691+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_FWD, 0));
692+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_FWD));
693+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_A_REV, (uint32_t)dutyA));
694+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_A_REV));
695+
}
665696

666-
//ACTUAL DUTIES ARE: 56-89 FOR FORWARD (INCLUSIVE)
667-
//AND THEN 16 TO 49 FOR REVERSE (INCLUSIVE), 16 IS FASTER THAN 49
697+
// Right motor = motor B
698+
float dirB = currentDirection[1];
699+
float dutyB = getRawDutyFromBaseDirection(dirB);
700+
701+
if (fabsf(dirB) < 1.0f) {
702+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_FWD, 0));
703+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_FWD));
704+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_REV, 0));
705+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_REV));
706+
} else if (dirB > 0) {
707+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_FWD, (uint32_t)dutyB));
708+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_FWD));
709+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_REV, 0));
710+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_REV));
711+
} else {
712+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_FWD, 0));
713+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_FWD));
714+
ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CH_B_REV, (uint32_t)dutyB));
715+
ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CH_B_REV));
716+
}
668717

718+
// Now:
719+
// - (IN1,IN2) = (PWM,0) → forward with PWM
720+
// - (IN1,IN2) = (0,PWM) → reverse with PWM
721+
// - (0,0) → coast
669722
}
670723

724+
671725
//just quickly putting the on-chip LED to high
672726
void doBlink()
673727
{
@@ -677,7 +731,74 @@ void doBlink()
677731
gpio_set_level(BLINK_GPIO, s_led_state);
678732
}
679733

734+
void app_main(void)
735+
{
736+
//blink so we know the ESP32 booted
737+
//doBlink();
738+
//vTaskDelay(pdMS_TO_TICKS(500));
739+
740+
//set up PWM for the DRV8833 motor driver
741+
ledc_setup();
742+
move();
743+
744+
745+
while (1) {
746+
747+
//both motors forward (about 40% power)
748+
currentDirection[0] = 40; //left motor
749+
currentDirection[1] = 40; //right motor
750+
move();
751+
ESP_LOGI("MOTOR_TEST", "Forward");
752+
vTaskDelay(pdMS_TO_TICKS(2000));
753+
754+
755+
//stop
756+
currentDirection[0] = 0;
757+
currentDirection[1] = 0;
758+
move();
759+
ESP_LOGI("MOTOR_TEST", "Stop");
760+
vTaskDelay(pdMS_TO_TICKS(1500));
761+
762+
763+
//both motors reverse
764+
currentDirection[0] = -40;
765+
currentDirection[1] = -40;
766+
move();
767+
ESP_LOGI("MOTOR_TEST", "Reverse");
768+
vTaskDelay(pdMS_TO_TICKS(2000));
769+
770+
//spin in place (left fwd, right rev)
771+
currentDirection[0] = 40; // Left forward
772+
currentDirection[1] = -40; // Right reverse
773+
move();
774+
ESP_LOGI("MOTOR_TEST", "Spin in place");
775+
vTaskDelay(pdMS_TO_TICKS(2000));
776+
777+
//stop again
778+
currentDirection[0] = 0;
779+
currentDirection[1] = 0;
780+
move();
781+
ESP_LOGI("MOTOR_TEST", "Stop");
782+
vTaskDelay(pdMS_TO_TICKS(1500));
783+
}
784+
785+
}
786+
787+
788+
//for testing the goalpost mechanism only
789+
/*
790+
void app_main() {
791+
//blink LED to know if booted
792+
doBlink();
793+
vTaskDelay(pdMS_TO_TICKS(1000));
794+
795+
//only running the goalpost mechanism
796+
goalpost_mechanism_init(); // <- this never returns (has while(true))
797+
}
798+
*/
799+
680800
//where the program initially starts
801+
/*
681802
void app_main() {
682803
//allocate space for struct, initially set everything to false
683804
moveStruct = malloc(sizeof(Movement));
@@ -700,3 +821,4 @@ void app_main() {
700821
701822
vTaskDelay(pdMS_TO_TICKS(500));
702823
}
824+
*/

0 commit comments

Comments
 (0)