/
rotary_encoder2.py
131 lines (95 loc) · 3.37 KB
/
rotary_encoder2.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
#!/usr/bin/env python
import time
import pigpio
from pid_posicion import PID_Posicion as pidp
#import numpy as np
class decoder:
"""Class to decode mechanical rotary encoder pulses."""
def __init__(self, gpioA, gpioB,):
self.pi = pigpio.pi()
self.gpioA = gpioA
self.gpioB = gpioB
#self.callback = callback
self.levA = 0
self.levB = 0
self.lastGpio = None
self.pi.set_mode(gpioA, pigpio.INPUT)
self.pi.set_mode(gpioB, pigpio.INPUT)
self.pi.set_pull_up_down(gpioA, pigpio.PUD_UP)
self.pi.set_pull_up_down(gpioB, pigpio.PUD_UP)
"""VARIABLES DE INTERRUPCION"""
self.pos = 0
self.tiempo_pasado = time.time()
self.tiempo_actual = 0
self.velocidad = 0
self.pid_posicion = pidp()
self.status = 0
#self.archivo = open('datos_10mm_0.125_0_0.txt','w')
def callback(self,way):
self.pos += way#*0.12566370614359174
error = self.pid_posicion.pid.update(self.pos)
if abs(self.pos - self.pid_posicion.pid.set_point) > 0:
if error > 0 : direccion = 1
elif error < 0 : direccion = -1
#print error
sp = 3500 - abs(error)
if sp<0: self.pid_posicion.motor.avance(direccion)
else:
sp = sp * direccion
self.pid_posicion.motor.avance(sp)
else:
self.pid_posicion.motor.avance(0)
self.cancel()
self.status = 0
def _pulse(self, gpio, level, tick):
"""
Decodificamos el encoder rotatorio.
+---------+ +---------+ 0
| | | |
A | | | |
| | | |
+---------+ +---------+ +----- 1
+---------+ +---------+ 0
| | | |
B | | | |
| | | |
----+ +---------+ +---------+ 1
"""
if gpio == self.gpioA:
self.levA = level
else:
self.levB = level;
if gpio != self.lastGpio: # debounce
self.lastGpio = gpio
if gpio == self.gpioA and level == 1:
if self.levB == 1:
self.callback(-1)
elif gpio == self.gpioB and level == 1:
if self.levA == 1:
self.callback(1)
def cancel(self):
"""
Cancelamos las interrupciones , y por lo tanto el decoder.
"""
self.cbA.cancel()
self.cbB.cancel()
def SetPoint_posicion(self,setpoint):
self.cbA = self.pi.callback(self.gpioA, pigpio.EITHER_EDGE, self._pulse)
self.cbB = self.pi.callback(self.gpioB, pigpio.EITHER_EDGE, self._pulse)
if abs(setpoint-self.pos) < 0.5:
pass
else :
self.pid_posicion.SetPoint(setpoint)
self.status = 1
if __name__ == "__main__":
import rotary_encoder2
decoder = rotary_encoder2.decoder( 6, 13,)
try:
while 1:
sp = raw_input('ingresa comando : ')
if sp == 'pos': print "jfsdkfjsdfklsjdf %s"%decoder.pos
else : decoder.SetPoint_posicion(float(sp))
except KeyboardInterrupt:
decoder.pid_posicion.motor.parar()
decoder.cancel()
decoder.pi.stop()