First test
authorJordi <jordiguerrero@gmail.com>
Thu, 4 Feb 2016 14:35:00 +0000 (14:35 +0000)
committerJordi <jordiguerrero@gmail.com>
Thu, 4 Feb 2016 14:35:00 +0000 (14:35 +0000)
Fresonbot.py [new file with mode: 0644]
Fresonbot.pyc [new file with mode: 0644]
bashScripts/enable_encoder_slots.sh [new file with mode: 0755]
bashScripts/test_encoders.sh [new file with mode: 0755]
pinout.py [new file with mode: 0644]
pinout.pyc [new file with mode: 0644]
test_motors.py [new file with mode: 0644]

diff --git a/Fresonbot.py b/Fresonbot.py
new file mode 100644 (file)
index 0000000..896acf3
--- /dev/null
@@ -0,0 +1,135 @@
+import Adafruit_BBIO.PWM as PWM
+import Adafruit_BBIO.GPIO as GPIO
+import Adafruit_BBIO.ADC as ADC
+import time
+import subprocess
+import math
+
+import pinout 
+
+
+class Fresonbot(object):
+
+    # State PWM -- (LEFT, RIGHT)
+    pwm = [0, 0]
+
+    # Constraints
+    pwmLimits = [-100, 100]  # [min, max]
+
+    ticksPerTurn = 12 *100 # 12 counts per revolution * 100:1 reduction gearbox
+    WheelRadius = 16 # In mm
+    WheelDistance = 89 # In mm
+
+    duty_min = 3
+    duty_max = 14
+    duty_span = duty_max - duty_min
+
+
+    def __init__(self):
+
+        subprocess.call("bashScripts/enable_encoder_slots.sh")
+
+        GPIO.setup(pinout.PinMotorLeftPhase, GPIO.OUT)
+        GPIO.setup(pinout.PinMotorRightPhase, GPIO.OUT)
+        GPIO.output(pinout.PinMotorLeftPhase, 0)
+        GPIO.output(pinout.PinMotorRightPhase, 0)
+        PWM.start(pinout.PinMotorLeftPwm,0)
+        PWM.start(pinout.PinMotorRightPwm,0)
+
+        PWM.start(pinout.servo_IR, self.duty_span * 0.5, 60.0)
+        duty = ((90.0 / 180) * self.duty_span + self.duty_min)
+        PWM.set_duty_cycle(pinout.servo_IR, duty)
+
+        self.x = 0.0
+        self.y = 0.0
+        self.distance = 0.0
+        self.heading = 0.0
+        (TicksLeft, TicksRight) = self.getTicks();
+        self.StartTicksLeft = TicksLeft 
+        self.StartTicksRight = TicksRight 
+        ADC.setup()
+
+    def motion(self,VelLeft,VelRight):
+        AbsVelLeft = abs(VelLeft)
+        AbsVelRight = abs(VelRight)
+        if (VelLeft < 0):
+            PhaseLeft = 1
+        else:
+            PhaseLeft = 0
+        if (VelRight < 0):
+            PhaseRight = 1
+        else:
+            PhaseRight = 0
+        if (AbsVelLeft > 100):
+            AbsVelLeft = 100
+        if (AbsVelRight > 100):
+            AbsVelRight = 100
+
+        GPIO.output(pinout.PinMotorLeftPhase, PhaseLeft)
+        GPIO.output(pinout.PinMotorRightPhase, PhaseRight)
+        PWM.set_duty_cycle(pinout.PinMotorLeftPwm,AbsVelLeft)
+        PWM.set_duty_cycle(pinout.PinMotorRightPwm,AbsVelRight)
+        return 
+
+
+    def getTicks(self):
+        global TicksLeft
+        global TicksRight
+
+        fTicksLeft = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
+        fTicksRight = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
+        foTicksLeft = open(fTicksLeft, "r")
+        foTicksRight = open(fTicksRight, "r")
+
+        TicksLeft = foTicksLeft.read()
+        TicksLeft = int(TicksLeft.split('\n', 1)[0])
+        TicksRight = foTicksRight.read()
+        TicksRight = int(TicksRight.split('\n', 1)[0])
+
+        foTicksLeft.close()
+        foTicksRight.close()
+
+        return TicksLeft, TicksRight
+
+    def getPosition(self):
+
+        (TicksLeft, TicksRight) = self.getTicks()
+        EndTicksLeft = TicksLeft
+        EndTicksRight = TicksRight
+
+        IncTicksLeft = EndTicksLeft - self.StartTicksLeft
+        IncTicksRight = EndTicksRight - self.StartTicksRight
+
+        distanceLeft = 2 * math.pi * self.WheelRadius * (float(IncTicksLeft) / self.ticksPerTurn)
+        distanceRight = 2 * math.pi * self.WheelRadius * (float(IncTicksRight) / self.ticksPerTurn)
+
+        newdistance = (distanceLeft + distanceRight) / 2
+        self.distance += newdistance
+
+        self.heading += (distanceLeft - distanceRight) / self.WheelDistance
+        self.x += newdistance * math.cos(self.heading)
+        self.y += newdistance * math.sin(self.heading)
+        self.headingDec = math.degrees(self.heading)
+
+        self.StartTicksLeft = EndTicksLeft
+        self.StartTicksRight = EndTicksRight
+
+        return (self.x, self.y, self.heading,self.distance)
+
+
+    def stop(self):
+        self.motion(0,0);
+        return
+    def readIR(self):
+        voltage = ADC.read(pinout.PinIRFront)
+#        return value1 * 1.8
+        return 3.07427335017539*voltage**-1.18207892010248
+
+    def markDetection(self):
+        value1 = ADC.read(pinout.PinIRRightB)
+        return value1 * 1.8
+
+    def moveServo(self, angle):
+        duty = ((angle / 180.0) * self.duty_span + self.duty_min)
+        PWM.set_duty_cycle(pinout.servo_IR, duty)
+        return
diff --git a/Fresonbot.pyc b/Fresonbot.pyc
new file mode 100644 (file)
index 0000000..611c67e
Binary files /dev/null and b/Fresonbot.pyc differ
diff --git a/bashScripts/enable_encoder_slots.sh b/bashScripts/enable_encoder_slots.sh
new file mode 100755 (executable)
index 0000000..04306e4
--- /dev/null
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+left_slot="bone_eqep1"
+right_slot="bone_eqep2b"
+
+if grep -q  bone_eqep1 /sys/devices/bone_capemgr.9/slots; then
+    echo "left slot already enabled"
+else
+    echo bone_eqep1 > /sys/devices/bone_capemgr.9/slots
+fi
+
+if grep -q  bone_eqep2b /sys/devices/bone_capemgr.9/slots; then
+    echo "right slot already enabled"
+else
+    echo bone_eqep2b > /sys/devices/bone_capemgr.9/slots
+fi
+
diff --git a/bashScripts/test_encoders.sh b/bashScripts/test_encoders.sh
new file mode 100755 (executable)
index 0000000..e38fbd3
--- /dev/null
@@ -0,0 +1,18 @@
+#!/bin/bash
+
+./enable_encoder_slots.sh
+
+while :
+do
+    echo -ne Left: $(cat /sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position)
+    echo -ne " Right:" $(cat /sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position)
+    
+    echo "  -- Press q to quit!"
+
+    read -t 2 -n 1 key
+
+    if [[ $key = q ]]
+    then
+        break
+    fi
+done    
diff --git a/pinout.py b/pinout.py
new file mode 100644 (file)
index 0000000..0c7593c
--- /dev/null
+++ b/pinout.py
@@ -0,0 +1,22 @@
+PinMotorLeftPwm = "P9_14"
+PinMotorRightPwm = "P9_16"
+PinMotorLeftPhase = "P9_12"
+PinMotorRightPhase = "P9_23"
+
+#     
+#     _1_
+#  3/     \0
+#  |       |
+#  |       |
+# 4|       |2
+#
+
+PinIRFront = "AIN5"
+#
+#PinIRLeftF = "AIN3"
+#PinIRRightF = "AIN0"
+#PinIRLeftB = "AIN0"
+PinIRRightB = "AIN2"
+
+
+servo_IR = "P8_13"
diff --git a/pinout.pyc b/pinout.pyc
new file mode 100644 (file)
index 0000000..bd38279
Binary files /dev/null and b/pinout.pyc differ
diff --git a/test_motors.py b/test_motors.py
new file mode 100644 (file)
index 0000000..0987bd3
--- /dev/null
@@ -0,0 +1,24 @@
+from Fresonbot import Fresonbot
+import time
+
+t = 1
+R1 = Fresonbot()
+
+print ("Drive forward for 2 seconds")
+R1.motion(50,50)
+time.sleep(t)
+
+print ("Drive backward for 2 seconds")
+R1.motion(-50,-50)
+time.sleep(t)
+
+print ("Turn left for 2 seconds")
+R1.motion(-50,50)
+time.sleep(t)
+
+print ("Turn right for 2 seconds")
+R1.motion(50,-50)
+time.sleep(t)
+
+
+R1.stop()