├── LICENSE.md ├── README.md ├── encoder_N20_esp.py ├── media └── wire.png └── multi_motor.py /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 rakesh-i 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MicroPython-Encoder-motor 2 | Closed loop(PID) speed and position control library for N20 motors with encoders. 3 | ## Wiring example 4 | ![Schematic][wire] 5 | ## Usage 6 | ### Open loop speed control 7 | ``` 8 | from encoder_N20_esp import Motor 9 | # Create a motor object 10 | m = Motor(21, 22, 23, 16, 4) # Motor(M1, M2, EN, C1, C2, frequency) Default frequency is set at 50Hz 11 | # Call the speed method using the Motor object(m) 12 | # Speed range from -1000 to 1000(10 bit resolution). 13 | m.speed(100) 14 | # "-" symbol before the value suggests reverse direction of the rotation 15 | # m.speed(-100) # For reverse direction 16 | ``` 17 | ### Closed loop speed control 18 | ``` 19 | from encoder_N20_esp import Motor, PID 20 | m = Motor(21, 22, 23, 16, 4) # Motor object 21 | # Create a PID object with desired PID values(tested PID values for 12v 500rpm N20 motor) 22 | p = PID(m, 3, 0, 10, 800) # PID(Motor object, Propotional, Derivative, Integral, Max correction speed) 23 | # Create a loop 24 | # Call setSpeed method in a loop 25 | while(1): 26 | p.setSpeed(100) # setSpeed(Speed in RPM, Motor object) 27 | #p.setSpeed(-100) # For reverse direction 28 | ``` 29 | 30 | ### Closed loop position control 31 | ``` 32 | from encoder_N20_esp import Motor, PID 33 | m = Motor(21, 22, 23, 16, 4) 34 | p = PID(m, 5, 0.1, .001, 800) # Tested Pid values for 12v 500rpm N20 motor 35 | while(1): 36 | p.setTarget(1000) # setTarget(Number of encoder ticks) 37 | ``` 38 | Check the multi_motor.py for multiple motor contorl. 39 | #### Note1: Swap the C1 and C2 pins if encoder counts in only one direction 40 | #### Note2: To stop the motor set the motor speed to 0 by using "m.speed(0)" line. 41 | #### Note3: When you exit the loop, remember to set the motor speed to 0 just after the exit. 42 | #### Note4: Time delta is pre defined in the setTarget and setSpeed methods due to a bug in MicroPython. You can see the source code on how to overide it. (Do at you own will, results may vary) 43 | 44 | [wire]: media/wire.png 45 | -------------------------------------------------------------------------------- /encoder_N20_esp.py: -------------------------------------------------------------------------------- 1 | """ 2 | MIT License 3 | 4 | Copyright (c) 2022 rakesh-i 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | """ 24 | 25 | 26 | from time import sleep_ms, ticks_us 27 | from machine import Pin, PWM, disable_irq, enable_irq 28 | 29 | # A class for functions related to motors 30 | class Motor: 31 | 32 | # Instance variable for keeping the record of the encoder position 33 | pos = 0 34 | 35 | # Interrupt handler 36 | def handle_interrupt(self,pin): 37 | a = self.px.value() 38 | if a > 0: 39 | self.pos = self.pos+1 40 | else: 41 | self.pos = self.pos-1 42 | 43 | # Constroctor for initializing the motor pins 44 | def __init__(self,m1, m2, en, c1, c2, freq=50): 45 | self.px = Pin(c1, Pin.IN) 46 | self.py = Pin(c2, Pin.IN) 47 | self.freq = freq 48 | self.p_in1 = Pin(m1, Pin.OUT) 49 | self.p_in2 = Pin(m2, Pin.OUT) 50 | self.p_en = PWM(Pin(en,Pin.OUT), freq) 51 | # Interrupt initialization 52 | self.py.irq(trigger=Pin.IRQ_RISING, handler=self.handle_interrupt) 53 | 54 | # Arduino's map() function implementation in python 55 | def convert(self, x, i_m, i_M, o_m, o_M): 56 | return max(min(o_M, (x - i_m) * (o_M - o_m) // (i_M - i_m) + o_m), o_m) 57 | 58 | # A function for speed control without feedback(Open loop speed control) 59 | def speed(self,M): 60 | pwm = self.convert(abs(M),0, 1000, 0, 1000) 61 | self.p_en.duty(pwm) 62 | if M>0: 63 | self.p_in1(1) 64 | self.p_in2(0) 65 | else: 66 | self.p_in1(0) 67 | self.p_in2(1) 68 | 69 | # A class for closed loop speed and postion control 70 | class PID: 71 | 72 | # Instance variable for this class 73 | posPrev = 0 74 | 75 | # Constructor for initializing PID values 76 | def __init__(self, M, kp=1, kd=0, ki=0, umaxIn=800, eprev=0, eintegral=0): 77 | self.kp = kp 78 | self.kd = kd 79 | self.ki = ki 80 | self.M = M # Motor object 81 | self.umaxIn =umaxIn 82 | self.eprev = eprev 83 | self.eintegral = eintegral 84 | 85 | # Function for calculating the Feedback signal. It takes the current value, user target value and the time delta. 86 | def evalu(self,value, target, deltaT): 87 | 88 | # Propotional 89 | e = target-value 90 | 91 | # Derivative 92 | dedt = (e-self.eprev)/(deltaT) 93 | 94 | # Integral 95 | self.eintegral = self.eintegral + e*deltaT 96 | 97 | # Control signal 98 | u = self.kp*e + self.kd*dedt + self.ki*self.eintegral 99 | 100 | # Direction and power of the control signal 101 | if u > 0: 102 | if u > self.umaxIn: 103 | u = self.umaxIn 104 | else: 105 | u = u 106 | else: 107 | if u < -self.umaxIn: 108 | u = -self.umaxIn 109 | else: 110 | u = u 111 | self.eprev = e 112 | return u 113 | 114 | # Function for closed loop position control 115 | def setTarget(self,target): 116 | 117 | # Time delta is predefined as we have set a constat time for the loop.(Initial dealy is 118 | # very high,interfers with response time)(Integral part becomes very high due to huge deltaT value at the beginning) 119 | # Use tick_us() to calculate the delay manually(remove slee_ms() if you use realtime delay) 120 | deltaT = .01 121 | 122 | # Disable the interrupt to read the position of the encoder(encoder tick) 123 | state = disable_irq() 124 | step = self.M.pos 125 | 126 | # Enable the intrrupt after reading the position value 127 | enable_irq(state) 128 | 129 | # Control signal call 130 | x = int(self.evalu(step, target, deltaT)) 131 | 132 | # Set the speed 133 | self.M.speed(x) 134 | print(step, target) # For debugging 135 | 136 | # Constant delay 137 | sleep_ms(10) 138 | 139 | # Function for closed loop speed control 140 | def setSpeed(self, target): 141 | state = disable_irq() 142 | posi = self.M.pos 143 | enable_irq(state) 144 | 145 | # Delta is high because small delta causes drastic speed stepping. 146 | deltaT = .05 147 | 148 | # Target RPM 149 | vt = target 150 | 151 | # Current encoder tick rate 152 | velocity = (posi - self.posPrev)/deltaT 153 | self.posPrev = posi 154 | 155 | # Converted to RPM 156 | # 350 ticks per revolution of output shaft of the motor 157 | # For different gearing differnt values 158 | # Run the function without setting the motor speed and calculate the ticks per revolution by manually rotaing the motor 159 | v = velocity/350*60 160 | 161 | # Call for control signal 162 | x = int(self.evalu(v, vt, deltaT)) 163 | 164 | # Set the motor speed 165 | self.M.speed(x) 166 | print(v, vt) # For debugging 167 | 168 | # Constant delay 169 | sleep_ms(50) 170 | 171 | 172 | -------------------------------------------------------------------------------- /media/wire.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rakesh-i/MicroPython-Encoder-motor/fc895f10c849aa5eeb89dc45848d0303c125c3b2/media/wire.png -------------------------------------------------------------------------------- /multi_motor.py: -------------------------------------------------------------------------------- 1 | from encoder_N20_esp import PID, Motor 2 | 3 | # Creating objects of each motor 4 | m1 = Motor(21, 22, 23, 16, 4) 5 | m2 = Motor(19, 18, 5, 14, 27) 6 | 7 | # Creating PID objects for each motor 8 | p1 = PID(m1, 5, 0.1, 0.001, 800) 9 | p2 = PID(m2, 5, 0.1, 0.001, 800) 10 | 11 | while(1): 12 | p1.setTarget(1000) 13 | p2.setTarget(-1000) 14 | --------------------------------------------------------------------------------