Skip to content

Commit 06b5465

Browse files
freaf87freaf87
authored
Merge pull request #4 from freaf87/master
first implementation of PDC.py
2 parents a1f1c01 + e928f7b commit 06b5465

7 files changed

Lines changed: 304 additions & 9 deletions

File tree

‎02_Code/PDC_python/applications/PDC.py‎

Whitespace-only changes.

‎02_Code/PDC_python/applications/__init__.py‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#!/usr/bin/env python
2+
# -*- coding: utf-8 -*-
3+
# This file is part of FSE 2019.
4+
#
5+
# FSE 2019 is free software: you can redistribute it and/or modify
6+
# it under the terms of the GNU General Public License as published by
7+
# the Free Software Foundation, either version 3 of the License, or
8+
# (at your option) any later version.
9+
#
10+
# FSE 2019 is distributed in the hope that it will be useful,
11+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
12+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13+
# GNU General Public License for more details.
14+
#
15+
# You should have received a copy of the GNU General Public License
16+
# along with FSE 2019. If not, see <http://www.gnu.org/licenses/>.
17+
18+
"""Hardware drivers for FSEDevBoard."""

‎02_Code/PDC_python/drivers/PDC.py‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
2+
#!/usr/bin/env python
3+
# -*- coding: utf-8 -*-
4+
# This file is part of FSE 2019.
5+
#
6+
# FSE 2019 is free software: you can redistribute it and/or modify
7+
# it under the terms of the GNU General Public License as published by
8+
# the Free Software Foundation, either version 3 of the License, or
9+
# (at your option) any later version.
10+
#
11+
# FSE 2019 is distributed in the hope that it will be useful,
12+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
13+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14+
# GNU General Public License for more details.
15+
#
16+
# You should have received a copy of the GNU General Public License
17+
# along with FSE 2019. If not, see <http://www.gnu.org/licenses/>.
18+
19+
"""Robot interface combining ultrasonic, LED and motor drivers."""
20+
21+
import time
22+
from buzzer import BUZZER
23+
from rgbLed import RGBLED
24+
from hcsr04 import HCSR04, UltrasonicTimeoutError
25+
26+
class PDC(object):
27+
""" PDC class """
28+
def __init__(self):
29+
self.ultrasonic = HCSR04()
30+
self.rgbled = RGBLED()
31+
self.rgbled.pwmDriver.setPwmFreq(600)
32+
33+
def setPDCColor(self, color):
34+
""" Set RGB color """
35+
self.rgbled.setRChannelPWM(color[0]/255.0*100)
36+
self.rgbled.setGChannelPWM(color[1]/255.0*100)
37+
self.rgbled.setBChannelPWM(color[2]/255.0*100)
38+
39+
def __enter__(self):
40+
return self
41+
42+
def __exit__(self, *args):
43+
"""Release internally used resources."""
44+
self.ultrasonic.__exit__()
45+
self.rgbled.__exit__()
46+
47+
@property
48+
def getDistance(self):
49+
"""Return distance to nearest obstacle in cm or None."""
50+
try:
51+
distance = self.ultrasonic.average_distance
52+
except UltrasonicTimeoutError:
53+
distance = None
54+
return distance
55+
56+
def dist2color(self, dist):
57+
if dist > 100:
58+
return (0,0,0)
59+
elif dist <= 100 and dist > 25:
60+
return (0,255,0)
61+
elif dist <=25 and dist > 15:
62+
arg_r = -25.5 * dist + 637.5
63+
return (arg_r, 255,0)
64+
elif dist <=15 and dist> 5:
65+
arg_g = 25.5 * dist - 127.5
66+
return (255,arg_g,0)
67+
else:
68+
return (0,0,0)
69+
70+
@property
71+
def hex_to_rgb(hex_value):
72+
r_dec = (hex_value & 0xff0000) >> 16
73+
g_dec = (hex_value & 0x00ff00) >> 8
74+
b_dec = (hex_value & 0x0000ff)
75+
return (r_dec, g_dec, b_dec)
76+
77+
if __name__ == "__main__":
78+
with PDC() as pdc:
79+
while True:
80+
dist = pdc.getDistance
81+
if dist is not None:
82+
pdc.setPDCColor(pdc.dist2color(dist))
83+
print(dist)
84+
85+

‎02_Code/PDC_python/drivers/__init__.py‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#!/usr/bin/env python
2+
# -*- coding: utf-8 -*-
3+
# This file is part of FSE 2019.
4+
#
5+
# FSE 2019 is free software: you can redistribute it and/or modify
6+
# it under the terms of the GNU General Public License as published by
7+
# the Free Software Foundation, either version 3 of the License, or
8+
# (at your option) any later version.
9+
#
10+
# FSE 2019 is distributed in the hope that it will be useful,
11+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
12+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13+
# GNU General Public License for more details.
14+
#
15+
# You should have received a copy of the GNU General Public License
16+
# along with FSE 2019. If not, see <http://www.gnu.org/licenses/>.
17+
18+
"""Hardware drivers for FSEDevBoard."""

02_Code/PDC_python/drivers/ultrasonicRanger.py renamed to 02_Code/PDC_python/drivers/hcsr04.py

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ class UltrasonicTimeoutError(Exception):
2626
"""UltrasonicRanger does not measure response to ping."""
2727

2828

29-
class UltrasonicRanger(GPIO_Manager):
29+
class HCSR04(GPIO_Manager):
3030
"""Interface with an HCSR04 ultrasonic range sensor."""
3131

3232
_trigger_pin = 15
@@ -36,10 +36,16 @@ class UltrasonicRanger(GPIO_Manager):
3636
_average_count = 1
3737

3838
def __init__(self):
39-
super(UltrasonicRanger, self).__init__()
39+
super(HCSR04, self).__init__()
4040
wiringpi.pinMode(self._trigger_pin, wiringpi.OUTPUT)
4141
wiringpi.pinMode(self._echo_pin, wiringpi.INPUT)
4242

43+
def __enter__(self):
44+
return self
45+
46+
def __exit__(self, *args):
47+
return self
48+
4349
@property
4450
def distance(self):
4551
"""Measure time between sent impulse and measured reflectance."""
@@ -84,7 +90,7 @@ def average_distance(self):
8490

8591

8692
if __name__ == "__main__":
87-
with UltrasonicRanger() as ultrasonic:
93+
with HCSR04() as ultrasonic:
8894
while True:
8995
try:
9096
dis = ultrasonic.average_distance

‎02_Code/PDC_python/drivers/rgbLed.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,12 @@
1515
# You should have received a copy of the GNU General Public License
1616

1717

18-
from PCA9685 import PWM_Driver
18+
from pca9685 import PWM_Driver
1919
import os
2020
import time
2121
import sys
2222

23-
class RGB_LED():
23+
class RGBLED():
2424
""" Interface class for FSEDevBoard onboard LED """
2525
def __init__(self):
2626
self._RChannel = 13
@@ -46,12 +46,12 @@ def __enter__(self):
4646
return self
4747

4848
def __exit__(self, *args):
49-
rgbLed.setRChannelPWM(0)
50-
rgbLed.setGChannelPWM(0)
51-
rgbLed.setBChannelPWM(0)
49+
self.setRChannelPWM(0)
50+
self.setGChannelPWM(0)
51+
self.setBChannelPWM(0)
5252

5353
if __name__ == "__main__":
54-
with RGB_LED() as rgbLed:
54+
with RGBLED() as rgbLed:
5555
try:
5656
rgbLed.pwmDriver.setPwmFreq(600)
5757
while True:

‎02_Code/PDC_python/drivers/tb6612fng.py‎

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
#!/usr/bin/env python
2+
# -*- coding: utf-8 -*-
3+
# This file is part of FSE 2019.
4+
#
5+
# FSE 2019 is free software: you can redistribute it and/or modify
6+
# it under the terms of the GNU General Public License as published by
7+
# the Free Software Foundation, either version 3 of the License, or
8+
# (at your option) any later version.
9+
#
10+
# FSE 2019 is distributed in the hope that it will be useful,
11+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
12+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13+
# GNU General Public License for more details.
14+
#
15+
# You should have received a copy of the GNU General Public License
16+
# along with FSE 2019. If not, see <http://www.gnu.org/licenses/>.
17+
18+
"""Driver for 2 TB6612FNG DC motors mounted to a single chassis."""
19+
20+
import os
21+
import time
22+
23+
import wiringpi
24+
25+
from gpio_manager import GPIO_Manager
26+
27+
28+
class MotorDriver(GPIO_Manager):
29+
"""Interface with 2 TB6612FNG DC Motor drivers."""
30+
# Configure motor 1
31+
_m1_dir1_pin = 6
32+
_m1_dir2_pin = 12
33+
_m1_pwm_pin_annex = 5 # solve error mapping (Vers.01)
34+
_m1_pwm_pin = 18
35+
36+
# Configure motor 2
37+
_m2_dir1_pin = 19
38+
_m2_dir2_pin = 16
39+
_m2_pwm_pin_annex = 26 # solve error mapping (Vers.01)
40+
_m2_pwm_pin = 13
41+
42+
_standby_pin = 20
43+
44+
PWM_OUTPUTS = [_m1_pwm_pin, _m2_pwm_pin]
45+
46+
INPUT_PINS = [_m1_pwm_pin_annex, _m2_pwm_pin_annex]
47+
48+
OUTPUT_PINS = [_m1_dir1_pin, _m1_dir2_pin,
49+
_m2_dir1_pin, _m2_dir2_pin,
50+
_standby_pin]
51+
_pins = PWM_OUTPUTS + INPUT_PINS + OUTPUT_PINS
52+
53+
@staticmethod
54+
def to_dc(dc):
55+
return (1023 * dc) / 100
56+
57+
def __init__(self):
58+
if os.getuid():
59+
raise RuntimeError("MotorDriver can only be used by sudoer.")
60+
super(MotorDriver, self).__init__()
61+
for pin in self.OUTPUT_PINS:
62+
wiringpi.pinMode(pin, wiringpi.OUTPUT)
63+
64+
for pin in self.PWM_OUTPUTS:
65+
wiringpi.pinMode(pin, wiringpi.PWM_OUTPUT)
66+
67+
for pin in self.INPUT_PINS:
68+
wiringpi.pinMode(pin, wiringpi.INPUT)
69+
# Make sure all annex_pin are set LOW
70+
wiringpi.pullUpDnControl(pin, wiringpi.PUD_DOWN)
71+
72+
def right_forward(self):
73+
"""Drive right motor forward."""
74+
wiringpi.digitalWrite(self._m1_dir1_pin, wiringpi.LOW)
75+
wiringpi.digitalWrite(self._m1_dir2_pin, wiringpi.HIGH)
76+
77+
def right_back(self):
78+
"""Drive right motor back."""
79+
wiringpi.digitalWrite(self._m1_dir1_pin, wiringpi.HIGH)
80+
wiringpi.digitalWrite(self._m1_dir2_pin, wiringpi.LOW)
81+
82+
def left_forward(self):
83+
"""Drive left motor forward."""
84+
wiringpi.digitalWrite(self._m2_dir1_pin, wiringpi.HIGH)
85+
wiringpi.digitalWrite(self._m2_dir2_pin, wiringpi.LOW)
86+
87+
def left_back(self):
88+
"""Drive left motor back."""
89+
wiringpi.digitalWrite(self._m2_dir1_pin, wiringpi.LOW)
90+
wiringpi.digitalWrite(self._m2_dir2_pin, wiringpi.HIGH)
91+
92+
def forward(self, duty_cycle=25):
93+
"""Drive chassis forward."""
94+
self.right_forward()
95+
self.left_forward()
96+
wiringpi.pwmWrite(self._m1_pwm_pin, self.to_dc(duty_cycle))
97+
wiringpi.pwmWrite(self._m2_pwm_pin, self.to_dc(duty_cycle))
98+
wiringpi.digitalWrite(self._standby_pin, wiringpi.HIGH)
99+
100+
def reverse(self, duty_cycle=25):
101+
"""Drive chassis back."""
102+
self.right_back()
103+
self.left_back()
104+
wiringpi.pwmWrite(self._m1_pwm_pin, self.to_dc(duty_cycle))
105+
wiringpi.pwmWrite(self._m2_pwm_pin, self.to_dc(duty_cycle))
106+
wiringpi.digitalWrite(self._standby_pin, wiringpi.HIGH)
107+
108+
def right(self, duty_cycle=25):
109+
"""Turn chassis clockwise."""
110+
self.right_forward()
111+
self.left_back()
112+
wiringpi.pwmWrite(self._m1_pwm_pin, self.to_dc(duty_cycle))
113+
wiringpi.pwmWrite(self._m2_pwm_pin, self.to_dc(duty_cycle))
114+
wiringpi.digitalWrite(self._standby_pin, wiringpi.HIGH)
115+
116+
def left(self, duty_cycle=25):
117+
"""Turn chassis counterclockwise."""
118+
self.right_back()
119+
self.left_forward()
120+
wiringpi.pwmWrite(self._m1_pwm_pin, self.to_dc(duty_cycle))
121+
wiringpi.pwmWrite(self._m2_pwm_pin, self.to_dc(duty_cycle))
122+
wiringpi.digitalWrite(self._standby_pin, wiringpi.HIGH)
123+
124+
def stop(self):
125+
"""Stop chassis."""
126+
for pin in self.PWM_OUTPUTS:
127+
wiringpi.pwmWrite(pin, 0)
128+
for pin in self.OUTPUT_PINS:
129+
wiringpi.digitalWrite(pin, wiringpi.LOW)
130+
131+
def __exit__(self, *args):
132+
wiringpi.pwmWrite(self._m1_pwm_pin, 0)
133+
wiringpi.pwmWrite(self._m2_pwm_pin, 0)
134+
super(MotorDriver, self).__exit__()
135+
136+
137+
if __name__ == '__main__':
138+
with MotorDriver() as tb6612fng:
139+
while True:
140+
print "forward"
141+
tb6612fng.forward()
142+
time.sleep(3)
143+
144+
tb6612fng.stop()
145+
time.sleep(3)
146+
147+
print "reverse"
148+
tb6612fng.reverse()
149+
time.sleep(3)
150+
151+
tb6612fng.stop()
152+
time.sleep(3)
153+
154+
print "left"
155+
tb6612fng.left()
156+
time.sleep(3)
157+
158+
tb6612fng.stop()
159+
time.sleep(3)
160+
161+
print "right"
162+
tb6612fng.right()
163+
time.sleep(3)
164+
165+
tb6612fng.stop()
166+
time.sleep(3)
167+
168+
print "Done!"

0 commit comments

Comments
 (0)