Search notes

Search titles and full post content.

    ← All notes

    Motor/Encoder Control Code

    Arduino Code, MatLab 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:

    Pic 1
    Pic 2
    Pic 3

    A quick start for the stepper motor controller.

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    69
    70
    71
    72
    73
    74
    75
    76
    77
    78
    79
    80
    81
    82
    83
    84
    85
    86
    87
    88
    89
    90
    91
    92
    93
    94
    95
    96
    97
    98
    99
    100
    101
    102
    103
    104
    105
    106
    107
    108
    109
    110
    111
    112
    113
    114
    115
    116
    117
    118
    119
    120
    121
    122
    123
    124
    125
    126
    127
    128
    129
    130
    131
    132
    133
    134
    135
    136
    137
    138
    139
    140
    141
    142
    143
    144
    145
    146
    147
    148
    149
    150
    151
    152
    153
    154
    155
    156
    157
    158
    159
    160
    161
    162
    163
    164
    165
    166
    167
    168
    169
    170
    171
    172
    173
    174
    175
    176
    177
    178
    179
    180
    181
    182
    183
    184
    185
    186
    187
    188
    189
    190
    191
    192
    193
    194
    195
    196
    197
    198
    199
    200
    201
    202
    203
    204
    205
    206
    207
    208
    209
    210
    211
    212
    213
    214
    215
    216
    217
    218
    219
    220
    221
    222
    223
    224
    225
    226
    227
    228
    229
    230
    231
    232
    233
    234
    235
    236
    237
    238
    239
    240
    241
    242
    243
    244
    245
    246
    247
    248
    249
    250
    251
    252
    253
    254
    255
    256
    257
    258
    259
    260
    261
    262
    263
    264
    265
    266
    267
    268
    269
    270
    271
    272
    273
    274
    275
    276
    277
    278
    279
    280
    281
    282
    283
    284
    285
    286
    /**
    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

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    69
    70
    71
    72
    73
    74
    75
    76
    77
    78
    79
    80
    81
    82
    83
    84
    85
    86
    87
    88
    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