import utime

import time

from machine import Pin, I2C, PWM

import math

class PCA9685:

# Registers/etc.

__SUBADR1 = 0x02

__SUBADR2 = 0x03

__SUBADR3 = 0x04

__MODE1 = 0x00

__PRESCALE = 0xFE

__LED0_ON_L = 0x06

__LED0_ON_H = 0x07

__LED0_OFF_L = 0x08

__LED0_OFF_H = 0x09

__ALLLED_ON_L = 0xFA

__ALLLED_ON_H = 0xFB

__ALLLED_OFF_L = 0xFC65

__ALLLED_OFF_H = 0xFD

def __init__(self, address=0x40, debug=False):

self.i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=100000)

self.address = address

self.debug = debug

if (self.debug):

print("Reseting PCA9685")

self.write(self.__MODE1, 0x00)

def write(self, cmd, value):

self.i2c.writeto_mem(int(self.address), int(cmd), bytes([int(value)]))

if (self.debug):

print("I2C: Write 0x%02X to register 0x%02X" % (value, cmd))

def read(self, reg):

rdate = self.i2c.readfrom_mem(int(self.address), int(reg), 1)

if (self.debug):

print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % (self.address, int(reg), rdate[0]))

return rdate[0]

def setPWMFreq(self, freq):

"Sets the PWM frequency"

prescaleval = 25000000.0 

prescaleval /= 4096.0 

prescaleval /= float(freq)

prescaleval -= 1.0

if (self.debug):

print("Setting PWM frequency to %d Hz" % freq)

print("Estimated pre-scale: %d" % prescaleval)

prescale = math.floor(prescaleval + 0.5)

if (self.debug):

print("Final pre-scale: %d" % prescale)

oldmode = self.read(self.__MODE1)

newmode = (oldmode & 0x7F) | 0x10 

self.write(self.__MODE1, newmode) 

self.write(self.__PRESCALE, int(math.floor(prescale)))

self.write(self.__MODE1, oldmode)

time.sleep(0.005)

self.write(self.__MODE1, oldmode | 0x80)

def setPWM(self, channel, on, off):

"Sets a single PWM channel"

self.write(self.__LED0_ON_L+4*channel, on & 0xFF)

self.write(self.__LED0_ON_H+4*channel, on >> 8)

self.write(self.__LED0_OFF_L+4*channel, off & 0xFF)

self.write(self.__LED0_OFF_H+4*channel, off >> 8)

if (self.debug):

print("channel: %d LED_ON: %d LED_OFF: %d" % (channel,on,off))

def setServoPulse(self, channel, pulse):

pulse = pulse * (4095 / 100)

self.setPWM(channel, 0, int(pulse))

def setLevel(self, channel, value):

if (value == 1):

self.setPWM(channel, 0, 4095)

else:

self.setPWM(channel, 0, 0)

class MotorDriver():

def __init__(self, debug=False):

self.debug = debug

self.pwm = PCA9685()

self.pwm.setPWMFreq(50)

self.MotorPin = ['MA', 0,1,2, 'MB',3,4,5, 'MC',6,7,8, 'MD',9,10,11]

self.MotorDir = ['forward', 0,1, 'backward',1,0]

def MotorRun(self, motor, mdir, speed, runtime):

if speed > 100:

return

mPin = self.MotorPin.index(motor)

mDir = self.MotorDir.index(mdir)

if (self.debug):

print("set PWM PIN %d, speed %d" %(self.MotorPin[mPin+1], speed))

print("set pin A %d , dir %d" %(self.MotorPin[mPin+2], self.MotorDir[mDir+1]))

print("set pin b %d , dir %d" %(self.MotorPin[mPin+3], self.MotorDir[mDir+2]))

self.pwm.setServoPulse(self.MotorPin[mPin+1], speed)

self.pwm.setLevel(self.MotorPin[mPin+2], self.MotorDir[mDir+1])

self.pwm.setLevel(self.MotorPin[mPin+3], self.MotorDir[mDir+2])

time.sleep(runtime)

self.pwm.setServoPulse(self.MotorPin[mPin+1], 0)

self.pwm.setLevel(self.MotorPin[mPin+2], 0)

self.pwm.setLevel(self.MotorPin[mPin+3], 0)

def MotorStop(self, motor):

mPin = self.MotorPin.index(motor)

self.pwm.setServoPulse(self.MotorPin[mPin+1], 0)

class Servo:

def init(self, pin, min_us=1000, max_us=2000, angle=180):

self.pwm = PWM(Pin(pin))

self.min_us = min_us

self.max_us = max_us

self.angle = angle

self.pwm.freq(50)

self.set_angle(0)

def set_angle(self, angle):

angle = max(0, min(angle, self.angle))

us = self.min_us + (angle / self.angle) * (self.max_us - self.min_us)

self.pwm.duty_ns(int(us * 1000))

servo = Servo(12)

servo2 = Servo(15)

servo.set_angle(0)

servo2.set_angle(180)

p11 = Pin(11, Pin.OUT)

p10 = Pin(10, Pin.OUT)

p11.value(1)

p10.value(1)

if __name__ == '__main__':

m = MotorDriver()

servo = Servo(12)

count3 = 0

while True:

m.MotorRun('MB', 'forward', 75, 1)

time.sleep(1)

count = 1 

count2 = 179

while True: 

servo.set_angle(count)

servo2.set_angle(count2)

count += 10

count2 -= 10

utime.sleep(0.2)

if count 121 and count2 59:

servo.set_angle(count)

servo2.set_angle(count2)

break

utime.sleep(0.15)

m.MotorRun('MB', 'backward', 75, 1)

utime.sleep(0.2)

while True: # 팔 돌려놓기

servo.set_angle(count)

servo2.set_angle(count2)

time.sleep(0.1)

count -= 10

count2 += 10

time.sleep(0.04)

if count 1 and count2 179:

servo.set_angle(count)

servo2.set_angle(count2)

break

if count3 == 1000:

break