Motor/Encoder Control Code

I developed a stepper motor controller using a Pololu board and Arduino, designed to manage a stepper motor up to 4 A and read a single-end encoder. The system uses serial communction for interaction between the motor, encoder, and PC. That means you can even use MATLAB (with the example code shown below) to operate the motor and capture encoder counts.

Take a closer look:

A quick start for the stepper motor controller.

/**
Title: Controller Box Code
Description: This code is used for a controller box with a single-end encoder and a four-wire step motor (with the current up to 4 A).
Created by: YX at Melboune, 202404 
*/

/*
 * Arduino Code for Controlling Motor and Reading Encoder
 *
 * Instructions:
 * 1. Connect your Arduino board to a serial terminal program (e.g., Arduino IDE's Serial Monitor or a dedicated serial terminal software).
 * 2. Set the baud rate of the serial terminal program to match the baud rate set in this code (115200).
 * 3. Send command strings to the board via the serial terminal program for various operations.
 *    - To get the encoder value, send the command string "E".
 *    - To drive the motor forward by a certain number of steps, send the command string "+N", where N is the desired number of steps.
 *    - To drive the motor backward by a certain number of steps, send the command string "-N", where N is the desired number of steps.
 *    - To set the motor current, send the command string "A<current>", where <current> is the desired current value in milliamperes.
 * 4. The board will process the commands and display the results or feedback in the serial terminal program.
 */

#include <SPI.h>
#include <HighPowerStepperDriver.h>
#include "Arduino.h"
#include <Servo.h>
#include <digitalWriteFast.h>  // library for high performance reads and writes by jrraines
                               // see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811/0
                               // and http://code.google.com/p/digitalwritefast/

const uint8_t DirPin = 8;
const uint8_t StepPin = 9;
const uint8_t CSPin = 10;

const uint8_t SleepPin = 7;
const uint8_t RstPin   = 6;
const uint8_t FaultPin   = 5;

// This period is the length of the delay between steps, which controls the
// stepper motor's speed.  You can increase the delay to make the stepper motor
// go slower.  If you decrease the delay, the stepper motor will go faster, but
// there is a limit to how fast it can go before it starts missing steps.
const uint16_t StepPeriodUs = 1000;

char ch;
float DutyCircle;
const byte numChars = 32;
char comdata[numChars];
int motorState;

boolean newData = false;

HighPowerStepperDriver sd;

// Quadrature encoders
#define zChannel 4
#define EncoderPinA 2
#define EncoderPinB 3
#define EncoderIsReversed
volatile bool EncoderBSet;
volatile long EncoderTicks = 0;
unsigned int flagA = 0;
unsigned int flagB = 0;


void setup()
{
   Serial.begin(115200);
  // Serial.println("Waiting for sending a signal...\n");
  SPI.begin();
  sd.setChipSelectPin(CSPin);

  // Drive the STEP and DIR pins low initially.
  pinMode(StepPin, OUTPUT);
  digitalWrite(StepPin, LOW);
  pinMode(DirPin, OUTPUT);
  digitalWrite(DirPin, LOW);

  pinMode(SleepPin, OUTPUT);
  digitalWrite(SleepPin, HIGH);
  pinMode(RstPin, OUTPUT);
  digitalWrite(RstPin, LOW);

  pinMode(FaultPin, INPUT);

  // Give the driver some time to power up.
  delay(10);

  // Reset the driver to its default settings and clear latched status
  // conditions.
  sd.resetSettings();
  sd.clearStatus();
  // Select auto mixed decay.  TI's DRV8711 documentation recommends this mode
  // for most applications, and we find that it usually works well.
  sd.setDecayMode(HPSDDecayMode::AutoMixed);

  // Set the current limit. You should change the number here to an appropriate
  // value for your particular system.
  sd.setCurrentMilliamps36v4(600);

  // Set the number of microsteps that correspond to one full step.
  sd.setStepMode(HPSDStepMode::MicroStep16);

  // Enable the motor outputs.
  sd.enableDriver();

  // Quardrature
   pinMode(EncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(EncoderPinA, LOW);  // 
  pinMode(EncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(EncoderPinB, LOW);  //  
  pinMode(zChannel,INPUT);
  attachInterrupt(digitalPinToInterrupt(EncoderPinA), HandleInterruptA, RISING);
}

void loop()
{
  checkStatus();
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB
  }

  if (Serial.available() > 0) {
        newData = false;
    recvWithStartEndMarkers();
   if (newData == true){

   if (comdata[0] == '+' || comdata[0] == '-' || comdata[0] == 'A' || comdata[0] == 'E'){
    ch = comdata[0];
    char* numStart = &comdata[1];     // Pointer to the start of the numerical part of the string.
    char* endptr;
    long stepp = strtol(numStart, &endptr, 10);
    if (numStart == endptr) {
      Serial.println("Error: No digits were found after the sign. Please re-enter the command.");
        return;  // Skip the rest of the loop and wait for new input.
     } else if (*endptr != '\0') {
      Serial.println("Error: Extra characters after number. Please re-enter the command.");
        return;  // Skip the rest of the loop and wait for new input.
    } else if (comdata[0] == '+' || comdata[0] == '-') {
          DoSerial(ch, stepp);
            }
       else if (comdata[0] == 'A') {
                if (stepp > 1000 || stepp < 200) {
                  Serial.println("Over/Below Current");
                  return;
                }
               sd.setCurrentMilliamps36v4(stepp);
               Serial.print("The motor current is set to be ");
               Serial.print(stepp);
               Serial.println(" mA");
            }    
       else if (comdata[0] == 'E') {
               if (digitalReadFast(zChannel)){
               Serial.println("High");
               delay(100);
               }
               Serial.print("number of pulses: ");
               Serial.println(EncoderTicks);
            }          
    } else {
    Serial.println("Error: Command must start with '+', '-', 'A', or 'E'. Please re-enter the command.");
          return;  // Skip the rest of the loop and wait for new input.
           }
    newData = false;
   }

  }
}

// Sends a pulse on the STEP pin to tell the driver to take one step, and also
//delays to control the speed of the motor.
void step()
{
  // The STEP minimum high pulse width is 1.9 microseconds.
  digitalWrite(StepPin, HIGH);
  delayMicroseconds(3);
  digitalWrite(StepPin, LOW);
  delayMicroseconds(3);
}

// Writes a high or low value to the direction pin to specify what direction to
// turn the motor.
void setDirection(bool dir)
{
  // The STEP pin must not change for at least 200 nanoseconds before and after
  // changing the DIR pin.
  delayMicroseconds(1);
  digitalWrite(DirPin, dir);
  delayMicroseconds(1);
}




//===============================================================================
void DoSerial(char ch, long stepp)
{
//  char ch = toupper(Serial.read());   // Read the character we received
//  // and convert to upper case
  switch (ch) {
    case '+':                        
      setDirection(0);
 for(unsigned int x = 0; x < stepp; x++)
  {
    step();
    delayMicroseconds(StepPeriodUs);
    checkStatus();
  }
      break;
      case '-':
      setDirection(1);
 for(unsigned int x = 0; x < stepp; x++)
  {
    step();
    delayMicroseconds(StepPeriodUs);
    checkStatus();
  }
      break;
    default:
      break;
  }
}


// ============================================
void recvWithStartEndMarkers() {
    static boolean recvInProgress = false;
    static byte ndx = 0;
    char startMarker = '<';
    char endMarker = '>';
    char rc;
 // if (Serial.available() > 0) {
    while (Serial.available() > 0 && newData == false) {
        rc = Serial.read();
        delay(2);
        if (recvInProgress == true) {
            if (rc != endMarker) {
                comdata[ndx] = rc;
                ndx++;
                if (ndx >= numChars) {
                    ndx = numChars - 1;
                }
            }
            else {
                comdata[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                ndx = 0;
                newData = true;
            }
        }

        else if (rc == startMarker) {
            recvInProgress = true;
        }
    }
}

// Interrupt service routines for the quadrature encoder
void HandleInterruptA()//
{
  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
  EncoderBSet = digitalReadFast(EncoderPinB);   // read the input pin
  // // and adjust counter + if A leads B
  // #ifdef LeftEncoderIsReversed
    EncoderTicks -= EncoderBSet ? -1 : +1;
  // #else
    // _LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
  // #endif
   char i;
   i = digitalRead(EncoderPinB);
   if (i == 1)
   flagA += 1;
   else
   flagB += 1;
}
 
void checkStatus()
{
  motorState = digitalRead(FaultPin);
  delay(2);
  if (motorState == 0){
   Serial.println("Faulty");
   delay(2);
   digitalWrite(SleepPin, LOW);
  }
}
//============================================

MATLAB Code

function manageMotor(action, motorPort, varargin)
    % manageMotor - Control a stepper motor via serial connection.
    %
    % Syntax: manageMotor(action, motorPort, varargin)
    %
    % Parameters:
    %   action (string): The action to perform. Options are 'initialize',
    %                   'rotateMotor', and 'cleanup'.
    %   motorPort (string): The COM port (e.g., 'COM3') where the motor is connected.
    %   varargin: Additional parameters for 'rotateMotor':
    %     - angle (double, required for 'rotateMotor'): The rotation angle in degrees.
    %     - stepsPerRev (double, optional): Steps per revolution (default is 200).
    %     - microSteps (double, optional): Microsteps per step (default is 16).
    %
    % Usage:
    %   - Initialize the motor:
    %       manageMotor('initialize', 'COM3')
    %   - Rotate the motor (default parameters):
    %       manageMotor('rotateMotor', 'COM3', 90)
    %   - Rotate the motor (custom parameters):
    %       manageMotor('rotateMotor', 'COM3', 90, 400, 32)
    %   - Cleanup:
    %       manageMotor('cleanup', 'COM3')
    %
    % Created by YX on 2024.10.08

    persistent sMotor
    motorBaudRate = 115200;

    switch action
        case 'initialize'
            % Initialize the motor connection if not already done
            if isempty(sMotor)
                sMotor = serialport(motorPort, motorBaudRate);
                sMotor.Timeout = 3;
            end           

        case 'rotateMotor'
            % Ensure motor is initialized
            if isempty(sMotor)
                error('Motor not initialized');
            end
            % Check if the angle is provided
            if nargin >= 3
                angle = varargin{1};

                % Default values for steps and microsteps
                stepsPerRev = 200; 
                microSteps = 16;
                
                % Override defaults if specified
                if nargin >= 4
                    stepsPerRev = varargin{2};
                end
                if nargin >= 5
                    microSteps = varargin{3};
                end
                
                % Rotate the motor
                motorRoting(sMotor, angle, stepsPerRev, microSteps);
            else
                error('Angle not specified');
            end
            
        case 'cleanup'
            % Clear the motor connection
            if ~isempty(sMotor)
                clear sMotor;
            end
    end
end

function motorRoting(serialObj, angle, stepsPerRev, microSteps)
    % Determine rotation direction
    directionSign = '+';
    if angle < 0
        directionSign = '-';
    end

    % Calculate total steps based on angle
    totalSteps = round(abs(angle / 360 * stepsPerRev * microSteps));
    
    % Construct and send the command string
    commandString = ['<' directionSign num2str(totalSteps) '>'];
    writeline(serialObj, commandString);
    pause(0.2);
end

打赏