# File: WHAM.py
# Motor object to control 1 motor through L293D
# updated 13/11/2021
# Use Motor("L") or Motor("R") to choose which motor
# left/front/right wall snors
# 2/12/21


from machine import Pin, PWM, ADC
import time, utime

##################### IO PORTS#####################
# buttons
Button1 = Pin(15, Pin.IN, Pin.PULL_DOWN) #Hw20 Left button
Button3 = Pin(20, Pin.IN, Pin.PULL_DOWN) #Hw26 Right Button
Button = Button3

#switches
SW1 = Pin(16, Pin.IN, Pin.PULL_DOWN)  #Hw21 1st switch
SW2 = Pin(17, Pin.IN, Pin.PULL_DOWN)  #Hw22 2nd switch
SW3 = Pin(18, Pin.IN, Pin.PULL_DOWN)  #Hw24 3rd switch
SW4 = Pin(19, Pin.IN, Pin.PULL_DOWN)  #Hw25 4th switch


led  = Pin(25, Pin.OUT)   # inbuilt LED

R_led = Pin(2, Pin.OUT)   #  HW4 Right LED
M_led = Pin(3, Pin.OUT)   #  HW5 Middle led
L_led = Pin(4, Pin.OUT)   #  HW6 Left LED

# sensors
IntSense = ADC(Pin(26))   #  HW31  Line sensor
LWall    = ADC(Pin(27))   #  HW31  Left wall sensor
FWall    = ADC(Pin(26))   #  HW31  Front wall sensor
RWall    = ADC(Pin(28))   #  HW31  Right wall sensor

Wheel    = Pin(5, Pin.IN) #  HW7   Wheel sensor

# provides pin numbers (fwd,rev,pwm)for each motor
#LeftMotor  = Motor("L")      # uses 7,6,9 to set up left motor object
#RightMotor = Motor("R")      # uses 8,11,10 to set up right motor object

class Motor:
# Handles pin numbers and fwd reverse
# Drives an L293D motor driver chip
    Pinfwd = 0
    Pinback = 0
    Pinspeed = 0
    
    # constructor
    def __init__(self,type):   #tye="L" or "R"
        if ((type=="L")or(type=="l")):
            fwd  = 7 # pin number for forwards motion
            back = 6 # pin number for backwards motion
            spd  = 9 # pin number for speed control
        else:
            fwd  = 8  # pin number for forwards motion
            back = 11 # pin number for backwards motion
            spd  = 10 # pin number for speed control
            
        self.Pinfwd = Pin(fwd, Pin.OUT)    # forward pin object
        self.Pinback = Pin(back, Pin.OUT)  # reverse pin object
        self.Pinspeed = PWM(Pin(spd))      # PWM pin object
        self.Pinspeed.freq(1000)           # PWM frequency fixed at 1000
  
    # sets motor speed and direction range -100 to +100
    def speed(self,spd):
        #spd = int(spd)
        if (spd > 0):
            self.Pinfwd.on()   # sets pin high to go forward
            self.Pinback.off() # set pin high to go backward
        else:
            self.Pinfwd.off()  # set pin high to go forward
            self.Pinback.on()  # sets pin high to go backward
            spd =-spd
        spd = spd *655
        self.Pinspeed.duty_u16(int(min(spd,65535))) # set speed to speed
        
    def stop(self):   # provides braking
        self.Pinfwd.on()   # sets pin high to go forward
        self.Pinback.on()  # sets pin high to go backward
        self.Pinspeed.duty_u16(65535)