import machine
import time
import pca9685
import motor

# set up i2c and pca9685
sdaPIN=machine.Pin(13)
sclPIN=machine.Pin(14)
i2c=machine.I2C(0,sda=sdaPIN, scl=sclPIN)

# Initialize the PCA9685 using the default address (0x40).
pwm = pca9685.PCA9685(address=0x5f, i2c = i2c)
# Set the PWM frequency to 60Hz, good for servos.
pwm.freq(60)

motors = motor.DCMotors(address=0x5f, i2c = i2c)

def motor_move(motors, m1_speed=0, m2_speed=0, m3_speed=0, m4_speed=0):
    motors.speed(index=0, value= m1_speed)
    motors.speed(index=1, value= m2_speed)
    motors.speed(index=2, value= m3_speed)
    motors.speed(index=3, value= m4_speed)
    
# delay to unplug car
time.sleep(1)

for n in range(3):
    # move forward
    motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=2000, m4_speed=2000)
    time.sleep(1)
    motor_move(motors) # stops motor as default speed=0
    time.sleep(1)
    #move backward
    motor_move(motors, m1_speed=-2000, m2_speed=-2000, m3_speed=-2000, m4_speed=-2000)
    time.sleep(1)
    motor_move(motors)
    time.sleep(1)
    # turn left
    motor_move(motors, m1_speed=-2000, m2_speed=-2000, m3_speed=2000, m4_speed=2000)
    time.sleep(1)
    motor_move(motors)
    time.sleep(1)
    # turn right
    motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=-2000, m4_speed=-2000)
    time.sleep(1)
    motor_move(motors)
    time.sleep(1)