changes in eqep2
[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 #IR
4 import subprocess
5 import math
6
7 import pinout 
8
9
10 class Fresonbot(object):
11
12     ticksPerTurn = 12 *100 # 12 counts per revolution * 100:1 reduction gearbox
13 ## you have to take this measures accurately
14 #    WheelRadius = 31.8/2 # I took the diameter and divided by 2
15 #    WheelDistance = 88.9 # between centers
16     WheelRadius = 16 # In mm
17     WheelDistance = 89 # In mm
18
19     duty_min = 3
20     duty_max = 14
21     duty_span = duty_max - duty_min
22
23
24     def __init__(self):
25
26         subprocess.call("bashScripts/enable_encoder_slots.sh")
27
28         GPIO.setup(pinout.PinMotorLeftPhase, GPIO.OUT)
29         GPIO.setup(pinout.PinMotorRightPhase, GPIO.OUT)
30         GPIO.output(pinout.PinMotorLeftPhase, 0)
31         GPIO.output(pinout.PinMotorRightPhase, 0)
32         PWM.start(pinout.PinMotorLeftPwm,0)
33         PWM.start(pinout.PinMotorRightPwm,0)
34
35         self.x = 0.0
36         self.y = 0.0
37         self.distance = 0.0
38         self.heading = 0.0
39         (TicksLeft, TicksRight) = self.getTicks();
40         self.StartTicksLeft = TicksLeft 
41         self.StartTicksRight = TicksRight 
42 #        ADC.setup() # IR
43
44     def motion(self,VelLeft,VelRight):
45         AbsVelLeft = abs(VelLeft)
46         AbsVelRight = abs(VelRight)
47         if (VelLeft < 0):
48             PhaseLeft = 1
49         else:
50             PhaseLeft = 0
51         if (VelRight < 0):
52             PhaseRight = 1
53         else:
54             PhaseRight = 0
55         if (AbsVelLeft > 100):
56             AbsVelLeft = 100
57         if (AbsVelRight > 100):
58             AbsVelRight = 100
59
60         GPIO.output(pinout.PinMotorLeftPhase, PhaseLeft)
61         GPIO.output(pinout.PinMotorRightPhase, PhaseRight)
62         PWM.set_duty_cycle(pinout.PinMotorLeftPwm,AbsVelLeft)
63         PWM.set_duty_cycle(pinout.PinMotorRightPwm,AbsVelRight)
64         return 
65
66
67     def getTicks(self):
68         global TicksLeft
69         global TicksRight
70
71         fTicksLeft = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
72         fTicksRight = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
73         foTicksLeft = open(fTicksLeft, "r")
74         foTicksRight = open(fTicksRight, "r")
75
76         TicksLeft = foTicksLeft.read()
77         TicksLeft = int(TicksLeft.split('\n', 1)[0])
78         TicksRight = foTicksRight.read()
79         TicksRight = int(TicksRight.split('\n', 1)[0])
80
81         foTicksLeft.close()
82         foTicksRight.close()
83
84         return TicksLeft, TicksRight
85
86     def getPosition(self):
87
88         (TicksLeft, TicksRight) = self.getTicks()
89         EndTicksLeft = TicksLeft
90         EndTicksRight = TicksRight
91
92         IncTicksLeft = EndTicksLeft - self.StartTicksLeft
93         IncTicksRight = EndTicksRight - self.StartTicksRight
94
95         distanceLeft = 2 * math.pi * self.WheelRadius * (float(IncTicksLeft) / self.ticksPerTurn)
96         distanceRight = 2 * math.pi * self.WheelRadius * (float(IncTicksRight) / self.ticksPerTurn)
97
98         newdistance = (distanceLeft + distanceRight) / 2
99         self.distance += newdistance
100
101         self.heading += (distanceLeft - distanceRight) / self.WheelDistance
102         self.x += newdistance * math.cos(self.heading)
103         self.y += newdistance * math.sin(self.heading)
104         self.headingDec = math.degrees(self.heading)
105
106         self.StartTicksLeft = EndTicksLeft
107         self.StartTicksRight = EndTicksRight
108
109         return (self.x, self.y, self.heading,self.distance)
110
111
112     def stop(self):
113         self.motion(0,0);
114
115         return
116
117 #    def readIR(self):
118 #        voltage = ADC.read(pinout.PinIRFront)
119 ##        return value1 * 1.8
120 #        return 3.07427335017539*voltage**-1.18207892010248
121
122