Skip to content
This repository was archived by the owner on Feb 21, 2021. It is now read-only.

Control in position #1271

Merged
aitormf merged 4 commits into from
Oct 8, 2018
Merged

Control in position #1271

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
bc3471b
function move for real PiBot implemented
fgonzalezr1998 Oct 2, 2018
d3e2f21
latest version of piBot.py for real PiBot
fgonzalezr1998 Oct 3, 2018
4d19852
"leerangulorueda" function added to the driver pibot.py
fgonzalezr1998 Oct 6, 2018
c7fb367
"leerangulorueda" function added to the driver pibot.py
fgonzalezr1998 Oct 6, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
316 changes: 234 additions & 82 deletions src/drivers/PiBot/real/piBot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,24 +63,47 @@ def avanzar(self, vel):
@type vel: entero
@param vel: velocidad de avance del robot (máximo 255)
'''
# Puertos de datos para servos izquierdo y derecho
#puertos donde van conectados los motores
puertoL = 4
puertoR = 18
if(vel <= 0.0355):
self._dit.set_servo_pulsewidth(puertoL, 1527)
self._dit.set_servo_pulsewidth(puertoR, 1515)
elif(vel > 0.0355 and vel <= 0.0655):
self._dit.set_servo_pulsewidth(puertoL, 1540)
self._dit.set_servo_pulsewidth(puertoR, 1501)
elif(vel > 0.0655 and vel <= 0.0925):
self._dit.set_servo_pulsewidth(puertoL, 1550)
self._dit.set_servo_pulsewidth(puertoR, 1492)
elif(vel > 0.0925 and vel <= 0.13):
self._dit.set_servo_pulsewidth(puertoL, 1570)
self._dit.set_servo_pulsewidth(puertoR, 1472)

def esnegativo(vel):
return vel < 0

if(not esnegativo(vel)):
#self._dit.set_servo_pulsewidth(puertoL, 1720) #hacia el frente
#self._dit.set_servo_pulsewidth(puertoR, 1240) #hacia el frente
if(vel <= 0.0355): #velocidad muy lenta
self._dit.set_servo_pulsewidth(puertoL, 1529) #hacia el frente, 1526-1537
self._dit.set_servo_pulsewidth(puertoR, 1510) #hacia el frente, 1510-1512
elif(vel > 0.0355 and vel <= 0.0655):
self._dit.set_servo_pulsewidth(puertoL, 1540)
self._dit.set_servo_pulsewidth(puertoR, 1501)
elif(vel > 0.0655 and vel <= 0.0925):
self._dit.set_servo_pulsewidth(puertoL, 1550)
self._dit.set_servo_pulsewidth(puertoR, 1490)
elif(vel > 0.0925 and vel <= 0.13):
self._dit.set_servo_pulsewidth(puertoL, 1570)
self._dit.set_servo_pulsewidth(puertoR, 1474)
else: #velocidad muy rapida
self._dit.set_servo_pulsewidth(puertoL, 2500)
self._dit.set_servo_pulsewidth(puertoR, 500)
else:
self._dit.set_servo_pulsewidth(puertoL, 2500)
self._dit.set_servo_pulsewidth(puertoR, 500)
if(vel >= -0.0355): #velocidad muy lenta
self._dit.set_servo_pulsewidth(puertoL, 1498)
self._dit.set_servo_pulsewidth(puertoR, 1541)
elif(vel < -0.0355 and vel >= -0.0655):
self._dit.set_servo_pulsewidth(puertoL, 1490)
self._dit.set_servo_pulsewidth(puertoR, 1545)
elif(vel < -0.0655 and vel >= -0.0925):
self._dit.set_servo_pulsewidth(puertoL, 1480)
self._dit.set_servo_pulsewidth(puertoR, 1559)
elif(vel < -0.0925 and vel >= -0.13):
self._dit.set_servo_pulsewidth(puertoL, 1470)
self._dit.set_servo_pulsewidth(puertoR, 1568)
else: #velocidad muy rapida
self._dit.set_servo_pulsewidth(puertoL, 500)
self._dit.set_servo_pulsewidth(puertoR, 2500)

def retroceder(self, vel):
'''
Expand Down Expand Up @@ -138,78 +161,207 @@ def move(self, velV, velW):
# Puertos de datos para servos izquierdo y derecho
puertoL = 4
puertoR = 18
#servos = pigpio.pi()
if ((velV == 0) and (velW != 0)): # pivotar
if (velW > 0): # hacia derecha
if (velW < 3): # despacio
self._dit.set_servo_pulsewidth(puertoL, 1550)
self._dit.set_servo_pulsewidth(puertoR, 1520)

else: # a tope
self._dit.set_servo_pulsewidth(puertoL, 1700)
self._dit.set_servo_pulsewidth(puertoR, 1700)
def esnegativo(n):
return n < 0

else: # hacia izquierda
if (velW > -3): # despacio
self._dit.set_servo_pulsewidth(puertoL, 1520)
self._dit.set_servo_pulsewidth(puertoR, 1490)
def espositivo(n):
return n > 0

def escero(n):
return n == 0

else: # a tope
self._dit.set_servo_pulsewidth(puertoL, 1300)
self._dit.set_servo_pulsewidth(puertoR, 1300)
def movermotorizq(vel):

elif ((velV != 0) and (velW == 0)): # linea recta
if (velV > 0): # adelante
if (velV < 2): # minimo
self._dit.set_servo_pulsewidth(puertoL, 1530)
self._dit.set_servo_pulsewidth(puertoR, 1509)
elif (velV <= 4): # despacio
if(not esnegativo(vel)):

if(escero(vel)):
self._dit.set_servo_pulsewidth(puertoL, 1510) #parado 1525
elif(vel <= 0.0355): #velocidad muy lenta
self._dit.set_servo_pulsewidth(puertoL, 1529) #hacia el frente, 1526-1537
elif(vel > 0.0355 and vel <= 0.0655):
self._dit.set_servo_pulsewidth(puertoL, 1540)
self._dit.set_servo_pulsewidth(puertoR, 1500)
else: # a tope
self._dit.set_servo_pulsewidth(puertoL, 1700)
self._dit.set_servo_pulsewidth(puertoR, 1300)

else: # atras
if (velV > -3): # despacio
self._dit.set_servo_pulsewidth(puertoL, 1520)
self._dit.set_servo_pulsewidth(puertoR, 1520)

else: # a tope
self._dit.set_servo_pulsewidth(puertoL, 1300)
self._dit.set_servo_pulsewidth(puertoR, 1700)

elif ((velV == 0) and (velW == 0)): # PARADO
self._dit.set_servo_pulsewidth(puertoL, 1525)
self._dit.set_servo_pulsewidth(puertoR, 1510)

elif ((velV > 0) and (velV <= 3)): # COMBINACION DE VELOCIDADES, AVANZANDO DESPACIO...
if ((velW > 0) and (velW <= 3)): # ...mientras gira despacio a derecha
self._dit.set_servo_pulsewidth(puertoL, 1540)
self._dit.set_servo_pulsewidth(puertoR, 1509)
elif (velW > 3): # ...mientras gira rapido a derecha
self._dit.set_servo_pulsewidth(puertoL, 1700)
self._dit.set_servo_pulsewidth(puertoR, 1509)
elif ((velW < 0) and (velW >= -3)): # ...mientras gira despacio a izquierda
self._dit.set_servo_pulsewidth(puertoL, 1530)
self._dit.set_servo_pulsewidth(puertoR, 1500)
elif (velW < -3): # ...mientras gira rapido a izquierda
self._dit.set_servo_pulsewidth(puertoL, 1530)
self._dit.set_servo_pulsewidth(puertoR, 1300)

elif (velV > 3): # COMBINACION DE VELOCIDADES, AVANZANDO RAPIDO...
if ((velW > 0) and (velW <= 3)): # ...mientras gira despacio a derecha
self._dit.set_servo_pulsewidth(puertoL, 1700)
self._dit.set_servo_pulsewidth(puertoR, 1500)
elif (velW > 3): # ...mientras gira rapido a derecha
self._dit.set_servo_pulsewidth(puertoL, 1700)
self._dit.set_servo_pulsewidth(puertoR, 1509)
elif ((velW < 0) and (velW >= -3)): # ...mientras gira despacio a izquierda
self._dit.set_servo_pulsewidth(puertoL, 1540)
self._dit.set_servo_pulsewidth(puertoR, 1300)
elif (velW < -3): # ...mientras gira rapido a izquierda
self._dit.set_servo_pulsewidth(puertoL, 1530)
self._dit.set_servo_pulsewidth(puertoR, 1300)
elif(vel > 0.0655 and vel <= 0.0925):
self._dit.set_servo_pulsewidth(puertoL, 1550)
elif(vel > 0.0925 and vel <= 0.13):
self._dit.set_servo_pulsewidth(puertoL, 1570)
else: #velocidad muy rapida
self._dit.set_servo_pulsewidth(puertoL, 2500)
else:
if(vel >= -0.0355): #velocidad muy lenta
self._dit.set_servo_pulsewidth(puertoL, 1498)
elif(vel < -0.0355 and vel >= -0.0655):
self._dit.set_servo_pulsewidth(puertoL, 1490)
elif(vel < -0.0655 and vel >= -0.0925):
self._dit.set_servo_pulsewidth(puertoL, 1480)
elif(vel < -0.0925 and vel >= -0.13):
self._dit.set_servo_pulsewidth(puertoL, 1470)
else: #velocidad muy rapida
self._dit.set_servo_pulsewidth(puertoL, 500)

def movermotordcho(vel):

if(not esnegativo(vel)):

if(escero(vel)):
self._dit.set_servo_pulsewidth(puertoR, 1525) #parado 1510
if(vel <= 0.0355): #velocidad muy lenta
self._dit.set_servo_pulsewidth(puertoR, 1510) #hacia el frente, 1510-1512
elif(vel > 0.0355 and vel <= 0.0655):
self._dit.set_servo_pulsewidth(puertoR, 1501)
elif(vel > 0.0655 and vel <= 0.0925):
self._dit.set_servo_pulsewidth(puertoR, 1490)
elif(vel > 0.0925 and vel <= 0.13):
self._dit.set_servo_pulsewidth(puertoR, 1474)
else: #velocidad muy rapida
self._dit.set_servo_pulsewidth(puertoR, 500)
else:
if(vel >= -0.0355): #velocidad muy lenta
self._dit.set_servo_pulsewidth(puertoR, 1541)
elif(vel < -0.0355 and vel >= -0.0655):
self._dit.set_servo_pulsewidth(puertoR, 1545)
elif(vel < -0.0655 and vel >= -0.0925):
self._dit.set_servo_pulsewidth(puertoR, 1559)
elif(vel < -0.0925 and vel >= -0.13):
self._dit.set_servo_pulsewidth(puertoR, 1568)
else: #velocidad muy rapida
self._dit.set_servo_pulsewidth(puertoR, 2500)

#Aqui empieza la algoritmia principal

if(velW != 0):
rcir = abs(velV / velW) #Es el radio de la circunferencia que tengo que trazar. En valor absoluto
velmotorgiro = abs(velV) + rcir #Velocidad a la que tiene que girar el motor encargado del giro del robot
if(escero(velV) and not escero(velW)):
#Motor izquierdo hacia atras y motor derecho hacia adelante a velocidad maxima
if(espositivo(velW)):
movermotordcho(1)
movermotorizq(-1)
else:
movermotordcho(-1)
movermotorizq(1)

elif(not escero(velV) and escero(velW)):
#Avanza hacia el frente a la velocidad lineal dada
movermotordcho(velV)
movermotorizq(velV)

elif(espositivo(velV) and espositivo(velW)):
movermotorizq(velV)
movermotordcho(velmotorgiro)
elif(espositivo(velV) and esnegativo(velW)):
movermotorizq(velmotorgiro)
movermotordcho(velV)
elif(esnegativo(velV) and espositivo(velW)):
movermotorizq(-velmotorgiro)
movermotordcho(velV)
elif(esnegativo(velV) and esnegativo(velW)):
movermotorizq(velV)
movermotordcho(-velmotorgiro)

def leerangulorueda(self, Encoder):
#devuelve el angulo en el que se encuentra la rueda cuyo pin de feedback
#se encuentra en el pin "Encoder" en el momento en que se llama a la funcion

#Constantes:
DcMin = 29
DcMax = 971
Pi = 3.1416
FullCircle = 2 * Pi
DutyScale = 1000
Q2Min = FullCircle / 4 #angulo minimo perteneciente al segundo cuadrante
Q3Max = Q2Min * 3 #angulo maximo perteneciente al tercer cuadrante

turns = 0

def pulse_in(inp, bit):

def readuntil(inp, bit):
rec = self._GPIO.input(inp)
if(rec == bit):
#esperar hasta terminar de leer unos
while(rec == bit):
rec = self._GPIO.input(inp)
#leer ceros hasta que me llegue el primer uno
if(bit == 1):
while(rec == 0):
rec = self._GPIO.input(inp)
#ahora me acaba de llegar el primer uno despues de los ceros
else:
while(rec == 1):
rec = self._GPIO.input(inp)
#ahora me acaba de llegar el primer cero despues de los unos

if(bit != 0 and bit != 1):
return 0
else:
readuntil(inp, bit) #leo hasta que me llega ese bit
start = time.time() #guardo la hora actual
if(bit == 1):
readuntil(inp, 0) #leo hasta que me llega un bit contrario al anterior
else:
readuntil(inp, 1)
finish = time.time()
elapsed = (finish - start) * 1000000 #tiempo en microsegundos

return elapsed #todavia esto no son microsegundos. Hay que pasarlo

def initangle():
timeHigh = pulse_in(Encoder, 1) #devuelve el tiempo en microsegundos
timeLow = pulse_in(Encoder, 0) #devuelve el tiempo en microsegundos
timeCycle = timeHigh + timeLow
dutyCycle = (DutyScale * timeHigh) / timeCycle #calculo el ciclo de trabajo
return (FullCircle - 1) - ((dutyCycle - DcMin) * FullCircle) / (DcMax - DcMin + 1)


#se inicializa la configuracion de los pines correspondientes:
self._GPIO.setmode(self._GPIO.BCM)
self._GPIO.setup(Encoder, self._GPIO.IN)


#calculo el angulo inicial
angle = initangle()
p_angle = angle

finish = False
while(not finish):
timeHigh = pulse_in(Encoder, 1) #devuelve el tiempo en microsegundos
timeLow = pulse_in(Encoder, 0) #devuelve el tiempo en microsegundos

timeCycle = timeHigh + timeLow

if((timeCycle > 1000) and (timeCycle < 1200)):
finish = True

dutyCycle = (DutyScale * timeHigh)/ timeCycle #calculo el ciclo de trabajo

angle = (FullCircle - 1) - ((dutyCycle - DcMin) * FullCircle) / (DcMax - DcMin + 1)
if(angle < 0):
angle = 0
elif(angle > (FullCircle - 1)):
angle = FullCircle - 1

#Si la transicion va del cuarto cuadrante al primero, incremento 'turns'
if((angle < Q2Min) and (p_angle > Q3Max)):
turns = turns + 1
#Si la transicion va del primer cuadrante al cuarto, incremento 'turns'
elif((p_angle < Q2Min) and (angle > Q3Max)):
turns = turns - 1

#Calculo el angulo
if(turns >= 0):
angle = (turns * FullCircle) + angle
elif(turns < 0):
angle = ((turns + 1) * FullCircle) - (FullCircle - angle)
#Esto lo hago para que cuando repita la vuelta se ponga a cero de nuevo
if(angle >= FullCircle):
angle = angle - FullCircle
turns = 0
elif(angle <= -FullCircle):
angle = angle + FullCircle
turns = 0

return angle

def leerIRSigueLineas(self): #devuelve el estado de los sensores IR

Expand Down