/
pi2kf.py
251 lines (215 loc) · 7.84 KB
/
pi2kf.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
#!/usr/bin/python
#
# Python Module to externalise all pi2kf specific hardware
#
# Created by Gareth Davies and Zachary Igielman, May 2014
# Updated June 2014 to include pi2kf-Lite within same framework
# Copyright 4tronix
#
# This code is in the public domain and may be freely copied and used
# No warranty is provided or implied
#
#======================================================================
from Adafruit_I2C import Adafruit_I2C
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
import time
import atexit
mh = Adafruit_MotorHAT(addr=0x60)
# auto-disable motors on shutdown!
def turnOffMotors():
mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
atexit.register(turnOffMotors)
mL = mh.getMotor(1)
mR = mh.getMotor(2)
#======================================================================
# General Functions
# (Both versions)
#
# init(). Initialises GPIO pins, switches motors and LEDs Off, etc
# cleanup(). Sets all motors and LEDs off and sets GPIO to standard values
# version(). Returns 1 for Full pi2kf, and 2 for pi2kf-Lite. Invalid until after init() has been called
#======================================================================
#======================================================================
# Motor Functions
# (Both Versions)
#
# stop(): Stops both motors
# go(leftSpeed, rightSpeed): controls motors in both directions independently using different positive/negative speeds. -100<= leftSpeed,rightSpeed <= 100
#======================================================================
#======================================================================
# UltraSonic Functions
# (Both Versions)
#
# getDistance(). Returns the distance in cm to the nearest reflecting object. 0 == no object
#======================================================================
#======================================================================
# Servo Functions
#
# setServo(Servo, degrees). Sets the servo to position in degrees -90 to +90
#======================================================================
#======================================================================
# Switch Functions
#
# getSwitch(). Returns the value of the tact switch: True==pressed
#======================================================================
# Import all necessary libraries
#import RPi.GPIO as GPIO
import sys, threading, time, os
from Adafruit_PWM_Servo_Driver import PWM
# Define Type of pi2kf
PI2KF = 1
TAVBOT = 2
# Define Sonar Pin (same pin for both Ping and Echo
sonar = 8
# Define pins for switch (different on each version)
switch = 16
Lswitch = 23
def setLed(on, brightness=255):
if on:
mh.getMotor(3).setSpeed(brightness)
mh.getMotor(3).run(Adafruit_MotorHAT.FORWARD)
else:
mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
#======================================================================
# General Functions
#
# init(). Initialises GPIO pins, switches motors and LEDs Off, etc
def init():
global p, q, a, b, pwm, pcfADC, PGType
PGType = PI2KF
# Initialise the PCA9685 PWM device using the default address
try:
pwm = PWM(0x60, debug = False)
pwm.setPWMFreq(60)
except:
print "can't set PWM frequency!"
mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
# cleanup(). Sets all motors and LEDs off and sets GPIO to standard values
def cleanup():
stop()
time.sleep(1)
#GPIO.cleanup()
# version(). Returns 1 pi2kf, 2 for tavbot
def version():
return PGType
# End of General Functions
#======================================================================
#======================================================================
# Motor Functions
# (both versions)
#
# stop(): Stops both motors
def stop():
mL.run(Adafruit_MotorHAT.RELEASE)
mR.run(Adafruit_MotorHAT.RELEASE)
# go(leftSpeed, rightSpeed): controls motors in both directions independently using different positive/negative speeds. -100<= leftSpeed,rightSpeed <= 100
def go(leftSpeed, rightSpeed):
global pwm
l = min(abs(leftSpeed), abs(rightSpeed))
freq = 1940 * ((l/100.0) ** 3) + 60
pwm.setPWMFreq(freq)
mL.setSpeed(int(abs(leftSpeed*2.53)))
mR.setSpeed(int(abs(rightSpeed*2.53)))
#print("FREQ: {:5.2f}".format(freq))
#print(int(abs(leftSpeed*2.53)))
#print(int(abs(rightSpeed*2.53)))
mL.run(Adafruit_MotorHAT.FORWARD if leftSpeed > 0 else Adafruit_MotorHAT.BACKWARD)
mR.run(Adafruit_MotorHAT.FORWARD if rightSpeed > 0 else Adafruit_MotorHAT.BACKWARD)
if leftSpeed == 0:
mL.run(Adafruit_MotorHAT.RELEASE)
if rightSpeed == 0:
mR.run(Adafruit_MotorHAT.RELEASE)
# End of Motor Functions
#======================================================================
#======================================================================
# UltraSonic Functions
#
# getDistance(). Returns the distance in cm to the nearest reflecting object. 0 == no object
# (Both versions)
#
def getDistance():
GPIO.setup(sonar, GPIO.OUT)
# Send 10us pulse to trigger
GPIO.output(sonar, True)
time.sleep(0.00001)
GPIO.output(sonar, False)
start = time.time()
count=time.time()
GPIO.setup(sonar,GPIO.IN)
while GPIO.input(sonar)==0 and time.time()-count<0.1:
start = time.time()
count=time.time()
stop=count
while GPIO.input(sonar)==1 and time.time()-count<0.1:
stop = time.time()
# Calculate pulse length
elapsed = stop-start
# Distance pulse travelled in that time is time
# multiplied by the speed of sound 34000(cm/s) divided by 2
distance = elapsed * 17000
return distance
# End of UltraSonic Functions
#======================================================================
#======================================================================
# Switch Functions
#
# getSwitch(). Returns the value of the tact switch: True==pressed
def getSwitch():
if PGType == 1:
val = GPIO.input(switch)
else:
val = GPIO.input(Lswitch)
return (val == 0)
#
# End of switch functions
#======================================================================
#======================================================================
# Servo Functions
# needs servoblaster started by init:
# https://github.com/richardghirst/PiBits/tree/master/ServoBlaster/user
# servod --pcm --idle-timeout=400 --p1pins="11,12"
def setServo(pin, degrees):
#pinString = "echo " + str(pin) + "=" + str(((degrees+90)*0.001)) + " > /dev/pi-blaster"
pinString = str(pin) + "=" + str(50+ ((90 - degrees) * 200 / 180))
#print (pinString)
with open('/dev/servoblaster', 'w') as servoblaster:
servoblaster.write(pinString + "\n")
class FuelGauge:
def __init__(self, address):
self.i2c = Adafruit_I2C(address)
self.i2c.write16(0x06, 0x0040)
self.percent = 0.0
self.voltage = 0.0
def update(self):
fg_ad = self.i2c.readU16(0x2)
fg_ad = fg_ad >> 4
fg_ad = self.i2c.reverseByteOrder(fg_ad)
if fg_ad:
self.voltage = fg_ad*0.00125;
fg_pc1 = self.i2c.readU8(0x4)
fg_pc2 = self.i2c.readU8(0x5)
self.percent = fg_pc1 + fg_pc2/256.0;
class BlinkThread(threading.Thread):
def __init__(self):
super(BlinkThread, self).__init__()
self.b=127
self.speed=0.5
self.blink=True
def run(self):
while self.blink:
setLed(True, self.b)
time.sleep(self.speed)
setLed(False)
time.sleep(self.speed)
def setBrightness(self,br):
self.b = br
def setBlinkSpeed(self,sp):
self.speed = sp
def quit(self):
self.blink=False