/
non_threaded_pid.py
157 lines (131 loc) · 3.71 KB
/
non_threaded_pid.py
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
import time
import rp2
import machine
from machine import Pin, PWM
machine.freq(100000000) # set the CPU frequency to 240 MHz
m1pin1 = Pin(5)
m1pin2 = Pin(4)
m2pin1 = Pin(18)
m2pin2 = Pin(17)
m1pwm1 = PWM(m1pin1)
m1pwm2 = PWM(m1pin2)
m2pwm1 = PWM(m2pin1)
m2pwm2 = PWM(m2pin2)
LED = Pin(6, Pin.OUT)
max_duty = 65535 # constant
saturated_duty = 16000 # choice for max speed
turn90ticks = 115
turn_error = 10
enc_max_value = 20000
kp = 0.8
ki = 0.08
kd = 0.04
encpins = (15, 25, 7, 27)
enc1p1 = Pin(encpins[0], Pin.IN)
enc1p2 = Pin(encpins[1], Pin.IN)
enc2p1 = Pin(encpins[2], Pin.IN)
enc2p2 = Pin(encpins[3], Pin.IN)
enc1 = 0
enc2 = 0
enc1dir = 1
enc2dir = 1
def enc_pin_high(pin):
global enc1dir
global enc2dir
global enc1
global enc2
if pin == encpins[0] or pin == encpins[1]:
if enc1p1.value() == 1 and enc1p2.value() == 1:
enc1 += 1 * enc1dir
elif enc1p1.value() == 1:
enc1dir = 1
else:
enc1dir = -1
if pin == encpins[2] or pin == encpins[3]:
if enc2p1.value() == 1 and enc2p2.value() == 1:
enc2 += 1 * enc2dir
elif enc2p1.value() == 1:
enc2dir = -1
else:
enc2dir = 1
enc1p1.irq(lambda pin: enc_pin_high(15), Pin.IRQ_RISING)
enc1p2.irq(lambda pin: enc_pin_high(25), Pin.IRQ_RISING)
enc2p1.irq(lambda pin: enc_pin_high(7), Pin.IRQ_RISING)
enc2p2.irq(lambda pin: enc_pin_high(27), Pin.IRQ_RISING)
def calc_duty(duty_100):
return int(duty_100 * max_duty / 100)
def m1Forward(dutyCycle):
m1pwm1.duty_u16(min(calc_duty(dutyCycle), saturated_duty))
m1pwm2.duty_u16(0)
def m1Backward(dutyCycle):
m1pwm1.duty_u16(0)
m1pwm2.duty_u16(min(calc_duty(dutyCycle), saturated_duty))
def m1Signed(dutyCycle):
if dutyCycle >= 0:
m1Forward(dutyCycle)
else:
m1Backward(-dutyCycle)
def m2Forward(dutyCycle):
m2pwm1.duty_u16(min(calc_duty(dutyCycle), saturated_duty))
m2pwm2.duty_u16(0)
def m2Backward(dutyCycle):
m2pwm1.duty_u16(0)
m2pwm2.duty_u16(min(calc_duty(dutyCycle), saturated_duty))
def m2Signed(dutyCycle):
if dutyCycle >= 0:
m2Forward(dutyCycle)
else:
m2Backward(-dutyCycle)
def allStop():
# set all duty cycles to 0
m1pwm1.duty_u16(0)
m1pwm2.duty_u16(0)
m2pwm1.duty_u16(0)
m2pwm2.duty_u16(0)
def setup():
# initialize frequencies
m1pwm1.freq(1000)
m1pwm2.freq(1000)
m2pwm1.freq(1000)
m2pwm2.freq(1000)
def ccw():
global enc1
global enc2
enc1 = 0
enc2 = 0
m1_integral = 0
m2_integral = 0
period = 1/10
m1_last_error = None
m2_last_error = None
while abs(enc1 - turn90ticks) > turn_error or abs(enc2 + turn90ticks) > turn_error:
m1_current_error = turn90ticks - enc1
m2_current_error = -turn90ticks - enc2
m1_integral += m1_current_error * period
m2_integral += m2_current_error * period
m1_derivative = 0
if not m1_last_error is None:
m1_derivative = (m1_current_error - m1_last_error) / period
m2_derivative = 0
if not m2_last_error is None:
m2_derivative = (m2_current_error - m2_last_error) / period
m1Signed(kp * m1_current_error + ki * m1_integral + kd * m1_derivative)
m2Signed(kp * m2_current_error + ki * m2_integral + kd * m2_derivative)
m1_last_error = m1_current_error
m2_last_error = m2_current_error
print(f'{enc1} {enc2} {m1_current_error} {m2_current_error} {m1_integral} {m2_integral}')
time.sleep(period)
setup()
while True:
LED.off()
m1Forward(90)
m2Forward(90)
time.sleep(1)
allStop()
time.sleep(1)
m1Backward(90)
m2Backward(90)
time.sleep(1)
allStop()
LED.on()
time.sleep(2)