Help me with my project, please

[rbeetle004]

Not sure if I got the right Category, I'm building Tanks and learning along the way, this project is nearly done. I am attempting to control speed of a stepper motor AccelStepper.h library and Sbus. I have gotten the stepper moving with variable speed in accordance to stick position, BUT it is a few steps at a time. Very slow. I am not sure what is causing it. The whole system works, if I use my original code, everything works and the steppers moved at an single speed. I am just trying to include speed control.

I know this is alot to read, and i'm sorry.. my previous code I code'd the stepper pins in directly. I am using Nema 17's and TB6600 drivers, thanks!

#include <Servo.h>
#include "sbus.h"
#include <AccelStepper.h>



/* SBUS object, reading SBUS */
bfs::SbusRx sbus_rx(&Serial2);
/* SBUS object, writing SBUS */
bfs::SbusTx sbus_tx(&Serial2);
/* SBUS data */
bfs::SbusData data;

Servo forearm;
Servo jaw;
Servo jawclamp;

AccelStepper stepper_3(1, 11, 29);
#define EN_1 28

#define step_2 8
#define direction_2 23
#define EN_2 22

#define motorLeft_EN 26 // Enable pin for left motor
#define motorLeft_IN1 10
#define motorLeft_IN2 9
#define motorRight_EN 25 // Enable pin for right motor
#define motorRight_IN1 12
#define motorRight_IN2 13

#define laser 5 

int ch0, ch1, ch2, ch3, ch4, ch5, ch6, ch7 = 0;
int motorSpeed, steeringValue, leftMotorSpeed, rightMotorSpeed, stepperSpeed2 = 0;
int forearmVAL;
int jawVAL;
int jawclampVAL;


void setup() {
  /* Serial to display data */
  Serial.begin(115200);
  while (!Serial) {}
  /* Begin the SBUS communication */
  sbus_rx.Begin();
  sbus_tx.Begin();



  //stepper motors control - set stationary
  //pinMode(step_1, OUTPUT);
  //pinMode(direction_1, OUTPUT);
  //pinMode(EN_1, OUTPUT);
  //digitalWrite(step_1, LOW);
  //digitalWrite(direction_1, LOW);
  //digitalWrite(EN_1, LOW);

  //stepper motor 2 control - set stationary 
  pinMode(step_2, OUTPUT);
  pinMode(direction_2, OUTPUT);
  pinMode(EN_2, OUTPUT);
  digitalWrite(step_2, LOW);
  digitalWrite(direction_2, LOW);
  digitalWrite(EN_2, LOW);

  forearm.attach(4);
  jaw.attach(2);
  jawclamp.attach(3);
  
  pinMode(laser, OUTPUT);
   
  // DC motors control - set stationary
  pinMode(motorLeft_EN, OUTPUT);
  pinMode(motorLeft_IN1, OUTPUT);
  pinMode(motorLeft_IN2, OUTPUT);
  digitalWrite(motorLeft_EN, LOW); // Set enable pin low initially
  digitalWrite(motorLeft_IN1, LOW);   // PWM value
  digitalWrite(motorLeft_IN2, LOW); // Forward

  pinMode(motorRight_EN, OUTPUT);
  pinMode(motorRight_IN1, OUTPUT);
  pinMode(motorRight_IN2, OUTPUT);
  digitalWrite(motorRight_EN, LOW); // Set enable pin low initially
  digitalWrite(motorRight_IN1, LOW);   // PWM value
  digitalWrite(motorRight_IN2, LOW); // Forward

    stepper_3.setMaxSpeed(1000);
    stepper_3.setAcceleration(500);
    pinMode(EN_1, OUTPUT);
    digitalWrite(EN_1, LOW);
  }


void loop () {
   if (sbus_rx.Read()) {
    /* Grab the received data */
    data = sbus_rx.data();

    ch0 = data.ch[0]; //channel 0 controls shoulder verticle
    ch1 = data.ch[1]; //channel 1 controls steering
    ch2 = data.ch[2]; //channel 2 controls motor speed
    ch3 = data.ch[3]; //channel 3 controls shoulder rotation
    ch4 = data.ch[4]; //channel 4 controls forarm verticle
    ch5 = data.ch[5]; //channel 5 control jaw verticle
    ch6 = data.ch[6]; //channel 6 control jaw vice
    ch7 = data.ch[7]; //channel 7 control laser
    
    steeringValue = map(ch1, 172, 1811, -100, 100); // 0 to 185 range because then I add +70 in order to avoid low PWM values as to motors won't start if so
    motorSpeed = map(ch2, 172, 1711, -155, 155); // Map motor speed to 0-255 range
     // arm rotation speed
     //arm verticle

    motorSpeed = abs(motorSpeed); // Constrain motor speed to ensure it's within valid PWM range
    //stepperSpeed = abs(stepperSpeed); //constrain stepper motor (1)
    stepperSpeed2 = abs(stepperSpeed2);//constrain stepper motor(2)
    
    leftMotorSpeed = 0.0 + motorSpeed + steeringValue; // Adjust left motor speed
    rightMotorSpeed = 0.0 + motorSpeed - steeringValue; // Adjust right motor speed

    // Ensure motor speeds are within valid PWM range
    leftMotorSpeed = constrain(leftMotorSpeed, 0, 255);
    rightMotorSpeed = constrain(rightMotorSpeed, 0, 255);
    //stepperSpeed = constrain(stepperSpeed, 0, 200);
    stepperSpeed2 = constrain(stepperSpeed2, 0, 200);
    
   

     // if PWM is lower than 72, set PWM value to 0

    if (leftMotorSpeed < 40) {
    leftMotorSpeed = 0;
    }
    if (rightMotorSpeed < 40) {
    rightMotorSpeed = 0;
    }
    if (steeringValue < 25) {
      steeringValue = 0;
    }
    //if (-2 < stepperSpeed > 2) {
      //stepperSpeed = 0;
    //}
    if (-2 < stepperSpeed2 > 2) {
      stepperSpeed2 = 0;
    }
      
    if (data.ch[1] > 1100 && data.ch[1] <1815) { // channel 1 controls right motion 
      digitalWrite(motorLeft_EN, HIGH); // Enable left motor
      analogWrite(motorLeft_IN1, LOW);  // PWM input
      digitalWrite(motorLeft_IN2, leftMotorSpeed); // Direction - Forward
      digitalWrite(motorRight_EN, HIGH); // Enable right motor
      digitalWrite(motorRight_IN1, rightMotorSpeed);  // Direction - Backward
      analogWrite(motorRight_IN2, LOW); // PWM input
    }
    if (data.ch[1] > 170 && data.ch[1] < 800) { //channel 2 controls left motion
      digitalWrite(motorLeft_EN, HIGH); // Enable left motor
      digitalWrite(motorLeft_IN1, leftMotorSpeed);  // Direction - Backward
      analogWrite(motorLeft_IN2, LOW); // PWM input
      digitalWrite(motorRight_EN, HIGH); // Enable right motor
      analogWrite(motorRight_IN1, LOW);  // PWM input
      digitalWrite(motorRight_IN2, rightMotorSpeed); // Direction - Forward
    }
    
    if (data.ch[2] > 1100 && data.ch[2] < 1820) { //channel 3 controls forward motion
      digitalWrite(motorLeft_EN, HIGH); // Enable left motor
      analogWrite(motorLeft_IN1, leftMotorSpeed);  // PWM input
      digitalWrite(motorLeft_IN2, LOW); // Direction - Forward
      digitalWrite(motorRight_EN, HIGH); // Enable right motor
      analogWrite(motorRight_IN1, rightMotorSpeed);  // PWM input
      digitalWrite(motorRight_IN2, LOW); // Direction - Forward
    } 
    else if (data.ch[2] > 170 && data.ch[2] < 800) { //channel 3 controls backward motion
      digitalWrite(motorLeft_EN, HIGH); // Enable left motor
      digitalWrite(motorLeft_IN1, LOW);  // Direction - Backward
      analogWrite(motorLeft_IN2, leftMotorSpeed); // PWM input
      digitalWrite(motorRight_EN, HIGH); // Enable right motor
      digitalWrite(motorRight_IN1, LOW);  // Direction - Backward
      analogWrite(motorRight_IN2, rightMotorSpeed); // PWM input
    } 
    else { // Stop motors if channel 2 is at midpoint
      digitalWrite(motorLeft_EN, LOW); // Disable left motor
      digitalWrite(motorRight_EN, LOW); // Disable right motor
    } 


    
     
    if (data.ch[3] > 1000 && data.ch[3] <1815) { // channel 4 controls rotation
     int stepperSpeed = map(data.ch[3], 1300, 1815, 200, 500); 
     stepper_3.setSpeed(-stepperSpeed);
     stepper_3.runSpeed();
     
     //digitalWrite(direction_1, LOW);
     digitalWrite(EN_1, LOW);
    }  
    else if (data.ch[3] > 170 && data.ch[3] < 800) {
     int stepperSpeed = map(data.ch[3], 172, 600, 500, 200); 
     stepper_3.setSpeed(stepperSpeed);
     stepper_3.runSpeed();
     //digitalWrite(step_1, stepperSpeed);
     //digitalWrite(direction_1, HIGH);
     digitalWrite(EN_1, LOW);
    }

    else //(data.ch[3] > 800 && data.ch[3] <1000) 
    {
      stepper_3.setSpeed(0);
    }
         stepper_3.runSpeed();
      
     
     
     //int stepperSpeed = map(data.ch[3], 800, 1000, 0, 0);
     //stepper_3.setSpeed(stepperSpeed);
     //stepper_3.runSpeed();
     //digitalWrite(step_1, stepperSpeed);
     //digitalWrite(direction_1, HIGH);
     digitalWrite(EN_1, LOW);

        
    if (data.ch[0] > 1200 && data.ch[0] <1815) { // channel 0 controls verticle arm
     stepperSpeed2 = map(ch0, 1300, 1815, 0, 25);
     analogWrite(step_2, stepperSpeed2);
     digitalWrite(direction_2, LOW);
     digitalWrite(EN_2, LOW);
    }  
    else if (data.ch[0] > 170 && data.ch[0] < 700) {
     stepperSpeed2 = map(ch0, 172, 600, 25, 0);
     analogWrite(step_2, stepperSpeed2);
     digitalWrite(direction_2, HIGH);
     digitalWrite(EN_2, LOW);
    }

    if (data.ch[0] > 800 && data.ch[0] <1200) {
     stepperSpeed2 = map(ch0, 800, 1200, 0, 0);
     analogWrite(step_2, stepperSpeed2);
     digitalWrite(direction_2, HIGH);
     digitalWrite(EN_2, LOW);
    }
    
    {forearmVAL = data.ch[6];
    forearmVAL = map(forearmVAL, 170, 1820, 0, 180);
    forearm.write(forearmVAL);
    delay(15);
    }
  
    {jawVAL = data.ch[4];
    jawVAL = map(jawVAL, 170, 1820, 0, 180);
    jaw.write(jawVAL);
    delay(15);
    }

    {jawclampVAL = data.ch[5];
    jawclampVAL = map(jawclampVAL, 170, 1820, 0, 34);
    jawclamp.write(jawclampVAL);
    delay(15);
    }

    if (data.ch[7] > 1200) {
      digitalWrite(laser, HIGH);
    }
      else if (data.ch[7] <750) {
        digitalWrite(laser, LOW);
    }
      
    // Debugging - print received SBUS data
    for (int8_t i = 0; i < data.NUM_CH; i++) {
      Serial.print(data.ch[i]);
      Serial.print("\t");
    }
    // Debugging - print lost frames and failsafe data
    Serial.print(data.lost_frame);
    Serial.print("\t");
    Serial.println(data.failsafe);

    // Set the SBUS TX data to the received data and write to servos
    sbus_tx.data(data);
    sbus_tx.Write();
    }
}

[Paul_KD7HB]

Begin to debug your code by using serial.Print() to show you the variable values that are being used and to show you the point in your code that is being executed.

[DaveX]

From a quick look, I think a big drag is that stepper_3.runSpeed() is only called inside the if (sbus_rx.Read()) {...} so you only get one step per sbus message. Move the runSpeed() outside the conditional so it gets a chance to step the motor more frequently.

Afterwards, work on speeding up the processing of the sbus message.

[rbeetle004]

 unsigned long startTime = millis();
  while (millis() - startTime < 3000) {  // Run for 3 seconds
    stepper_3.runSpeed();  // Keep running the motor at the set speed
  }

Thanks guys, I have checked my serial monitor and everything is as it should be.. just can not get the stepper motor to perform like I want.. it does go tick tick tick very slowly but it is not smooth.. I am playing with incorporating some kind of time function... although I can't figure how I would set it to count and run the setSpeed until it stops receiving values on that channel [usable values between <1000<

[DaveX]

@rbeetle004

 unsigned long startTime = millis();
  while (millis() - startTime < 3000) {  // Run for 3 seconds
    stepper_3.runSpeed();  // Keep running the motor at the set speed
  }

That looks like a bad idea. It would block the rest of the code from reading any sbus data for 3 seconds.

Can you tell from your serial monitor how often do you receive and process sbus messages?

I'd add a Serial.print("S") in the sbus conditional, and a Serial.print("M") next to the runSpeed().

I'd suspect that they tick tick tick along with each other at a 1:1 ratio, and ideally, you'd like the stepper to run as fast as it needs to, at the same time that you read the sbus as fast as packets come in, with neither waiting for the other. The several delay(15) lines inside the if (sbus_rx.Read()) {...} is going to make you skip sbus packets and slow the stepping to less than 1/(0.015s*3)=22Hz. A 200step/rev motor in a program with that won't run faster than 200/22*60=6.6RPM

The code looks like it needs to consider the tricks in :

@DaveX

• Planning and Implementing an Arduino Program by @Robin2-- A great way to plan your program to be flexible, easy to read, understand, maintain, and extend.
• Demonstration code for several things at the same time by @Robin2-- An example running a couple blinking LEDS, a button and two servos, all independently. (Live Simulation)