'''
///////////////////////////////////////////////////////////////////////////
// Programa desenvolvido por Cristiano Teixeira //
// Sob Licença Apache 2.0 //
// https://github.com/ProfessorCristiano //
///////////////////////////////////////////////////////////////////////////
'''
print("INICIO")
from machine import Pin, PWM, ADC
import time
###############VARIAVEIS-INICIAIS##############
# Intevalo para sincronizar os movimentos
intervalo = 0
###POSIÇÕES-MEMORIZADAS###
#ombro frente direito inicial
ofdi = 90
#ombro frente direito frente varia em +50 do inicial
f = 50
ofdf = ofdi + f
#ombro frente direito tras varia em -30 do inicial
t = -30
ofdt = ofdi + t
ofei = 90
ofef = ofei - f
ofet = ofei - t
otdi = 90
otdf = otdi + f
otdt = otdi + t
otei = 90
otef = otei - f
otet = otei - t
#joelho frente direito normal
jfdn = 90
#joelho frente direito medio varia em +10 do inicial
m = 10
jfdm = jfdn + m
#joelho frente direito alto varia em +20 do inicial
a = 20
jfda = jfdn + a
#joelho frente direito baixo varia em -50 do inicial
b = -50
jfdb = jfdn + b
jfen = 90
jfem = jfen + m
jfea = jfen + a
jfeb = jfen + b
jtdn = 90
jtdm = jtdn + m
jtda = jtdn + a
jtdb = jtdn + b
jten = 90
jtem = jten + m
jtea = jten + a
jteb = jten + b
##################################################################
#Classe Servo do site: https://www.upesy.com/blogs/tutorials/esp32-servo-motor-sg90-on-micropython#
class Servo:
# these defaults work for the standard TowerPro SG90
__servo_pwm_freq = 50
__min_u10_duty = 26 - 0 # offset for correction
__max_u10_duty = 123- 0 # offset for correction
min_angle = 0
max_angle = 180
current_angle = 0.001
def __init__(self, pin):
self.__initialise(pin)
def update_settings(self, servo_pwm_freq, min_u10_duty, max_u10_duty, min_angle, max_angle, pin):
self.__servo_pwm_freq = servo_pwm_freq
self.__min_u10_duty = min_u10_duty
self.__max_u10_duty = max_u10_duty
self.min_angle = min_angle
self.max_angle = max_angle
self.__initialise(pin)
def move(self, angle):
# round to 2 decimal places, so we have a chance of reducing unwanted servo adjustments
angle = round(angle, 2)
# do we need to move?
if angle == self.current_angle:
return
self.current_angle = angle
# calculate the new duty cycle and move the motor
duty_u10 = self.__angle_to_u10_duty(angle)
self.__motor.duty(duty_u10)
def __angle_to_u10_duty(self, angle):
return int((angle - self.min_angle) * self.__angle_conversion_factor) + self.__min_u10_duty
def __initialise(self, pin):
self.current_angle = -0.001
self.__angle_conversion_factor = (self.__max_u10_duty - self.__min_u10_duty) / (self.max_angle - self.min_angle)
self.__motor = PWM(Pin(pin))
self.__motor.freq(self.__servo_pwm_freq)
##########################################
# Criamos a instância para o servo nos pinos
ofd = Servo(pin=19)
ofe = Servo(pin=21)
otd = Servo(pin=5)
ote = Servo(pin=4)
jfd = Servo(pin=32)
jfe = Servo(pin=33)
jtd = Servo(pin=13)
jte = Servo(pin=14)
###################################
###FUNÇÕES-PARA-SERVOS###################
#Função que faz o movimento
def mexe (motor, x, y):
if x < y:
for k in range (x,y,1):
motor.move(k)
else:
for k in range (x,y,-1):
motor.move(k)
##########################################
#Função que faz o movimento pausado
def mexep (motor1, x1, y1):
if x1 < y1:
ik1 = 1
else:
ik1 = -1
k1=x1
f1=y1
for j in range (k1,f1,ik1):
#print(j)
if(k1 != f1):
motor1.move(k1)
k1=k1+ik1
time.sleep(intervalo)
##########################################
#Função que faz 2 movimentos conjuntos
def mexe2 (motor1, x1, y1, motor2, x2, y2):
if x1 < y1:
ik1 = 1
v1=y1-x1
else:
ik1 = -1
v1=x1-y1
if x2 < y2:
ik2 = 1
v2=y2-x2
else:
ik2 = -1
v2=x2-y2
k1=x1
f1=y1
k2=x2
f2=y2
#A variação sempre será a maior de todas
v=max(v1,v2)
# A repetição ocorre na maior variação
for j in range (0,v,1):
#porém tem o if para saber se um motor move ou se fica parado
if(k1 != f1):
motor1.move(k1)
if(k2 != f2):
motor2.move(k2)
k1=k1+ik1
k2=k2+ik2
time.sleep(intervalo)
##########################################
#Função que faz 4 movimentos conjuntos
def mexe4 (motor1, x1, y1, motor2, x2, y2, motor3, x3, y3, motor4, x4, y4):
if x1 < y1:
ik1 = 1
v1=y1-x1
else:
ik1 = -1
v1=x1-y1
if x2 < y2:
ik2 = 1
v2=y2-x2
else:
ik2 = -1
v2=x2-y2
if x3 < y3:
ik3 = 1
v3=y3-x3
else:
ik3 = -1
v3=x3-y3
if x4 < y4:
ik4 = 1
v4=y4-x4
else:
ik4 = -1
v4=x4-y4
v=max(v1,v2,v3,v4)
k1=x1
f1=y1
k2=x2
f2=y2
k3=x3
f3=y3
k4=x4
f4=y4
for j in range (0,v,1):
if(k1 != f1):
motor1.move(k1)
if(k2 != f2):
motor2.move(k2)
if(k3 != f3):
motor3.move(k3)
if(k4 != f4):
motor4.move(k4)
k1=k1+ik1
k2=k2+ik2
k3=k3+ik3
k4=k4+ik4
time.sleep(intervalo)
##########################################
##FUNÇÕES-DE-MOVIENTOS-PRE-DEFINIDOS##
def posicaoinicial():
#move todos para a Posição inicial:
ofd.move(ofdi)
ofe.move(ofei)
otd.move(otdi)
ote.move(otei)
jfd.move(jfdn)
jfe.move(jfen)
jtd.move(jtdn)
jte.move(jten)
##########################################
def alturanormal():
mexe4(jfd,jfdn,jfdn,jfe,jfen,jfen,jtd,jtdn,jtdn,jte,jten,jten)
##########################################
def medio():
mexe4(jfd,jfdn,jfdm,jfe,jfen,jfem,jtd,jtdn,jtdm,jte,jten,jtem)
##########################################
def deita():
mexe4(jfd,jfdn,jfdb,jfe,jfen,jfeb,jtd,jtdn,jtdb,jte,jten,jteb)
##########################################
def sobe():
mexe4(jfd,jfdn,jfda,jfe,jfen,jfea,jtd,jtdn,jtda,jte,jten,jtea)
##########################################
def inclinafrente():
mexe4(jfd,jfdn,jfdb,jfe,jfen,jfeb,jtd,jtdn,jtda,jte,jten,jtea)
##########################################
def inclinatras():
mexe4(jfd,jfdn,jfda,jfe,jfen,jfea,jtd,jtdn,jtdb,jte,jten,jteb)
##########################################
def inclinadireita():
mexe4(jfd,jfdn,jfdb,jfe,jfen,jfea,jtd,jtdn,jtdb,jte,jten,jtea)
##########################################
def inclinaesquerda():
mexe4(jfd,jfdn,jfda,jfe,jfen,jfeb,jtd,jtdn,jtda,jte,jten,jteb)
##########################################
def andarfrente():
print("Andar frente")
# o
#
#
# o
#levanta as patas
mexe2(jfd, jfdn, jfdm, jte, jten, jtem)
time.sleep(0.5)
mexe4(ofd, ofdf, ofdt, ote, otef, otet, ofe, ofet, ofef, otd, otdt, otdf)
time.sleep(0.5)
#abaixa as patas
mexe2(jfd, jfdm, jfdn, jte, jtem, jten)
# o
#
#
# o
#levanta as outras
mexe2(jfe, jfen, jfem, jtd, jtdn, jtdm)
time.sleep(0.5)
mexe4(ofd, ofdt, ofdf, ote, otet, otef, ofe, ofef, ofet, otd, otdf, otdt)
mexe2(jfe, jfem, jfen, jtd, jtdm, jtdn)
#########################################
def frentetras():
mexe4(ofd,ofdi,ofdf,ofe,ofei,ofef,otd,otdi,otdf,ote,otei,otef)
print("frente")
time.sleep(2)
mexe4(ofd,ofdf,ofdt,ofe,ofef,ofet,otd,otdf,otdt,ote,otef,otet)
print("tras")
#########################################
def sobedesce():
mexe4(jfd,jfdn,jfda,jfe,jfen,jfea,jtd,jtdn,jtda,jte,jten,jtea)
mexe4(jfd,jfda,jfdn,jfe,jfea,jfen,jtd,jtda,jtdn,jte,jtea,jten)
mexe4(jfd,jfdn,jfdb,jfe,jfen,jfeb,jtd,jtdn,jtdb,jte,jten,jteb)
mexe4(jfd,jfdb,jfdn,jfe,jfeb,jfen,jtd,jtdb,jtdn,jte,jteb,jten)
#########################################
########################INICIO#########################
#mexep(jfd,jfdb,jfdb)
while True:
#frentetras()
#time.sleep(1)
#sobedesce()
#time.sleep(1)
andarfrente()