First test
[fresonbot.git] / Fresonbot.py
1 import Adafruit_BBIO.PWM as PWM
2 import Adafruit_BBIO.GPIO as GPIO
3 import Adafruit_BBIO.ADC as ADC
4 import time
5 import subprocess
6 import math
7
8 import pinout 
9
10
11 class Fresonbot(object):
12
13     # State PWM -- (LEFT, RIGHT)
14     pwm = [0, 0]
15
16     # Constraints
17     pwmLimits = [-100, 100]  # [min, max]
18
19     ticksPerTurn = 12 *100 # 12 counts per revolution * 100:1 reduction gearbox
20     WheelRadius = 16 # In mm
21     WheelDistance = 89 # In mm
22
23     duty_min = 3
24     duty_max = 14
25     duty_span = duty_max - duty_min
26
27
28     def __init__(self):
29
30         subprocess.call("bashScripts/enable_encoder_slots.sh")
31
32         GPIO.setup(pinout.PinMotorLeftPhase, GPIO.OUT)
33         GPIO.setup(pinout.PinMotorRightPhase, GPIO.OUT)
34         GPIO.output(pinout.PinMotorLeftPhase, 0)
35         GPIO.output(pinout.PinMotorRightPhase, 0)
36         PWM.start(pinout.PinMotorLeftPwm,0)
37         PWM.start(pinout.PinMotorRightPwm,0)
38
39         PWM.start(pinout.servo_IR, self.duty_span * 0.5, 60.0)
40         duty = ((90.0 / 180) * self.duty_span + self.duty_min)
41         PWM.set_duty_cycle(pinout.servo_IR, duty)
42
43         self.x = 0.0
44         self.y = 0.0
45         self.distance = 0.0
46         self.heading = 0.0
47         (TicksLeft, TicksRight) = self.getTicks();
48         self.StartTicksLeft = TicksLeft 
49         self.StartTicksRight = TicksRight 
50         ADC.setup()
51
52     def motion(self,VelLeft,VelRight):
53         AbsVelLeft = abs(VelLeft)
54         AbsVelRight = abs(VelRight)
55         if (VelLeft < 0):
56             PhaseLeft = 1
57         else:
58             PhaseLeft = 0
59         if (VelRight < 0):
60             PhaseRight = 1
61         else:
62             PhaseRight = 0
63         if (AbsVelLeft > 100):
64             AbsVelLeft = 100
65         if (AbsVelRight > 100):
66             AbsVelRight = 100
67
68         GPIO.output(pinout.PinMotorLeftPhase, PhaseLeft)
69         GPIO.output(pinout.PinMotorRightPhase, PhaseRight)
70         PWM.set_duty_cycle(pinout.PinMotorLeftPwm,AbsVelLeft)
71         PWM.set_duty_cycle(pinout.PinMotorRightPwm,AbsVelRight)
72         return 
73
74
75     def getTicks(self):
76         global TicksLeft
77         global TicksRight
78
79         fTicksLeft = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
80         fTicksRight = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
81         foTicksLeft = open(fTicksLeft, "r")
82         foTicksRight = open(fTicksRight, "r")
83
84         TicksLeft = foTicksLeft.read()
85         TicksLeft = int(TicksLeft.split('\n', 1)[0])
86         TicksRight = foTicksRight.read()
87         TicksRight = int(TicksRight.split('\n', 1)[0])
88
89         foTicksLeft.close()
90         foTicksRight.close()
91
92         return TicksLeft, TicksRight
93
94     def getPosition(self):
95
96         (TicksLeft, TicksRight) = self.getTicks()
97         EndTicksLeft = TicksLeft
98         EndTicksRight = TicksRight
99
100         IncTicksLeft = EndTicksLeft - self.StartTicksLeft
101         IncTicksRight = EndTicksRight - self.StartTicksRight
102
103         distanceLeft = 2 * math.pi * self.WheelRadius * (float(IncTicksLeft) / self.ticksPerTurn)
104         distanceRight = 2 * math.pi * self.WheelRadius * (float(IncTicksRight) / self.ticksPerTurn)
105
106         newdistance = (distanceLeft + distanceRight) / 2
107         self.distance += newdistance
108
109         self.heading += (distanceLeft - distanceRight) / self.WheelDistance
110         self.x += newdistance * math.cos(self.heading)
111         self.y += newdistance * math.sin(self.heading)
112         self.headingDec = math.degrees(self.heading)
113
114         self.StartTicksLeft = EndTicksLeft
115         self.StartTicksRight = EndTicksRight
116
117         return (self.x, self.y, self.heading,self.distance)
118
119
120     def stop(self):
121         self.motion(0,0);
122         return
123     def readIR(self):
124         voltage = ADC.read(pinout.PinIRFront)
125 #        return value1 * 1.8
126         return 3.07427335017539*voltage**-1.18207892010248
127
128     def markDetection(self):
129         value1 = ADC.read(pinout.PinIRRightB)
130         return value1 * 1.8
131
132     def moveServo(self, angle):
133         duty = ((angle / 180.0) * self.duty_span + self.duty_min)
134         PWM.set_duty_cycle(pinout.servo_IR, duty)
135         return