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