####################################################################################################
# control-zumo.py                                                                                  #
#                                                                                                  #
# control zumo robot via bluetooth                                                                 #
#                                                                                                  #
# prerequisites see:                                                                               #
# https://www.reddit.com/r/raspberry_pi/comments/6nchaj/guide_how_to_establish_bluetooth_serial/   #
#                                                                                                  #
# The Circuit:                                                                                     #
#   Output pins:                                                                                   #
#     - 10 (BCM) <->  NotorBoard MotorA In1                                                        #
#     -  9 (BCM) <->  NotorBoard MotorA In2                                                        #
#     -  8 (BCM) <->  NotorBoard MotorB In1                                                        #
#     -  7 (BCM) <->  NotorBoard MotorB In2                                                        #
#                                                                                                  #
# Created: 27.04.2020                                                                              #
# By: Henning Buchholz                                                                             #
# https://www.hennisoft.ie                                                                         #
#                                                                                                  #
# Released into the public domain.                                                                 #
#                                                                                                  #
####################################################################################################

import RPi.GPIO as GPIO  # Import the GPIO Library
import time  # Import the Time library
from bluetooth import * # import bluetooth

# setup bluetooth rfcomm communication
server_sock=BluetoothSocket( RFCOMM )
server_sock.bind(("",PORT_ANY))
server_sock.listen(1)

port = server_sock.getsockname()[1]

uuid = "94f39d29-7d6d-437d-973b-fba39e49d4ee"

advertise_service( server_sock, "SampleServer",
                   service_id = uuid,
                   service_classes = [ uuid, SERIAL_PORT_CLASS ],
                   profiles = [ SERIAL_PORT_PROFILE ], 
                    )

# Set the GPIO modes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

# Set variables for the GPIO motor pins
pinMotorAForwards = 10
pinMotorABackwards = 9
pinMotorBForwards = 8
pinMotorBBackwards = 7

# How many times to turn the pin on and off each second
Frequency = 20
# How long the pin stays on each cycle, as a percent
DutyCycle = 30
#DutyCycleB = 30
# Setting the duty cycle to 0 means the motors will not turn
Stop = 0

# Set the GPIO Pin mode to be Output
GPIO.setup(pinMotorAForwards, GPIO.OUT)
GPIO.setup(pinMotorABackwards, GPIO.OUT)
GPIO.setup(pinMotorBForwards, GPIO.OUT)
GPIO.setup(pinMotorBBackwards, GPIO.OUT)

# Set the GPIO to software PWM at 'Frequency' Hertz
pwmMotorAForwards = GPIO.PWM(pinMotorAForwards, Frequency)
pwmMotorABackwards = GPIO.PWM(pinMotorABackwards, Frequency)
pwmMotorBForwards = GPIO.PWM(pinMotorBForwards, Frequency)
pwmMotorBBackwards = GPIO.PWM(pinMotorBBackwards, Frequency)

# Start the software PWM with a duty cycle of 0 (i.e. not moving)
pwmMotorAForwards.start(Stop)
pwmMotorABackwards.start(Stop)
pwmMotorBForwards.start(Stop)
pwmMotorBBackwards.start(Stop)

# Turn all motors off
def stopmotors():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)

# Turn both motors forwards
def forwards():
    pwmMotorAForwards.ChangeDutyCycle(DutyCycle)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycle)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)

# Turn both motors backwards
def backwards():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycle)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycle)

# Turn left
def left():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycle)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycle)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)

# Turn Right
def right():
    pwmMotorAForwards.ChangeDutyCycle(DutyCycle)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycle)

def command(msg):
   #print "Got command:", msg    
   if msg[0] == "g":
      forwards()
      print("forwards")
   elif msg[0] == "b":    
      backwards()
      print("backwards")
   elif msg[0] == "r":    
      stopmotors()
      print("stopmotors")
   elif msg[0] == "o":    
      left()
      print("left")
   elif msg[0] == "a":    
      right()
      print("right")
   elif msg[0] == '*':
      global DutyCycle
      stopmotors()
      DutyCycle = int(msg[1:msg.find('~')])
      print("speed:", DutyCycle)

# check for incoming BT string
print("Waiting for connection on RFCOMM channel %d" % port)

client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)

try:
    while True:
        data = client_sock.recv(1024)
        if len(data) == 0: break
        command(data)
except IOError:
    pass

print("disconnected")

client_sock.close()
server_sock.close()
stopmotors()
GPIO.cleanup()
print("all done")