# 這是馬達獨立的模組
# 如果獨立執行這個程式的話，主程式的部分會讓馬達進行4種類型的運轉，
# (F, B, L, R)，從而確認馬達確實可被這個程式所掌控。

import RPi.GPIO as GPIO
from time import sleep
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
 
class MOTOR():
    def __init__(self,EnaA,In1A,In2A,EnaB,In1B,In2B):
        self.EnaA = EnaA
        self.In1A = In1A
        self.In2A = In2A
        self.EnaB = EnaB
        self.In1B = In1B
        self.In2B = In2B
        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        PA = 2
        PB = 3
        PC = 15
        MAP = 22
        MAN = 23
        MBP = 17
        MBN = 27
        pwmf= 100
        dc  = 20
        PR  = 18
        PL  = 21
        GPIO.setup(PA, GPIO.IN)
        GPIO.setup(PB, GPIO.IN)
        GPIO.setup(PC, GPIO.IN)
        GPIO.setup(PR, GPIO.IN)
        GPIO.setup(PL, GPIO.IN)
        GPIO.setup(MAP, GPIO.OUT)
        GPIO.setup(MAN, GPIO.OUT)
        GPIO.setup(MBP, GPIO.OUT)
        GPIO.setup(MBN, GPIO.OUT)
        self.pwmap=GPIO.PWM(MAP, pwmf)
        self.pwman=GPIO.PWM(MAN, pwmf)
        self.pwmbp=GPIO.PWM(MBP, pwmf)
        self.pwmbn=GPIO.PWM(MBN, pwmf)
        
        self.pwmap.start(0)
        self.pwman.start(0)
        self.pwmbp.start(0)
        self.pwmbn.start(0)
 


    #TURN(LS,RS,t); LS < RS for a left turn
    def TURN(self,LS,RS,t):
        LS *=100
        RS *=100
        if(LS > 0):
            self.pwmap.start(LS)
            self.pwman.start(0)
        else:
            self.pwmap.start(0)
            self.pwman.start(-LS)
        if(RS > 0):
            self.pwmbp.start(RS)
            self.pwmbn.start(0)
        else:
            self.pwmbp.start(0)
            self.pwmbn.start(-RS)
        sleep(t)

    def forward(self):
        self.TURN(0.3,0.3,0.05); self.TURN(0.0,0.0,0.01)

    def left(self):
        self.TURN(0.2,0.6,0.05); self.TURN(0.0,0.0,0.01)

    def right(self):
        self.TURN(0.6,0.2,0.05); self.TURN(0.0,0.0,0.01)
    
    def backward(self):
        self.TURN(-0.3,-0.3,0.05); self.TURN(0.0,0.0,0.01)

    def stop(self):
        self.TURN(0.0,0.0,0.2)


    def testmotor(self):
        print('drive FORWARD 2 s ...')
        self.TURN(0.4,0.4,2.0); self.TURN(0.0,0.0,1.0) # stop motor
        print('drive BACKWARD 2 s ...')
        self.TURN(-0.4,-0.4,2.0);self.TURN(0.0,0.0,1.0) #stop motor
        print('LEFT TURN 2 s ...')
        self.TURN(0.2,0.6,4.0); self.TURN(0.0,0.0,1.0) # stop motor
        print('RIGHT TURN 2 s ...')
        self.TURN(0.6,0.2,4.0); self.TURN(0.0,0.0,1.0) # stop motor


# 如果獨立執行這個程式的話，這個主程式的部分會讓馬達做4種類型的運轉，
# 從而確認馬達確實可被這個程式所掌控。
def main():
    input('testmotor1 F, B, L, R ...')
    motor.testmotor()
    input('testmotor2 F, B, L, R ...')
    input('motor.forward...')
    motor.forward()
    motor.forward()
    motor.forward()
    motor.stop()
    motor.backward()
    motor.backward()
    motor.backward()
    motor.stop()
    motor.left()
    motor.left()
    motor.left()
    motor.stop()
    motor.right()
    motor.right()
    motor.right()
    motor.stop()
    #motor.testmotor()
    print('drive forward 2 s ...')
    input('motor.TURN...')
    motor.TURN(0.4,0.4,2.0); motor.TURN(0.0,0.0,1.0) # stop motor
    motor.TURN(-0.4,-0.4,2.0); motor.TURN(0.0,0.0,1.0) # stop motor
    motor.TURN(0.2,0.6,2.0); motor.TURN(0.0,0.0,1.0) # stop motor
    motor.TURN(0.6,0.2,2.0); motor.TURN(0.0,0.0,1.0) # stop motor
    input('testmotor done...')

    

 
if __name__ == '__main__':
    motor= MOTOR(22,3,23,17,22,27) #ref
    #motor= MOTOR(3,22,23,4,17,27)
    print('MOTOR pins defined...')
    main()