From 157c61ad879dbf5e4b42bac038c4c9eb59e32ed9 Mon Sep 17 00:00:00 2001
From: Simon Pirkelmann <simon.pirkelmann@gmail.com>
Date: Fri, 12 Apr 2019 21:43:13 +0200
Subject: [PATCH] added robot class and example movement sequence

---
 micropython_firmware/main.py | 72 ++++++++++++++++++++++++++++--------
 1 file changed, 57 insertions(+), 15 deletions(-)

diff --git a/micropython_firmware/main.py b/micropython_firmware/main.py
index 1466360..97db979 100644
--- a/micropython_firmware/main.py
+++ b/micropython_firmware/main.py
@@ -4,6 +4,9 @@ import time
 class Motor:
     def __init__(self, enable, dir1, dir2):
         self.enable_pin = machine.Pin(enable, machine.Pin.OUT)
+        self.enable_pwm = machine.PWM(self.enable_pin)
+        self.enable_pwm.freq(500)
+
         self.dir1_pin = machine.Pin(dir1, machine.Pin.OUT)
         self.dir2_pin = machine.Pin(dir2, machine.Pin.OUT)
 
@@ -11,25 +14,64 @@ class Motor:
         self.reverse()
 
     def reverse(self):
-        print("reversing direction")
         self.direction = not self.direction
         self.dir1_pin.value(self.direction)
         self.dir2_pin.value(not self.direction)
 
-    def spin(self,seconds):
-        self.enable_pin.value(1)
+    def control(self, value):
+        if value > 0.0: # forward
+            if not self.direction: # switch direction if necessary
+                self.reverse()
+        else: # backward
+            if self.direction: # switch direction if necessary
+                self.reverse()
+        # apply value as pwm signal
+        self.enable_pwm.duty(int(abs(value)*1023))
+
+
+class Robot:
+    def __init__(self):
+        print("setting up motors")
+        self.m1 = Motor(4, 16, 5)
+        self.m2 = Motor(14, 0, 2)
+        print("setup complete")
+
+    def forward(self, seconds):
+        print("Onward, to glory!")
+        self.m1.control(1.0)
+        self.m2.control(1.0)
         time.sleep(seconds)
-        self.enable_pin.value(0)
+        self.m1.control(0.0)
+        self.m2.control(0.0)
+    
+    def backward(self, seconds):
+        print("Better retreat...")
+        self.m1.control(-1.0)
+        self.m2.control(-1.0)
+        time.sleep(seconds)
+        self.m1.control(0.0)
+        self.m2.control(0.0)
+    
+    def spin_right(self, seconds):
+        print("You spin my head right round, right round..")
+        self.m1.control(0.8)
+        self.m2.control(-0.8)
+        time.sleep(seconds)
+        self.m1.control(0.0)
+        self.m2.control(0.0)
+    
+    def spin_left(self, seconds):
+        print("spinning left")
+        self.m1.control(-0.8)
+        self.m2.control(0.8)
+        time.sleep(seconds)
+        self.m1.control(0.0)
+        self.m2.control(0.0)
 
-print("setting up pins")
-m1 = Motor(4, 16, 5)
-m2 = Motor(14, 0, 2)
-print("setup complete")
+    def sequence(self):
+        self.spin_left(0.3)
+        self.spin_right(0.3)
+        self.forward(0.5)
+        self.backward(0.5)
 
-print("Motor test")
-print("Motor 1:")
-m1.spin(0.1)
-print("Motor 2:")
-m2.spin(0.1)
-
-print("done!")
+wall_e = Robot()