import RPi.GPIO as GPIO
from picamera import PiCamera
import uuid
import boto3
import json
import paho.mqtt.client as mqtt
from time import time


FORWARD = "forward"
BACKWARD = "backward"
LEFT = "left"
RIGHT = "right"
STOP = "stop"
GEAR_1 = "1"
GEAR_2 = "2"
GEAR_3 = "3"
CLICK = "click"

GEAR_1_DUTYCYCLE = 50
GEAR_2_DUTYCYCLE = 70
GEAR_3_DUTYCYCLE = 100

WHEEL_LEFT_PWM = 2
WHEEL_RIGHT_PWM = 3

WHEEL_RIGHT_1 = 6
WHEEL_RIGHT_2 = 13

WHEEL_LEFT_1 = 19
WHEEL_LEFT_2 = 26

camera = PiCamera()
s3 = boto3.resource("s3", config= boto3.session.Config(signature_version='s3v4'))


def forward():
    print("Moving forward")
    GPIO.output(WHEEL_RIGHT_1, False)
    GPIO.output(WHEEL_RIGHT_2, True)
    GPIO.output(WHEEL_LEFT_1, False)
    GPIO.output(WHEEL_LEFT_2, True)


def backward():
    print("Moving backward")
    GPIO.output(WHEEL_RIGHT_1, True)
    GPIO.output(WHEEL_RIGHT_2, False)
    GPIO.output(WHEEL_LEFT_1, True)
    GPIO.output(WHEEL_LEFT_2, False)


def left():
    print("Moving left")
    GPIO.output(WHEEL_RIGHT_1, False)
    GPIO.output(WHEEL_RIGHT_2, True)
    GPIO.output(WHEEL_LEFT_1, True)
    GPIO.output(WHEEL_LEFT_2, False)


def right():
    print("Moving right")
    GPIO.output(WHEEL_RIGHT_1, True)
    GPIO.output(WHEEL_RIGHT_2, False)
    GPIO.output(WHEEL_LEFT_1, False)
    GPIO.output(WHEEL_LEFT_2, True)


def stop():
    print("Stop moving")
    GPIO.output(WHEEL_RIGHT_1, False)
    GPIO.output(WHEEL_RIGHT_2, False)
    GPIO.output(WHEEL_LEFT_1, False)
    GPIO.output(WHEEL_LEFT_2, False)


def changeGear(dutyCycle):
        print(("Change dutycyle to " + str(dutyCycle)))
        rightPwm.ChangeDutyCycle(dutyCycle)
        leftPwm.ChangeDutyCycle(dutyCycle)


def captureImage():
        print("Capture image")
        s3FileName = str(time()) + "_" + str(uuid.uuid4())
        camera.capture("image.jpg")
        data = open("image.jpg", "rb")
        print("Tranfer image to S3")
        s3.Bucket("com.senacor.tecco.insanerobot").put_object(Key="test/"+s3FileName, Body=data)
        print(("Send MQTT notification for image " + s3FileName))
        client.publish("robot/camera", json.JSONEncoder().encode({"image": s3FileName}))


def on_connect(client, userdata, flags, rc):
    print(("Connected with result code " + str(rc)))
    client.subscribe("robot/#")


def on_message(client, userdata, msg):
    command = str(msg.payload.decode("utf-8"))
    topic = msg.topic
    print((topic + " " + command))

    if topic == "robot/drive":
        if command == FORWARD:
            forward()
        elif command == BACKWARD:
            backward()
        elif command == LEFT:
            left()
        elif command == RIGHT:
            right()
        elif command == STOP:
            stop()
        else:
            print(("Unhandled command " + command + " in topic " + topic))
    elif topic == "robot/gear":
        if command == GEAR_1:
            changeGear(GEAR_1_DUTYCYCLE)
        elif command == GEAR_2:
            changeGear(GEAR_2_DUTYCYCLE)
        elif command == GEAR_3:
            changeGear(GEAR_3_DUTYCYCLE)
        else:
            print(("Unhandled command " + command + " in topic " + topic))
    elif topic == "robot/camera":
        if command == CLICK:
            captureImage()
        else:
            print(("Unhandled command " + command + " in topic " + topic))
    else:
        print(("Unknown topic " + topic))


def on_publish(client, userdata, mid):
    print((str(client) + " " + str(mid)))


try:
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(WHEEL_RIGHT_1, GPIO.OUT)
    GPIO.setup(WHEEL_RIGHT_2, GPIO.OUT)
    GPIO.setup(WHEEL_LEFT_1, GPIO.OUT)
    GPIO.setup(WHEEL_LEFT_2, GPIO.OUT)
    GPIO.setup(WHEEL_RIGHT_PWM, GPIO.OUT)
    GPIO.setup(WHEEL_LEFT_PWM, GPIO.OUT)

    rightPwm = GPIO.PWM(WHEEL_RIGHT_PWM, 100)
    rightPwm.start(GEAR_1_DUTYCYCLE)
    leftPwm = GPIO.PWM(WHEEL_LEFT_PWM, 100)
    leftPwm.start(GEAR_1_DUTYCYCLE)

    client = mqtt.Client()
    client.on_connect = on_connect
    client.on_message = on_message
    client.on_publish = on_publish
    client.connect("10.22.0.204", 1883, 60)

    client.loop_forever()


finally:
    rightPwm.stop()
    leftPwm.stop()
    GPIO.cleanup()

@@ -577,46 +577,77 @@
 def 
-forward():%0A    print(%22Moving forward%22)
+wheelState(state_right_1, state_right_2, state_left_1, state_left_2):
 %0A   
@@ -666,37 +666,45 @@
 (WHEEL_RIGHT_1, 
-False
+state_right_1
 )%0A    GPIO.outpu
@@ -712,36 +712,45 @@
 (WHEEL_RIGHT_2, 
-True
+state_right_2
 )%0A    GPIO.outpu
@@ -757,37 +757,44 @@
 t(WHEEL_LEFT_1, 
-False
+state_left_1
 )%0A    GPIO.outpu
@@ -809,28 +809,36 @@
 LEFT_2, 
-True
+state_left_2
 )%0A%0A%0Adef 
 backward
@@ -829,20 +829,19 @@
 )%0A%0A%0Adef 
-back
+for
 ward():%0A
@@ -858,20 +858,19 @@
 %22Moving 
-back
+for
 ward%22)%0A 
@@ -876,143 +876,124 @@
     
-GPIO.output(WHEEL_RIGHT_1, True)%0A    GPIO.output(WHEEL_RIGHT_2, False)%0A    GPIO.output(WHEEL_LEFT_1, True)%0A    GPIO.output(WHEEL_LEFT_2
+wheelState(False, True, False, True)%0A%0A%0Adef backward():%0A    print(%22Moving backward%22)%0A    wheelState(True, False, True
 , Fa
@@ -1233,332 +1233,118 @@
     
-GPIO.output(WHEEL_RIGHT_1, True)%0A    GPIO.output(WHEEL_RIGHT_2, False)%0A    GPIO.output(WHEEL_LEFT_1, False)%0A    GPIO.output(WHEEL_LEFT_2, True)%0A%0A%0Adef stop():%0A    print(%22Stop moving%22)%0A    GPIO.output(WHEEL_RIGHT_1, False)%0A    GPIO.output(WHEEL_RIGHT_2, False)%0A    GPIO.output(WHEEL_LEFT_1, False)%0A    GPIO.output(WHEEL_LEFT_2
+wheelState(True, False, False, True)%0A%0A%0Adef stop():%0A    print(%22Stop moving%22)%0A    wheelState(False, False, False
 , Fa

Refactor wheel state setting