etch-a-sketch demo concept using DC motors and eQEP for position

pid_controller/etchasketch.py

@@ -0,0 +1,150 @@
+# USAGE: PYTHONPATH="${PYTHON_PATH}:/root/beaglebot/encoders/api/python/" python ./etchasketch.py
+# EXIT: to kill run "pkill python" from another terminal
+# REQUIRED: echo bone_eqep0 > /sys/devices/bone_capemgr.*/slots
+# REQUIRED: echo bone_eqep2alt > /sys/devices/bone_capemgr.*/slot
+# INFO: x axis is 2800 position long
+# INFO: y axis is 2100 position long
+
+from eqep import eQEP
+#from pid import PID
+
+import Adafruit_BBIO.GPIO as gpio
+import Adafruit_BBIO.PWM as pwm
+
+import threading
+
+# Motor controller object
+class controller(threading.Thread):
+    # Instantiate the motor controller
+    def __init__(self, x_pwm_name, x_dir_a, x_dir_b, x_eqep, y_pwm_name, y_dir_a, y_dir_b, y_eqep):
+        # Initialize the thread base
+        super(controller, self).__init__()
+
+        # Create stop event object
+        self.stop_event = threading.Event()
+        self.stop_confirm = threading.Event()
+
+        # Store parameters
+        #self.xmax = 2500
+        #self.ymax = 2000
+        # making xmax same as ymax to keep diagonal line straight
+        self.xmax = 1000
+        self.ymax = 1000
+
+        self.x_pwm = x_pwm_name
+        self.x_dir_a = x_dir_a
+        self.x_dir_b = x_dir_b
+
+        self.y_pwm = y_pwm_name
+        self.y_dir_a = y_dir_a
+        self.y_dir_b = y_dir_b
+
+        # Create a PID controller
+        # not used for etch-a-sketch v1
+        # self.pid = PID(0.01, 0.01, 0.0, -100.0, 100.0, 0.0)
+
+        # Load the eQEP interface
+        #self.eqep = eQEP(eqep, eQEP.MODE_RELATIVE)
+	# ABSOLUTE needed for etch-a-sketch positioning system
+        self.x_eqep = eQEP(x_eqep, eQEP.MODE_ABSOLUTE)
+        self.y_eqep = eQEP(y_eqep, eQEP.MODE_ABSOLUTE)
+        self.x_eqep.set_period(10000000)
+        self.y_eqep.set_period(10000000)
+
+        # Setup the GPIO
+        gpio.setup(self.x_dir_a, gpio.OUT)
+        gpio.setup(self.x_dir_b, gpio.OUT)
+        gpio.output(self.x_dir_a, gpio.LOW)
+        gpio.output(self.x_dir_b, gpio.LOW)
+
+        gpio.setup(self.y_dir_a, gpio.OUT)
+        gpio.setup(self.y_dir_b, gpio.OUT)
+        gpio.output(self.y_dir_a, gpio.LOW)
+        gpio.output(self.y_dir_b, gpio.LOW)
+
+        # Setup the PWM
+        pwm.start(self.x_pwm, 100.0)
+        pwm.start(self.y_pwm, 100.0)
+
+    # Process of the running thing
+    def run(self):
+        FORWARD = -1 
+        REVERSE = 1
+	x_direction = FORWARD
+	y_direction = FORWARD
+        x_position = 0
+        y_position = 0
+	i = 0
+
+        while (self.stop_event.is_set() == False):
+            x_position = float(self.x_eqep.poll_position())
+            y_position = float(self.y_eqep.poll_position())
+            # not used for  etch-a-sketch v1
+            #output = self.pid.calculate(position)
+
+            if x_position >= self.xmax:
+               x_direction = REVERSE
+            if x_position <= 0:
+               x_direction = FORWARD
+
+            if y_position >= self.ymax:
+               y_direction = REVERSE
+            if y_position <= 0:
+               y_direction = FORWARD
+
+            # Calculate the output duty cycle 
+            # TODO: does this make sense?
+            duty = 50.0 
+
+            # Setup the IO lines correctly
+            # reverse on x axis motor
+            if(x_direction > 0.0):
+                gpio.output(self.x_dir_a, gpio.HIGH)
+                gpio.output(self.x_dir_b, gpio.LOW)
+            # forward on x axis motor
+            else:
+                gpio.output(self.x_dir_a, gpio.LOW)
+                gpio.output(self.x_dir_b, gpio.HIGH)
+
+            # Setup the IO lines correctly
+            # reverse on y axis motor
+            if(y_direction > 0.0):
+                gpio.output(self.y_dir_a, gpio.HIGH)
+                gpio.output(self.y_dir_b, gpio.LOW)
+            # forward on y axis motor
+            else:
+                gpio.output(self.y_dir_a, gpio.LOW)
+                gpio.output(self.y_dir_b, gpio.HIGH)
+
+            # Set the pwm
+            # do not need to change duty cycle if no PID loop
+            #pwm.set_duty_cycle(self.x_pwm, duty)
+            #pwm.set_duty_cycle(self.y_pwm, duty)
+
+            # Log 
+            i += 1
+            if (i % 50) == 0:
+                print "x=" + self.x_pwm + "\tx_position=" + str(x_position) + "\tx_direction=" + str(x_direction) + "\ty=" + self.y_pwm + "\ty_position=" + str(y_position) + "\ty_direction=" + str(y_direction) + "\tduty=" + str(duty)
+        # Confirm stop
+        self.stop_confirm.set()
+
+    # Stop the motor
+    def stop(self):
+        self.stop_event.set()
+        self.stop_confirm.wait()
+        self.stop_event.clear()
+        self.stop_confirm.clear()
+        gpio.output(self.x_dir_a, gpio.LOW)
+        gpio.output(self.x_dir_b, gpio.LOW)
+        gpio.output(self.y_dir_a, gpio.LOW)
+        gpio.output(self.y_dir_b, gpio.LOW)
+        #pwm.set_duty_cycle(self.x_pwm, 100.0)
+        #pwm.set_duty_cycle(self.y_pwm, 100.0)
+
+
+# Program starts here
+if __name__ == '__main__':
+    # args for controller(): x_pwm, x_dir_a, x_dir_b, x_eqep, y_pwm, y_dir_a, y_dir_b, y_eqep
+    myController = controller("P9_14","P8_7", "P8_8", "/sys/devices/ocp.3/48300000.epwmss/48300180.eqep", "P9_16", "P8_9", "P8_10", "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep")
+    myController.start()
+

pid_controller/run_etchasketch.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+echo bone_eqep0 > /sys/devices/bone_capemgr.*/slots
+echo bone_eqep2alt > /sys/devices/bone_capemgr.*/slots
+PYTHONPATH="${PYTHON_PATH}:/root/beaglebot/encoders/api/python/" python ./etchasketch.py