Skip to content

Commit 9c99858

Browse files
lukaslukas
committed
Added servo arm support
1 parent 002f135 commit 9c99858

File tree

5 files changed

+362
-2
lines changed

5 files changed

+362
-2
lines changed

‎Adafruit_I2C.py

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
#!/usr/bin/python
2+
import re
3+
import smbus
4+
5+
# ===========================================================================
6+
# Adafruit_I2C Class
7+
# ===========================================================================
8+
9+
class Adafruit_I2C(object):
10+
11+
@staticmethod
12+
def getPiRevision():
13+
"Gets the version number of the Raspberry Pi board"
14+
# Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
15+
try:
16+
with open('/proc/cpuinfo', 'r') as infile:
17+
for line in infile:
18+
# Match a line of the form "Revision : 0002" while ignoring extra
19+
# info in front of the revsion (like 1000 when the Pi was over-volted).
20+
match = re.match('Revision\s+:\s+.*(\w{4})$', line)
21+
if match and match.group(1) in ['0000', '0002', '0003']:
22+
# Return revision 1 if revision ends with 0000, 0002 or 0003.
23+
return 1
24+
elif match:
25+
# Assume revision 2 if revision ends with any other 4 chars.
26+
return 2
27+
# Couldn't find the revision, assume revision 0 like older code for compatibility.
28+
return 0
29+
except:
30+
return 0
31+
32+
@staticmethod
33+
def getPiI2CBusNumber():
34+
# Gets the I2C bus number /dev/i2c#
35+
return 1 if Adafruit_I2C.getPiRevision() > 1 else 0
36+
37+
def __init__(self, address, busnum=-1, debug=False):
38+
self.address = address
39+
# By default, the correct I2C bus is auto-detected using /proc/cpuinfo
40+
# Alternatively, you can hard-code the bus version below:
41+
# self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's)
42+
# self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's)
43+
self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber())
44+
self.debug = debug
45+
46+
def reverseByteOrder(self, data):
47+
"Reverses the byte order of an int (16-bit) or long (32-bit) value"
48+
# Courtesy Vishal Sapre
49+
byteCount = len(hex(data)[2:].replace('L','')[::2])
50+
val = 0
51+
for i in range(byteCount):
52+
val = (val << 8) | (data & 0xff)
53+
data >>= 8
54+
return val
55+
56+
def errMsg(self):
57+
print "Error accessing 0x%02X: Check your I2C address" % self.address
58+
return -1
59+
60+
def write8(self, reg, value):
61+
"Writes an 8-bit value to the specified register/address"
62+
try:
63+
self.bus.write_byte_data(self.address, reg, value)
64+
if self.debug:
65+
print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg)
66+
except IOError, err:
67+
return self.errMsg()
68+
69+
def write16(self, reg, value):
70+
"Writes a 16-bit value to the specified register/address pair"
71+
try:
72+
self.bus.write_word_data(self.address, reg, value)
73+
if self.debug:
74+
print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" %
75+
(value, reg, reg+1))
76+
except IOError, err:
77+
return self.errMsg()
78+
79+
def writeRaw8(self, value):
80+
"Writes an 8-bit value on the bus"
81+
try:
82+
self.bus.write_byte(self.address, value)
83+
if self.debug:
84+
print "I2C: Wrote 0x%02X" % value
85+
except IOError, err:
86+
return self.errMsg()
87+
88+
def writeList(self, reg, list):
89+
"Writes an array of bytes using I2C format"
90+
try:
91+
if self.debug:
92+
print "I2C: Writing list to register 0x%02X:" % reg
93+
print list
94+
self.bus.write_i2c_block_data(self.address, reg, list)
95+
except IOError, err:
96+
return self.errMsg()
97+
98+
def readList(self, reg, length):
99+
"Read a list of bytes from the I2C device"
100+
try:
101+
results = self.bus.read_i2c_block_data(self.address, reg, length)
102+
if self.debug:
103+
print ("I2C: Device 0x%02X returned the following from reg 0x%02X" %
104+
(self.address, reg))
105+
print results
106+
return results
107+
except IOError, err:
108+
return self.errMsg()
109+
110+
def readU8(self, reg):
111+
"Read an unsigned byte from the I2C device"
112+
try:
113+
result = self.bus.read_byte_data(self.address, reg)
114+
if self.debug:
115+
print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
116+
(self.address, result & 0xFF, reg))
117+
return result
118+
except IOError, err:
119+
return self.errMsg()
120+
121+
def readS8(self, reg):
122+
"Reads a signed byte from the I2C device"
123+
try:
124+
result = self.bus.read_byte_data(self.address, reg)
125+
if result > 127: result -= 256
126+
if self.debug:
127+
print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
128+
(self.address, result & 0xFF, reg))
129+
return result
130+
except IOError, err:
131+
return self.errMsg()
132+
133+
def readU16(self, reg, little_endian=True):
134+
"Reads an unsigned 16-bit value from the I2C device"
135+
try:
136+
result = self.bus.read_word_data(self.address,reg)
137+
# Swap bytes if using big endian because read_word_data assumes little
138+
# endian on ARM (little endian) systems.
139+
if not little_endian:
140+
result = ((result << 8) & 0xFF00) + (result >> 8)
141+
if (self.debug):
142+
print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg)
143+
return result
144+
except IOError, err:
145+
return self.errMsg()
146+
147+
def readS16(self, reg, little_endian=True):
148+
"Reads a signed 16-bit value from the I2C device"
149+
try:
150+
result = self.readU16(reg,little_endian)
151+
if result > 32767: result -= 65536
152+
return result
153+
except IOError, err:
154+
return self.errMsg()
155+
156+
if __name__ == '__main__':
157+
try:
158+
bus = Adafruit_I2C(address=0)
159+
print "Default I2C bus is accessible"
160+
except:
161+
print "Error accessing default I2C bus"

‎Adafruit_PWM_Servo_Driver.py

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
#!/usr/bin/python
2+
3+
import time
4+
import math
5+
from Adafruit_I2C import Adafruit_I2C
6+
7+
# ============================================================================
8+
# Adafruit PCA9685 16-Channel PWM Servo Driver
9+
# ============================================================================
10+
11+
class PWM :
12+
# Registers/etc.
13+
__MODE1 = 0x00
14+
__MODE2 = 0x01
15+
__SUBADR1 = 0x02
16+
__SUBADR2 = 0x03
17+
__SUBADR3 = 0x04
18+
__PRESCALE = 0xFE
19+
__LED0_ON_L = 0x06
20+
__LED0_ON_H = 0x07
21+
__LED0_OFF_L = 0x08
22+
__LED0_OFF_H = 0x09
23+
__ALL_LED_ON_L = 0xFA
24+
__ALL_LED_ON_H = 0xFB
25+
__ALL_LED_OFF_L = 0xFC
26+
__ALL_LED_OFF_H = 0xFD
27+
28+
# Bits
29+
__RESTART = 0x80
30+
__SLEEP = 0x10
31+
__ALLCALL = 0x01
32+
__INVRT = 0x10
33+
__OUTDRV = 0x04
34+
35+
general_call_i2c = Adafruit_I2C(0x00)
36+
37+
@classmethod
38+
def softwareReset(cls):
39+
"Sends a software reset (SWRST) command to all the servo drivers on the bus"
40+
cls.general_call_i2c.writeRaw8(0x06) # SWRST
41+
42+
def __init__(self, address=0x40, debug=False):
43+
self.i2c = Adafruit_I2C(address)
44+
self.i2c.debug = debug
45+
self.address = address
46+
self.debug = debug
47+
if (self.debug):
48+
print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2"
49+
self.setAllPWM(0, 0)
50+
self.i2c.write8(self.__MODE2, self.__OUTDRV)
51+
self.i2c.write8(self.__MODE1, self.__ALLCALL)
52+
time.sleep(0.005) # wait for oscillator
53+
54+
mode1 = self.i2c.readU8(self.__MODE1)
55+
mode1 = mode1 & ~self.__SLEEP # wake up (reset sleep)
56+
self.i2c.write8(self.__MODE1, mode1)
57+
time.sleep(0.005) # wait for oscillator
58+
59+
def setPWMFreq(self, freq):
60+
"Sets the PWM frequency"
61+
prescaleval = 25000000.0 # 25MHz
62+
prescaleval /= 4096.0 # 12-bit
63+
prescaleval /= float(freq)
64+
prescaleval -= 1.0
65+
if (self.debug):
66+
print "Setting PWM frequency to %d Hz" % freq
67+
print "Estimated pre-scale: %d" % prescaleval
68+
prescale = math.floor(prescaleval + 0.5)
69+
if (self.debug):
70+
print "Final pre-scale: %d" % prescale
71+
72+
oldmode = self.i2c.readU8(self.__MODE1);
73+
newmode = (oldmode & 0x7F) | 0x10 # sleep
74+
self.i2c.write8(self.__MODE1, newmode) # go to sleep
75+
self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
76+
self.i2c.write8(self.__MODE1, oldmode)
77+
time.sleep(0.005)
78+
self.i2c.write8(self.__MODE1, oldmode | 0x80)
79+
80+
def setPWM(self, channel, on, off):
81+
"Sets a single PWM channel"
82+
self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF)
83+
self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8)
84+
self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF)
85+
self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8)
86+
87+
def setAllPWM(self, on, off):
88+
"Sets a all PWM channels"
89+
self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF)
90+
self.i2c.write8(self.__ALL_LED_ON_H, on >> 8)
91+
self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF)
92+
self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8)

‎arm.py

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
#!/usr/bin/python
2+
3+
4+
from flask import Flask, render_template, request, Response, send_file
5+
6+
import os
7+
import atexit
8+
9+
from Adafruit_PWM_Servo_Driver import PWM
10+
import time
11+
12+
pwm = PWM(0x40, debug=True)
13+
14+
servoMin = 150 # Min pulse length out of 4096
15+
servoMax = 600 # Max pulse length out of 4096
16+
17+
pwm.setPWMFreq(60) # Set frequency to 60 Hz
18+
19+
app = Flask(__name__)
20+
21+
def setServoPulse(channel, pulse):
22+
pulseLength = 1000000 # 1,000,000 us per second
23+
pulseLength /= 60 # 60 Hz
24+
print "%d us per period" % pulseLength
25+
pulseLength /= 4096 # 12 bits of resolution
26+
print "%d us per bit" % pulseLength
27+
pulse *= 1000
28+
pulse /= pulseLength
29+
pwm.setPWM(channel, 0, pulse)
30+
31+
32+
@app.route("/")
33+
def main():
34+
return render_template('arm.html')
35+
36+
@app.route('/slider')
37+
def slider():
38+
num = int(request.args.get('num'))
39+
value = int(request.args.get('value'))
40+
print("%i %i" % (num, value))
41+
fraction = int(servoMin + (value/100.0)*(servoMax - servoMin))
42+
print("Setting servo to %i" % fraction)
43+
pwm.setPWM(num, 0, fraction)
44+
return ""
45+
46+
47+
if __name__ == "__main__":
48+
app.run()

‎drive.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,9 @@ def turnOffMotors():
2626

2727
################################# DC motor test!
2828
mFL = mh.getMotor(1)
29-
mBL = mh.getMotor(2)
29+
mBL = mh.getMotor(4)
3030
mBR = mh.getMotor(3)
31-
mFR = mh.getMotor(4)
31+
mFR = mh.getMotor(2)
3232

3333
def wakeup(m):
3434
# set the speed to start, from 0 (off) to 255 (max speed)

‎templates/arm.html

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
<html>
2+
<head>
3+
<link rel="stylesheet" href="http://cdnjs.cloudflare.com/ajax/libs/font-awesome/4.6.3/css/font-awesome.min.css">
4+
</head>
5+
<body>
6+
<script language='javascript'>
7+
8+
function call(name) {
9+
console.log(name);
10+
var xhr = new XMLHttpRequest();
11+
xhr.open('GET', name, true);
12+
xhr.send();
13+
}
14+
15+
function slider(num, value) {
16+
console.log(num);
17+
var xhr = new XMLHttpRequest();
18+
xhr.open('GET', "slider?num="+num+"&value="+value);
19+
xhr.send();
20+
}
21+
22+
document.onkeyup = function(e) {
23+
call('stop');
24+
}
25+
26+
document.onkeydown = function(e) {
27+
var key = e.keyCode ? e.keyCode : e.which;
28+
console.log(key);
29+
30+
if (key == 37) { //Left
31+
call('l');
32+
} else if (key == 38) { //up
33+
call('b');
34+
} else if (key == 39) {//right
35+
call('r');
36+
} else if (key == 40) {// down
37+
call('f');
38+
} else if (key == 90) {//z
39+
call('stop');
40+
} else if (key == 32) { //space
41+
call('img_rec');
42+
}
43+
}
44+
45+
46+
47+
</script>
48+
<ul style="list-style-type: none;">
49+
<li><input type="range" min="0" max="100" onchange="slider(0,this.value)"/>
50+
<li><input type="range" min="0" max="100" onchange="slider(1,this.value)"/>
51+
<li><input type="range" min="0" max="100" onchange="slider(2,this.value)"/>
52+
<li><input type="range" min="0" max="100" onchange="slider(3,this.value)"/>
53+
<li><input type="range" min="0" max="100" onchange="slider(4,this.value)"/>
54+
<li><input type="range" min="0" max="100" onchange="slider(5,this.value)"/>
55+
56+
</ul>
57+
</body>
58+
59+
</html>

0 commit comments

Comments
 (0)