Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[CODE MODIFICATION INQUIRY] - Manipulating RC Controls Output #464

Open
maximagination1 opened this issue Jul 25, 2024 · 33 comments
Open

[CODE MODIFICATION INQUIRY] - Manipulating RC Controls Output #464

maximagination1 opened this issue Jul 25, 2024 · 33 comments

Comments

@maximagination1
Copy link

Hi there,

I appreciate you adding RC controls to the code, this helps! However, how and where in the code can one change the way the motors behave for the control of a specific robot?

To be more specific, I'm building a Hexapedal Robot controlled with your camera application. I want to implement a specific motor control algorithm into your code to move its 6 encoded motors with legs that detect position - 3 rotating at a time when moving forward and in alternate directions when turning either way. Here is the robot with the leg movements I aim to replicate: https://www.youtube.com/watch?v=ISznqY3kESI

Any tips or bits of guidance would be appreciated. If you're willing to help more on this topic, may you give me your email or another direct form of contact? You don't have to, but I'd appreciate it if you can. Thanks!

Best,

Max

@s60sc
Copy link
Owner

s60sc commented Jul 25, 2024

Looks interesting.

The esp mcpwm peripheral can support 6 motors independently, but it would be simpler as a proof of concept to use 2 channels each driving 3 motors.
The MX1508 control board can drive 2 sets of low current motors, left and right

The mcpwm code is in peripherals.cpp - prepMotors() and motor*()
Create duplicates of functions for left and right using different channels - See mcpwm 4.4

The web interface is in appSpecific.cpp - appSpecificWsHandler()
change to:

    case 'M': 
      // motor speed
      motorSpeedL(controlVal);
      motorSpeedR(controlVal);
    break;
    case 'D':
      // steering, put one side in reverse
      controlVal < 0 ? motorSpeedL(controlVal) : motorSpeedR(0 - controlVal);
    break;

@maximagination1
Copy link
Author

Thanks for your reply! I have just read through the MCPWM documentation until the section on "Capture".

Assuming I use up both of the MCPWM units of my ESP32S3, would they able to support 6 encoded (brushed) motors with two hall sensors each? Or if not, would using up 1 hall sensor be enough if I want the motor to only stop in one exact position after each rotation using Capture? I am using these motors - https://www.dfrobot.com/product-1432.html

Would the Synchronize MCPWM feature be needed if I want specific motors (e.g 3) coded in the functions to turn at once so they don't go out of phase?

Thanks for your help.

Max

@s60sc
Copy link
Owner

s60sc commented Jul 26, 2024

I've only used single phase brushed motors with no feedback, so the 2 mcpwm controllers would support 6 of these independently, but a more complex 3 phase brushless motor with hall feedback would need an entire controller. As I understand it hall sensors are needed for brushless motors so the motors you referenced are brushless. Brushless motors need more complex electronics so the H bridge module I mentioned above would not work. I've not used brushless motors.

If you write the mcpwm code from scratch consider using the latest (for arduino) library mcpwm 5.1, as the library used by the app will eventually be deprecated. The new library is more complicated though and there are not many examples which is why I've not converted it yet.

@maximagination1
Copy link
Author

Thanks, but I am using brushed not brushless DC motors. They have hall effect sensors similar to brushless motors, but only 2 instead of 3. Would these types of brushed motors with feedback work to create the desired effect of pausing the motors at one exact position before resuming using MCPWM? I'm asking this to know if there's sufficient CAP1, CAP2, etc. Capture pins of each unit to do this and if those are capable.

Do the features of the MCPWM documentation you sent previously correspond to the latest Arduno MCPWM library?

So, do I need to create "void" functions in the peripheral tab code that use the latest MCPWM library with the calling functions as stated in Espressif's documentation?

Thanks!

Max

@s60sc
Copy link
Owner

s60sc commented Jul 26, 2024

I think hall affect on a brushed would be for rpm but its not my area of expertise. For exact position you would need stepper motors.
Arduino v2..x based on IDF4.4, Arduino v3.x based on IDF5.1, but latest IDF is 5.3

@maximagination1
Copy link
Author

maximagination1 commented Aug 3, 2024

Hi again,

I would appreciate it if you could help suggest how to implement my new robot's motor control code with your ESP32-CAM_MJPEG2SD code.

I've written a snippet of code to have 4 motors move 2 at a time, each with limit switches that stop a pair once they've made a full rotation and trigger the next 2 to turn, imitating the oscillating tripodal gait for the final robot I strive to create. This code on its own seems to work quite well for what I need. One motor from each pair will end up moving 2 belt-adjoined legs, while the other in each pair moves only 1, eliminating the need for 2 extra motors - leaving me with 4 in total.

With the way your current code handles the web UI for the thumb-slider input and PWM with the direction control of motors, would you please be able to guide me toward the way that this code can be implemented to replace the basic RC car output control?

By the way, I'm using the XIAO ESP32S3 sense, no mic, no SD needed for my application. I hope this code doesn't conflict with any of the defined pins.

`#include <Arduino.h>

// Define pin numbers with descriptive names using D notation
const int M1 = D2; // Motor 1 - Control pin 1
const int M2 = D3; // Motor 1 - Control pin 2
const int M3 = D8; // Motor 2 - Control pin 1
const int M4 = D9; // Motor 2 - Control pin 2
const int M5 = D0; // Motor 3 - Control pin 1
const int M6 = D1; // Motor 3 - Control pin 2
const int M7 = D10; // Motor 4 - Control pin 1
const int M8 = 41; // Motor 4 - Control pin 2

const int S1 = D4; // Switch 1
const int S2 = D5; // Switch 2
const int S3 = D7; // Switch 3
const int S4 = 42; // Switch 4

// Array of motor control pins
const int motorPins[4][2] = {
{M1, M2}, // Motor 1 (controls 2 legs on one side)
{M3, M4}, // Motor 2 (individual motor)
{M5, M6}, // Motor 3 (controls 2 legs on the other side)
{M7, M8} // Motor 4 (individual motor)
};

// Array of switch pins
const int switchPins[4] = {S1, S2, S3, S4};

// Variables to keep track of switch states and debounce timing
volatile bool switchPressed[4] = {false, false, false, false};
volatile unsigned long lastDebounceTime[4] = {0, 0, 0, 0};
const unsigned long debounceDelay = 50; // Debounce delay in milliseconds

// State management variables
bool set1Active = true; // Indicates which set of motors is currently active
bool motorsReady[4] = {false, false, false, false}; // Indicates if a motor has reached the bottom position

void setup() {
// Initialize serial communication for debugging
Serial.begin(115200);
Serial.println("Setup started");

// Initialize motor pins
for (int i = 0; i < 4; i++) {
pinMode(motorPins[i][0], OUTPUT);
pinMode(motorPins[i][1], OUTPUT);
digitalWrite(motorPins[i][0], LOW); // Ensure motors are off initially
digitalWrite(motorPins[i][1], LOW); // Ensure motors are off initially
}

// Initialize switch pins
for (int i = 0; i < 4; i++) {
pinMode(switchPins[i], INPUT);
attachInterrupt(digitalPinToInterrupt(switchPins[i]), switchISR, FALLING);
}

// Start the first set of motors
set1Active = true;
runMotor(0);
runMotor(1);
Serial.println("Motors 0 and 1 started.");
}

void loop() {
// Check if all motors in the active set are ready
if ((set1Active && motorsReady[0] && motorsReady[1]) ||
(!set1Active && motorsReady[2] && motorsReady[3])) {
// Stop all motors before switching sets
stopAllMotors();

// Switch to the other set
set1Active = !set1Active;
resetMotorsReady();
//delay(1000); // Optional delay for smoother transition

// Start the motors in the new active set
if (set1Active) {
  runMotor(0);
  runMotor(1);
  Serial.println("Switched to motors 0 and 1.");
} else {
  runMotor(2);
  runMotor(3);
  Serial.println("Switched to motors 2 and 3.");
}

}
}

// Function to run motor until it hits the switch
void runMotor(int motorIndex) {
Serial.print("Running motor ");
Serial.println(motorIndex);

if (!motorsReady[motorIndex]) {
digitalWrite(motorPins[motorIndex][0], HIGH);
digitalWrite(motorPins[motorIndex][1], LOW);
} else {
stopMotor(motorIndex);
}
}

// Function to stop motor
void stopMotor(int motorIndex) {
Serial.print("Stopping motor ");
Serial.println(motorIndex);

digitalWrite(motorPins[motorIndex][0], LOW);
digitalWrite(motorPins[motorIndex][1], LOW);
}

// Function to stop all motors
void stopAllMotors() {
for (int i = 0; i < 4; i++) {
stopMotor(i);
}
}

// Function to reset motor ready states
void resetMotorsReady() {
for (int i = 0; i < 4; i++) {
motorsReady[i] = false;
}
}

// Interrupt Service Routine for switches
void switchISR() {
unsigned long currentTime = millis();
for (int i = 0; i < 4; i++) {
if (digitalRead(switchPins[i]) == LOW) {
if (currentTime - lastDebounceTime[i] > debounceDelay) {
switchPressed[i] = true;
lastDebounceTime[i] = currentTime;

    // Update motorsReady based on which switch was pressed
    if (i == 0) {
      motorsReady[0] = true;
      stopMotor(0);
    } else if (i == 1) {
      motorsReady[1] = true;
      stopMotor(1);
    } else if (i == 2) {
      motorsReady[2] = true;
      stopMotor(2);
    } else if (i == 3) {
      motorsReady[3] = true;
      stopMotor(3);
    }
    
    Serial.print("Switch ");
    Serial.print(i);
    Serial.println(" pressed.");
  }
}

}
}`

Any tips on how this can be done would be appreciated!

Max

@s60sc
Copy link
Owner

s60sc commented Aug 3, 2024

put your code in a separate cpp file and change loop() to be a separate task.

in appSpecific.cpp - appSpecificWsHandler() change to:

    case 'M': 
      // motor speed
      if (tripodal) tripodalMove(controlVal, false);
      else motorSpeed(controlVal); 
    break;
    case 'D':
      // steering
      if (tripodal) tripodalMove(controlVal, true);
      else setSteering(controlVal);
    break;

where tripodal is a boolean config to switch between modes and tripodalMove() is defined in your cpp file:

static inline int clampValue(int value, int maxValue) {
  // clamp value to the allowable range
  return value > maxValue ? maxValue : (value < -maxValue ? -maxValue : value);
}

void  tripodalMove(int controlVal, bool steering) {
  // set left and right motor side speed values depending on requested speed and requested steering angle
  // steering = true ? controlVal = steer angle : controlVal = speed change
  static int driveSpeed = 0; // -ve for reverse
  static int steerAngle = 0; // -ve for left turn
  steering ? steerAngle = controlVal : driveSpeed = controlVal;
  // experiment with maxSteerAngle (default 45 deg) and maxTurnSpeed ( 0 .. 100)
  int turnSpeed = (clampValue(steerAngle, maxSteerAngle) * maxTurnSpeed / 2) / maxSteerAngle; 
   
  if (driveSpeed < 0) turnSpeed = 0 - turnSpeed;
  // can now derive relative speeds for left and right as PWM values
  // for a tight turn with forward motion one side may have a reverse speed
  int leftSpeed = clampValue(driveSpeed + turnSpeed, maxDutyCycle);
  int rightSpeed = clampValue(driveSpeed - turnSpeed, maxDutyCycle);
}

as you are not using PWM, use leftSpeed and rightSpeed to weight the number of oscillations per side and if your motors are
uni directional set negative values to 0

@maximagination1
Copy link
Author

maximagination1 commented Aug 3, 2024 via email

@s60sc
Copy link
Owner

s60sc commented Aug 3, 2024

Declare tripodal in your cpp and add as extern in appGlobals.h

@maximagination1
Copy link
Author

maximagination1 commented Aug 4, 2024 via email

@s60sc
Copy link
Owner

s60sc commented Aug 4, 2024

in appGlobals.h to switch to SPIFFS, uncomment:
//#define SIDE_ALARM

and set following to false:
#define INCLUDE_AUDIO true // audio.cpp (microphone)

what are the 12 pin numbers you are using?

@maximagination1
Copy link
Author

maximagination1 commented Aug 4, 2024 via email

@s60sc
Copy link
Owner

s60sc commented Aug 4, 2024

D2, D9 and D10 are pulled high by resistors unless you cut J3 or use them for active low pushbuttons

D11 and D12 are connected to mic unless you cut J1 and J2

@maximagination1
Copy link
Author

maximagination1 commented Aug 4, 2024 via email

@s60sc
Copy link
Owner

s60sc commented Aug 4, 2024

will call your code if tripodal = true. add some debug prints to see what is happening

@maximagination1
Copy link
Author

maximagination1 commented Aug 5, 2024 via email

@maximagination1
Copy link
Author

maximagination1 commented Aug 5, 2024 via email

@s60sc
Copy link
Owner

s60sc commented Aug 5, 2024

How are you supplying leftSpeed and rightSpeed from tripodalMove() to controlRobot() ?

Note in tripodalMove() change:
steering ? steerAngle = controlVal : driveSpeed = controlVal;
to:
steering ? steerAngle = controlVal - servoCenter : driveSpeed = controlVal;
to convert servo angle to direction angle

@maximagination1
Copy link
Author

maximagination1 commented Aug 5, 2024

This is how I am doing it:

`void tripodalMove(int controlVal, bool steering) {
controlRobot();
static int driveSpeed = 0; // -ve for reverse
static int steerAngle = 0; // -ve for left turn
extern int servoCenter;
steering ? steerAngle = controlVal - servoCenter : driveSpeed = controlVal;

int turnSpeed = (clampValue(steerAngle, maxSteerAngle) * maxTurnSpeed / 2) / maxSteerAngle;
if (driveSpeed < 0) turnSpeed = 0 - turnSpeed;

int leftSpeed = clampValue(driveSpeed + turnSpeed, maxDutyCycle);
int rightSpeed = clampValue(driveSpeed - turnSpeed, maxDutyCycle);

setMotorSpeed(0, leftSpeed);
setMotorSpeed(1, leftSpeed);
setMotorSpeed(2, rightSpeed);
setMotorSpeed(3, rightSpeed);
}`

Which is then handled by setMotorSpeed to control the speed via MCPWM:

'void setMotorSpeed(int motorIndex, int speed) {
float dutyCycle = abs(speed) / 100.0 * MAX_SPEED; // Normalize speed to duty cycle
mcpwm_operator_t op = motorIndex % 2 == 0 ? MCPWM_OPR_A : MCPWM_OPR_B;
if (speed >= 0) {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, op == MCPWM_OPR_A ? MCPWM_OPR_B : MCPWM_OPR_A);
mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, op, dutyCycle);
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_0, op, MCPWM_DUTY_MODE_0);
} else {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, op);
mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, op == MCPWM_OPR_A ? MCPWM_OPR_B : MCPWM_OPR_A, dutyCycle);
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_0, op == MCPWM_OPR_A ? MCPWM_OPR_B : MCPWM_OPR_A, MCPWM_DUTY_MODE_0);
Serial.print("Motor Speed Set:");
Serial.print(speed);
}
}'

Please let me know if you need to see the full code and if controlRobot() is being called correctly in the top piece of code.

@maximagination1
Copy link
Author

I am also getting errors in the serial monitor such as "Guru Meditation Error: Core 1 panic'ed (Unhandled debug exception)." and Guru Meditation Error: Core 1 panic'ed (Coprocessor exception). "

One is now appearing when I move the sliders and the other when I press the limit switches for my motors. If I remember correctly, the second one occurs somehow because of the interrupts (ISR). Any idea on how to stop this? Thanks

@s60sc
Copy link
Owner

s60sc commented Aug 6, 2024

use exception decoder to identify faulty code

@maximagination1
Copy link
Author

Thanks, that helped narrow down the cause of the issue.

I ran the tool and got a decoded message based on the error that seems to indicate a stack overflow.

The moment I move the sliders in the UI to drive the motors, an immediate Guru Meditation Error keeps happening no matter what I change in the code while keeping the motor control logic in its working order.

Could this be happening due to initializing the same MCPWM timers and other details in my robot control code as in the default code for controlling a car? I believe tripodal takes care of switching to my custom motor driving code, but it may not stop the MCPWM conflict?

Kindly let me know if this could be the problem.

@s60sc
Copy link
Owner

s60sc commented Aug 7, 2024

in prepPeripherals() comment out prepMotors() to be sure

@s60sc
Copy link
Owner

s60sc commented Aug 9, 2024

The app now incorporates the latest MCPWM library. This is more complex than the previous library but espressif supply an abstraction layer for brushed motors which is only available in the IDF but I've incorporated into the app (files esp_bdc_motor.h/c). This makes it easier to use than the old library which will eventually be dropped.

The app now has a track steer option which can be controlled by the RC web page to drive a left and right side motor.

The peripherals.cpp file contains the interface between the web page and the esp_bdc_motor code:

  • prepBDCmotor() enables each individual motor
  • motorDirection(uint32_t dutyTicks, int motorId, bool goFwd) in v3.x section drives each individual motor
  • motorSpeed() in v3.x section controls both motors
  • trackSteeering() is the equivalent of tripodalMove()

To control four motors in your code:

  • change index to 4 in static bdc_motor_handle_t motor[2] = {NULL, NULL};
  • one of the 4 calls to prepBDCmotor() will need groupId = 1 as each group only has 3 pairs of PWM outputs
  • implement your own version of motorSpeed() to control the 4 motors and the push buttons

@maximagination1
Copy link
Author

maximagination1 commented Aug 9, 2024 via email

@s60sc
Copy link
Owner

s60sc commented Aug 9, 2024

Enter pin numbers for following entries on RC config web page, save and reboot

Screenshot 2024-08-09 210246

@maximagination1
Copy link
Author

Cheers for that. I got your updated code working now on 2 of my motors. However, I still have not successfully implemented my piece of robot code with the update of 3.0 MCPWM.

So, I have a few questions with some doubts:

  1. I changed that index to 4 with "NULL, NULL, NULL, NULL" instead of only two nulls, does prepBDCmotor() function in peripherals.cpp need to have 2 new added pins definitions added in its function brackets? What would need to be added to my robot code having these changes?
  2. You mentioned that one of the 4 calls to prepBDCmotor() will need groupId = 1, where and how to add this extra groupId?
  3. Does the prepBDCmotor() function need to be called in my robot.cpp code with all the motor driving functions and how?

Much of this is still new to me, so I'd appreciate any more guidance you can give me.

Thanks.

@s60sc
Copy link
Owner

s60sc commented Aug 11, 2024

First of all check that all 4 motors can be driven with the current code, by amending peripherals.cpp with:

static bdc_motor_handle_t motor[4] = {NULL, NULL, NULL, NULL};

void prepMotors() {
#if !CONFIG_IDF_TARGET_ESP32C3
  if (RCactive) {
    if (motorFwdPin > 0) {
#if ESP_ARDUINO_VERSION >= ESP_ARDUINO_VERSION_VAL(3, 0, 0)
      // v3.x
      prepBDCmotor(0, 0, motorFwdPin, motorRevPin);
      if (trackSteer) prepBDCmotor(0, 1, motorFwdPinR, motorRevPinR);
      if (tripodal) {
        prepBDCmotor(0, 2, motor 3 pin A, motor 3 pin B); // left
        // need group id 1 for 4th motor
        prepBDCmotor(1, 0, motor 4 pin A, motor 4 pin B); // right
      } 
#else
      // v2.x
      prepMotor(MCPWM_UNIT_0, motorFwdPin, motorRevPin);
      if (trackSteer) prepMotor(MCPWM_UNIT_1, motorFwdPinR, motorRevPinR);
#endif
    } else LOG_WRN("RC motor pins not defined");
  }
#else
  RCactive = false;
  LOG_WRN("This function not compatible with ESP32-C3");
#endif
}

void motorSpeed(int speedVal, bool leftMotor) {
  // speedVal is signed duty cycle, convert to unsigned uint32_t duty ticks
  if (abs(speedVal) < minDutyCycle) speedVal = 0;
  uint32_t dutyTicks = abs(speedVal) * MCPWM_TIMER_HZ / pwmFreq / 100;
  if (leftMotor) {
    // left motor steering or all motor direction
    if (motorRevPin && speedVal < 0) {
	motorDirection(dutyTicks, 0, false); 
	motorDirection(dutyTicks, 2, false); 
    } else if (motorFwdPin) {
	motorDirection(dutyTicks, 0, true);
	motorDirection(dutyTicks, 2, true);
  } else {
    // right motor steering
    if (motorRevPinR && speedVal < 0) {
      motorDirection(dutyTicks, 1, false); 
      motorDirection(dutyTicks, 3, false); 
    } else if (motorFwdPinR) {
      motorDirection(dutyTicks, 1, true);
      motorDirection(dutyTicks, 3, true);
    }
  }
}

If this works then you can write your own version of motorSpeed()
trackSteeering() is providing the speed and left / right differential to motorSpeed()
In motorSpeed() you need to determine how you use this info to derive the values to call motorDirection() for each motor
and how you manage the pushbuttons

In prepMotors() add a call to init your pushbutton pins etc

@maximagination1
Copy link
Author

Hi,

Thanks for your guidance. However, I have decided to stick with using your older code with the 2.0 version of MCPWM as it's simpler to understand.

My overall motor switching control code works with your code to switch the motors from one pair to another except for one little hiccup. The motors only switch over to another pair in the middle of altering the controlVal with the sliders.

You may read my full thread on the issue here: https://forum.arduino.cc/t/esp32-robot-motor-switching-interruption-issue/1291803

I'd appreciate it if you could present a solution to this issue so I have seamless operation of my motors switching from one pair to another even while sending only one speed value.

Thanks.

@s60sc
Copy link
Owner

s60sc commented Aug 14, 2024

You have checkSwitches() called by a timer, and controlRobot() called from the web page which is only when a change occurs. Call controlRobot() from checkSwitches() instead. Use tripodalMove() just to set the left and right speed values, with checkSwitches() doing all the motor control.

@maximagination1
Copy link
Author

maximagination1 commented Aug 15, 2024

I got the code working with your help! Thank you very much.

Motors oscillate between each pair at any given speed value according to how the robot needs to move.

@s60sc
Copy link
Owner

s60sc commented Aug 15, 2024

Good to hear. When youve finished the code put it in a pull request and i'll incorporate it

@maximagination1
Copy link
Author

maximagination1 commented Aug 15, 2024 via email

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants