➡️Week 13: Output Devices

Project Hero

A DC motor being controlled with a Pico on a milled breakout board, it uses the encoder readings from inputs week to measure RPM and has a PID controller to regulate shaft speed to a desired RPM:

Group Assignment

In this weeks group assignment we measured the power consumption of an OLED screen with the Otii Arc. First of all, that Otii Arc is an incredibly tool and I would have one if it weren't for its steep price tag. But it was really nice to finally get a definitive answer on the power consumption of that OLED as I use it in nearly every project. I have always heard a ballpark quota of 30 mA of current for them at there max and the 26 mA was reassuring we were doing it correctly. I was also suprised at how little overhead there would be power consumption wise. It only drew 1.1 mA on a black screen and I thought there would be a running cost of about 5-10 mA at a minimum so it was nice to see how power efficient it could be. It was also nice to get a 0.1 mA reading of the device being un-initialised as in previous low-power projects I have disconected it with a transistor to save power, but its 0.1 mA consumption is negligible.

Getting Motors Going

This week we will be the Polulu motors on my robot and we will be driving them with these nice and cheap motor drivers. The break out board I made in week 8 makes the wiring for this extremely easy as I only need to connect 4x 4-pin headers to the 2 motor driver boards (without any wire cross overs). And I also need to plug in the motors. And the batteries which I need 2 voltages for as I am using the 24 volt and the 12 volt variants of these motors, but they should behave fine as we will be implementing some PID control. Here is a terribly drawn diagram of that:

And here is a photo of that, its hard to see whats going on so I made the diagram above.

Now that we have it wired up, lets work incrementally and start by just getting the motor going. Here is some started code I wrote, we import the required libraries and set up the pins to control one of the motors, one for PWM to regulate speed and another 2 to control the direction. Then I define a function which takes in a desired percentage of speed (which can be negative so -100% to 100%), and sets the directional pins and PWM pins accordingly. Then we just call in a half speed test. Here is the code for that:

from machine import Pin, PWM
import time

# Rear Right Pins
RRENB = Pin(10, Pin.OUT)
RRIN3 = Pin(11, Pin.OUT)
RRIN4 = Pin(12, Pin.OUT)

# PWM setup
RRpwm = PWM(RRENB)
RRpwm.freq(1000)

def set_motor_speed(RRspeed):
    # Rear Right Motor
    if RRspeed > 0:
        RRIN3.value(1)
        RRIN4.value(0)
    elif RRspeed < 0:
        RRIN3.value(0)
        RRIN4.value(1)
        RRspeed = -RRspeed
    else:
        RRIN3.value(0)
        RRIN4.value(0)

    # Convert speed percentage to duty cycle (0-65535)
    duty_cycle = int((RRspeed / 100) * 65535)
    RRpwm.duty_u16(duty_cycle)


set_motor_speed(50)

And that worked fine! (this picture is from when I originally got the motor working in isolation)

With 1 motor working, it was then time to add PID control to it. If I could summarise an entire mechatronic engineering degree it would be: "control it with PID and if thats not enough, MPC", and PID control is going to be more than enough for this. Here we apply the code we wrote in inputs week to this motor control code to regulate the speed of the motor to a target set point, here is what that looks like

from machine import Pin, PWM, Timer, UART
import time

# Rear Right Pins
RRENB = Pin(10, Pin.OUT)
RRIN3 = Pin(11, Pin.OUT)
RRIN4 = Pin(12, Pin.OUT)
RRencoderPinA = Pin(0, Pin.IN, Pin.PULL_UP)
RRencoderPinB = Pin(1, Pin.IN, Pin.PULL_UP)

# Encoder variables
RRencoder_position = 0

# PID control variables
Kp = 0.3  # Reduced Kp for less aggressive control
Ki = 0.2  # Reduced Ki
Kd = 0.3  # Reduced Kd
rpm_calculation_period = 0.04

# rpm stuff
RRsetpointRPM = 60
RRinputRPM = 0.0
RRoutputPWM = 0.0
RRprevious_error = 0.0
RRintegral = 0.0

# Constants
counts_per_revolution = 1920.0

# PWM setup
RRpwm = PWM(RRENB)
RRpwm.freq(1000)


# Interrupt handler for encoder
def RRencoder(pin):
    global RRencoder_position

    if RRencoderPinA.value() == RRencoderPinB.value():
        RRencoder_position += 1
    else:
        RRencoder_position -= 1


# Attach interrupt to encoder pins
RRencoderPinA.irq(trigger=Pin.IRQ_RISING, handler=RRencoder)

def pid_control():
    global RRprevious_error, RRintegral, RRoutputPWM

    # Rear Right Motor PID
    error = RRsetpointRPM - RRinputRPM
    RRintegral += error
    derivative = error - RRprevious_error
    RRoutputPWM = Kp * error + Ki * RRintegral + Kd * derivative
    RRprevious_error = error

def set_motor_speed(RRspeed):
    # Rear Right Motor
    if RRspeed > 0:
        RRIN3.value(1)
        RRIN4.value(0)
    elif RRspeed < 0:
        RRIN3.value(0)
        RRIN4.value(1)
        RRspeed = -RRspeed
    else:
        RRIN3.value(0)
        RRIN4.value(0)

    # Convert speed percentage to duty cycle (0-65535)
    duty_cycle = int((RRspeed / 100) * 65535)
    RRpwm.duty_u16(duty_cycle)

while True:
    #calculate current rpm from encoder counts over last sampling period
    RRinputRPM = (166.666 * RRencoder_position / counts_per_revolution) / rpm_calculation_period

    print("RR", RRinputRPM)

    # Reset encoder positions for the next calculation period
    RRencoder_position = 0


    pid_control()
    set_motor_speed(RRoutputPWM)

    # Sleep for calculation period
    time.sleep(rpm_calculation_period)

And here is a video of that in action. On the graph the blue line is the target RPM, the green is the PWM duty cycle being applied and the orange is the current RPM. As I grab the wheel, the PID makes the motor work harder to keep up with speed and you can see the green line jump up.

With it working on 1 motor, I then just copied it 3 times to get it working for the other motors. I didn't really encounter any issues controlling the output directly as I think I solved a lot of the issues in Inputs week before they happened, but I did need to tune the PID controller a bit as I was getting a severe issue where the motor would wildly swing from max speed to minimum and I just had to drop the RPM claculation window as low as possible tosmooth out this issue. At one stage I went down the rabbit hole of applying a small moving average to the RPM readings to help fix this, but I found that I didn't like the results and it made it even less responsive. But after some tweaking I found that this final code let the robot drive around well.

Final Code

from machine import Pin, PWM, Timer, UART
import time

# Rear Left Pins
RLENA = Pin(13, Pin.OUT)
RLIN1 = Pin(14, Pin.OUT)
RLIN2 = Pin(15, Pin.OUT)
RLencoderPinA = Pin(2, Pin.IN, Pin.PULL_UP)
RLencoderPinB = Pin(3, Pin.IN, Pin.PULL_UP)

# Rear Right Pins
RRENB = Pin(10, Pin.OUT)
RRIN3 = Pin(11, Pin.OUT)
RRIN4 = Pin(12, Pin.OUT)
RRencoderPinA = Pin(0, Pin.IN, Pin.PULL_UP)
RRencoderPinB = Pin(1, Pin.IN, Pin.PULL_UP)

# Front Left Pins
FLENA = Pin(18, Pin.OUT)
FLIN1 = Pin(17, Pin.OUT)
FLIN2 = Pin(16, Pin.OUT)
FLencoderPinA = Pin(26, Pin.IN, Pin.PULL_UP)
FLencoderPinB = Pin(22, Pin.IN, Pin.PULL_UP)

# Front Right Pins
FRENB = Pin(21, Pin.OUT)
FRIN3 = Pin(20, Pin.OUT)
FRIN4 = Pin(19, Pin.OUT)
FRencoderPinA = Pin(27, Pin.IN, Pin.PULL_UP)
FRencoderPinB = Pin(28, Pin.IN, Pin.PULL_UP)

# Encoder variables
RLencoder_position = 0
RRencoder_position = 0
FLencoder_position = 0
FRencoder_position = 0

# PID control variables
Kp = 0.3  # Reduced Kp for less aggressive control
Ki = 0.2  # Reduced Ki
Kd = 0.3  # Reduced Kd
rpm_calculation_period = 0.04

# rpm stuff
globalRPM = 30

RLsetpointRPM = globalRPM
RLinputRPM = 0.0
RLoutputPWM = 0.0
RLprevious_error = 0.0
RLintegral = 0.0

RRsetpointRPM = globalRPM
RRinputRPM = 0.0
RRoutputPWM = 0.0
RRprevious_error = 0.0
RRintegral = 0.0

FLsetpointRPM = globalRPM
FLinputRPM = 0.0
FLoutputPWM = 0.0
FLprevious_error = 0.0
FLintegral = 0.0

FRsetpointRPM = globalRPM
FRinputRPM = 0.0
FRoutputPWM = 0.0
FRprevious_error = 0.0
FRintegral = 0.0

# Constants
counts_per_revolution = 1920.0

# PWM setup
RLpwm = PWM(RLENA)
RLpwm.freq(1000)

RRpwm = PWM(RRENB)
RRpwm.freq(1000)

FLpwm = PWM(FLENA)
FLpwm.freq(1000)

FRpwm = PWM(FRENB)
FRpwm.freq(1000)

# Interrupt handler for encoder
def RRencoder(pin):
    global RRencoder_position

    if RRencoderPinA.value() == RRencoderPinB.value():
        RRencoder_position += 1
    else:
        RRencoder_position -= 1

def RLencoder(pin):
    global RLencoder_position

    if RLencoderPinA.value() == RLencoderPinB.value():
        RLencoder_position -= 1
    else:
        RLencoder_position += 1

def FRencoder(pin):
    global FRencoder_position

    if FRencoderPinA.value() == FRencoderPinB.value():
        FRencoder_position += 1
    else:
        FRencoder_position -= 1

def FLencoder(pin):
    global FLencoder_position

    if FLencoderPinA.value() == FLencoderPinB.value():
        FLencoder_position -= 1
    else:
        FLencoder_position += 1

# Attach interrupt to encoder pins
RLencoderPinA.irq(trigger=Pin.IRQ_RISING, handler=RLencoder)
RRencoderPinA.irq(trigger=Pin.IRQ_RISING, handler=RRencoder)
FLencoderPinA.irq(trigger=Pin.IRQ_RISING, handler=FLencoder)
FRencoderPinA.irq(trigger=Pin.IRQ_RISING, handler=FRencoder)

def pid_control():
    global RRprevious_error, RRintegral, RRoutputPWM
    global RLprevious_error, RLintegral, RLoutputPWM
    global FRprevious_error, FRintegral, FRoutputPWM
    global FLprevious_error, FLintegral, FLoutputPWM

    # Rear Right Motor PID
    error = RRsetpointRPM - RRinputRPM
    RRintegral += error
    derivative = error - RRprevious_error
    RRoutputPWM = Kp * error + Ki * RRintegral + Kd * derivative
    RRprevious_error = error

    # Rear Left Motor PID
    error = RLsetpointRPM - RLinputRPM
    RLintegral += error
    derivative = error - RLprevious_error
    RLoutputPWM = Kp * error + Ki * RLintegral + Kd * derivative
    RLprevious_error = error

    # Front Right Motor PID
    error = FRsetpointRPM - FRinputRPM
    FRintegral += error
    derivative = error - FRprevious_error
    FRoutputPWM = Kp * error + Ki * FRintegral + Kd * derivative
    FRprevious_error = error

    # Front Left Motor PID
    error = FLsetpointRPM - FLinputRPM
    FLintegral += error
    derivative = error - FLprevious_error
    FLoutputPWM = Kp * error + Ki * FLintegral + Kd * derivative
    FLprevious_error = error

def set_motor_speed(RLspeed, RRspeed, FLspeed, FRspeed):
    # Rear Right Motor
    if RRspeed > 0:
        RRIN3.value(1)
        RRIN4.value(0)
    elif RRspeed < 0:
        RRIN3.value(0)
        RRIN4.value(1)
        RRspeed = -RRspeed
    else:
        RRIN3.value(0)
        RRIN4.value(0)

    # Convert speed percentage to duty cycle (0-65535)
    duty_cycle = int((RRspeed / 100) * 65535)
    RRpwm.duty_u16(duty_cycle)

    # Rear Left Motor
    if RLspeed > 0:
        RLIN1.value(0)
        RLIN2.value(1)
    elif RLspeed < 0:
        RLIN1.value(1)
        RLIN2.value(0)
        RLspeed = -RLspeed
    else:
        RLIN1.value(0)
        RLIN2.value(0)

    # Convert speed percentage to duty cycle (0-65535)
    duty_cycle = int((RLspeed / 100) * 65535)
    RLpwm.duty_u16(duty_cycle)

    # Front Right Motor
    if FRspeed > 0:
        FRIN3.value(1)
        FRIN4.value(0)
    elif FRspeed < 0:
        FRIN3.value(0)
        FRIN4.value(1)
        FRspeed = -FRspeed
    else:
        FRIN3.value(0)
        FRIN4.value(0)

    # Convert speed percentage to duty cycle (0-65535)
    duty_cycle = int((FRspeed / 100) * 65535)
    FRpwm.duty_u16(duty_cycle)

    # Front Left Motor
    if FLspeed > 0:
        FLIN1.value(0)
        FLIN2.value(1)
    elif FLspeed < 0:
        FLIN1.value(1)
        FLIN2.value(0)
        FLspeed = -FLspeed
    else:
        FLIN1.value(0)
        FLIN2.value(0)

    # Convert speed percentage to duty cycle (0-65535)
    duty_cycle = int((FLspeed / 100) * 65535)
    FLpwm.duty_u16(duty_cycle)

while True:

    # Here is where in the future we will update our target rpms, but we will use static ones for now

    RRinputRPM = (166.666 * RRencoder_position / counts_per_revolution) / rpm_calculation_period
    RLinputRPM = (166.666 * RLencoder_position / counts_per_revolution) / rpm_calculation_period 
    FRinputRPM = (166.666 * FRencoder_position / counts_per_revolution) / rpm_calculation_period
    FLinputRPM = (166.666 * FLencoder_position / counts_per_revolution) / rpm_calculation_period

    print("RR", RRinputRPM, "RL", RLinputRPM, "FR", FRinputRPM, "FL", FLinputRPM)

    # Reset encoder positions for the next calculation period
    RRencoder_position = 0
    RLencoder_position = 0
    FRencoder_position = 0
    FLencoder_position = 0

    pid_control()
    set_motor_speed(RLoutputPWM, RRoutputPWM, FLoutputPWM, FRoutputPWM)

    # Small delay to prevent overwhelming the CPU
    time.sleep(rpm_calculation_period)