langspielplatte/iot-lab2020
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Endlich alles ins Git.

Browse Source
This commit is contained in:
langspielplatte
2020-09-11 09:01:51 +02:00
commit 08a5370c0f
20 changed files with 1525 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
Versuch\ 3/.venv/

24
Versuch 1/aufgabe_e.sh Normal file
View File

@@ -0,0 +1,24 @@
#!/bin/bash
LED_RED="17"
LED_GREEN="18"
# setup
echo "Setup red led on pin $LED_RED"
echo $LED_RED > /sys/class/gpio/export
echo "out" > /sys/class/gpio/gpio$LED_RED/direction
echo "Setup red led on pin $LED_GREEN"
echo $LED_GREEN > /sys/class/gpio/export
echo "out" > /sys/class/gpio/gpio$LED_GREEN/direction
while true; do
echo "1" > /sys/class/gpio/gpio$LED_RED/value
echo "0" > /sys/class/gpio/gpio$LED_GREEN/value
sleep 1
echo "0" > /sys/class/gpio/gpio$LED_RED/value
echo "1" > /sys/class/gpio/gpio$LED_GREEN/value
sleep 1
done

12
Versuch 1/aufgabe_f.sh Normal file
View File

@@ -0,0 +1,12 @@
#!/bin/bash
TASTER_PIN="2"
# setup
echo "Setup taster on pin $TASTER_PIN"
echo $TASTER_PIN > /sys/class/gpio/export
echo "in" > /sys/class/gpio/gpio$TASTER_PIN/direction
while true; do
echo -e "Taster sagt: "
cat /sys/class/gpio/gpio$TASTER_PIN/value
done

8
Versuch 1/setup.sh Normal file
View File

@@ -0,0 +1,8 @@
#!/bin/bash
mkdir -p /home/pi/Group03/Versuch{1,2,3,4,5,6,7,8}
cp -r /home/pi/src /home/pi/Group03/
chown -R pi:pi /home/pi/Group03
echo "Aufgabe in:"
grep -r --color AUFGABE /home/pi/Group03/src
echo -e "\n\n"

35
Versuch 2/aufgabe_a.py Normal file
View File

@@ -0,0 +1,35 @@
#!/usr/bin/env python3
from time import sleep
try:
import RPi.GPIO as GPIO
except RuntimeError:
print("Error importing RPi.GPIO! Run this script as root.")
exit(1)
LED_RED = 17
LED_GREEN = 18
def setup():
# Run GPIO with PI pin numbers.
GPIO.setmode(GPIO.BCM)
# Set outputs fpr led pins
GPIO.setup(LED_RED, GPIO.OUT)
GPIO.setup(LED_GREEN, GPIO.OUT)
def main():
setup()
try:
while True:
GPIO.output(LED_RED, GPIO.HIGH)
GPIO.output(LED_GREEn, GPIO.LOW)
sleep(1)
GPIO.output(LED_RED, GPIO.LOW)
GPIO.output(LED_GREEn, GPIO.HIGH)
sleep(1)
except KeyboardInterrupt:
GPIO.cleanup()
if __name__ == "__main__":
main()

58
Versuch 2/aufgabe_b.py Normal file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python3
from time import sleep
import threading
try:
import RPi.GPIO as GPIO
except RuntimeError:
print("Error importing RPi.GPIO! Run this script as root.")
exit(1)
class LED(Thread):
def __init__(self, pin, frequency, use_pwm=False):
Thread.__init__(self)
self.__pin = pin
self.__frequency
self.__use_pwm = use_pwm
self.__pwm = None
GPIO.setup(pin, GPIO.OUT)
if use_pwm:
self.__pwm = GPIO.PWM(pin, frequency)
def new_random_frequency(self):
self.__frequency = random.randint(1, 100)
def run(self):
try:
if self.__use_pwm:
self.__pwm.start(0)
while True:
for s in range(0, 1, 0.1):
self.__pwm.ChangeDutyCycle(math.sin(s * math.pi))
#sleep(1 / self.__frequency)
sleep(1)
else:
while True:
GPIO.output(LED_GREEn, GPIO.LOW)
sleep(1 / self.__frequency)
GPIO.output(LED_GREEn, GPIO.HIGH)
sleep(1 / self.__frequency)
except KeyboardInterrupt:
GPIO.cleanup()
def setup():
# Run GPIO with PI pin numbers.
GPIO.setmode(GPIO.BCM)
def main():
setup()
# Pin, frequency in Hz
led_thread = LED(17, 1000)
led_thread.run()
if __name__ == "__main__":
main()

41
Versuch 2/aufgabe_c.py Normal file
View File

@@ -0,0 +1,41 @@
#!/usr/bin/env python3
from time import sleep
import threading
import aufgabe_b
try:
import RPi.GPIO as GPIO
except RuntimeError:
print("Error importing RPi.GPIO! Run this script as root.")
exit(1)
TASTER_PIN = 2
def setup():
# Run GPIO with PI pin numbers.
GPIO.setmode(GPIO.BCM)
GPIO.setup(TASTER_PIN, GPIO.IN)
def main():
setup()
# Pin, frequency in Hz
led_thread1 = LED(17, 1000)
led_thread1.run()
led_thread2 = LED(18, 1000)
led_thread2.run()
# read taster
try:
while True:
if GPIO.input(TASTER_PIN) == 1:
print("Taster was pressed. Set new frequency.")
led_thread1.new_random_frequency()
led_thread2.new_random_frequency()
except KeyboardInterrupt:
GPIO.cleanup()
if __name__ == "__main__":
main()

26
Versuch 2/aufgabe_g.py Normal file
View File

@@ -0,0 +1,26 @@
#!/usr/bin/env python3
from time import sleep
import threading
import aufgabe_b
try:
import RPi.GPIO as GPIO
except RuntimeError:
print("Error importing RPi.GPIO! Run this script as root.")
exit(1)
def setup():
# Run GPIO with PI pin numbers.
GPIO.setmode(GPIO.BCM)
def main():
setup()
# Pin, frequency in Hz
led_thread1 = LED(17, 1000, use_pwm=True)
led_thread1.run()
if __name__ == "__main__":
main()

0
Versuch 2/servoblaster_ctl.py Normal file
View File

12
Versuch 3/Pipfile Normal file
View File

@@ -0,0 +1,12 @@
[[source]]
name = "pypi"
url = "https://pypi.org/simple"
verify_ssl = true
[dev-packages]
[packages]
pygame = "==1.9.4"
[requires]
python_version = "3.8"

51
Versuch 3/Pipfile.lock generated Normal file
View File

@@ -0,0 +1,51 @@
{
"_meta": {
"hash": {
"sha256": "66dc4fa611a558e59e4b96b5c720de3680644511017efcb95d30b2a242825bec"
},
"pipfile-spec": 6,
"requires": {
"python_version": "3.8"
},
"sources": [
{
"name": "pypi",
"url": "https://pypi.org/simple",
"verify_ssl": true
}
]
},
"default": {
"pygame": {
"hashes": [
"sha256:06dc92ccfea33b85f209db3d49f99a2a30c88fe9fb80fa2564cee443ece787b5",
"sha256:0919a2ec5fcb0d00518c2a5fa99858ccf22d7fbcc0e12818b317062d11386984",
"sha256:0a8c92e700e0042faefa998fa064616f330201890d6ea1c993eb3ff30ab53e99",
"sha256:220a1048ebb3d11a4d48cc4219ec8f65ca62fcafd255239478677625e8ead2e9",
"sha256:315861d2b8428f7b4d56d2c98d6c1acc18f08c77af4b129211bc036774f64be2",
"sha256:3469e87867832fe5226396626a8a6a9dac9b2e21a7819dd8cd96cf0e08bbcd41",
"sha256:54c19960180626165512d596235d75dc022d38844467cec769a8d8153fd66645",
"sha256:5ba598736ab9716f53dc943a659a9578f62acfe00c0c9c5490f3aca61d078f75",
"sha256:60ddc4f361babb30ff2d554132b1f3296490f3149d6c1c77682213563f59937a",
"sha256:6a49ab8616a9de534f1bf62c98beabf0e0bb0b6ff8917576bba22820bba3fdad",
"sha256:6d4966eeba652df2fd9a757b3fc5b29b578b47b58f991ad714471661ea2141cb",
"sha256:700d1781c999af25d11bfd1f3e158ebb660f72ebccb2040ecafe5069d0b2c0b6",
"sha256:73f4c28e894e76797b8ccaf6eb1205b433efdb803c70f489ebc3db6ac9c097e6",
"sha256:786eca2bea11abd924f3f67eb2483bcb22acff08f28dbdbf67130abe54b23797",
"sha256:7bcf586a1c51a735361ca03561979eea3180de45e6165bcdfa12878b752544af",
"sha256:82a1e93d82c1babceeb278c55012a9f5140e77665d372a6d97ec67786856d254",
"sha256:9e03589bc80a21ae951fca7659a767b7cac668289937e3756c0ab3d753cf6d24",
"sha256:aa8926a4e34fb0943abe1a8bb04a0ad82265341bf20064c0862db0a521100dfc",
"sha256:aa90689b889c417d2ac571ef2bbb5f7e735ae30c7553c60fae7508404f46c101",
"sha256:c9f8cdefee267a2e690bf17d61a8f5670b620f25a981f24781b034363a8eedc9",
"sha256:d9177afb2f46103bfc28a51fbc49ce18987a857e5c934db47b4a7030cb30fbd0",
"sha256:deb0551d4bbfb8131e2463a7fe1943bfcec5beb11acdf9c4bfa27fa5a9758d62",
"sha256:e7edfe57a5972aa9130ce9a186020a0f097e7a8e4c25e292109bdae1432b77f9",
"sha256:f0ad32efb9e26160645d62ba6cf3e5a5828dc4e82e8f41f9badfe7b685b07295"
],
"index": "pypi",
"version": "==1.9.4"
}
},
"develop": {}
}

32
Versuch 3/gpio_class.py Normal file
View File

@@ -0,0 +1,32 @@
#!/usr/bin/env python
import os
def sb_write(fd, servo, pulse):
try:
os.write(fd, '%d=%d\n' % (servo,pulse))
except IOError as e:
print e
def write(servo, pulse):
if servo == 1:
if pulse < 100 or pulse > 200:
print 'PWM %d out of range!' % (pulse)
return
if servo == 2:
if pulse < 100 or pulse > 200:
print 'PWM %d out of range!' % (pulse)
return
sb_write(fd, servo, pulse)
try:
fd = os.open('/dev/servoblaster', os.O_WRONLY)
except OSError as e:
print 'could not open /dev/servoblaster'
raise SystemExit(5)
except (KeyboardInterrupt, SystemExit):
os.close(fd)
pass

BIN
Versuch 3/gpio_class.pyc Normal file
View File

Binary file not shown.

101
Versuch 3/iot_car.py Normal file
View File

@@ -0,0 +1,101 @@
import math
def calculate_acceleration(speed, acc, V_MAX=11):
sigma = 2.5
mu = V_MAX / 2
return acc * (1 - 0.5 * (1 + (math.erf((abs(speed) - mu) / (math.sqrt(2 * sigma**2))))))
def calculate_friction(speed, f, V_MAX=11):
# TODO hier stimmt was nicht
sigma = 4
mu = V_MAX / 2
return (f/2) * (1 + (math.erf((abs(speed) - mu) / (math.sqrt(2 * sigma**2)))))
class Iot_car():
V_MAX = 11 #m/s
A_MAX = 45 # degree
acc = 2.6 # Max acceleration of the car (per sec.)
dec = 4.5 # Max deceleration of the car (per sec.)
frict = -1 # max friction
angle_acc = 300 # max change of angle (per sec.)
is_testmode_activated = True
is_testmode_servo_active = False
speed_cur = 0
speed_last = 0
angle_cur = 0
motor = None
streeting = None
def __init__(self, testmode=True):
self.is_testmode_activated = testmode
if not testmode:
import servo_ctrl
self.motor = Motor(1)
self.streeting = Steering(2)
def __testmode_accelerate(self):
calc_acc = calculate_acceleration(self.speed_cur, self.acc)
self.speed_last = self.speed_cur
self.speed_cur = min(self.V_MAX, self.speed_cur + calc_acc)
self.is_testmode_servo_active = True
def __testmode_decelerate(self):
calc_dec = calculate_acceleration(self.speed_cur, self.dec)
self.speed_last = self.speed_cur
self.speed_cur = max(-1 * self.V_MAX, self.speed_cur - calc_dec)
self.is_testmode_servo_active = True
def __accelerate(self, speed):
self.speed_last = self.speed_cur
self.speed_cur = speed
self.motor.set_speed(speed)
def accelerate(self, speed=0, direct=False):
if direct and self.is_testmode_activated: # Mouse testmode
self.last = self.speed_cur
self.speed_cur = speed
elif self.is_testmode_activated:
if speed > 0:
self.__testmode_accelerate()
elif speed < 0:
self.__testmode_decelerate()
else:
self.__accelerate(speed)
def reset_streeting(self):
self.set_angle(0)
def streeting_right(self, angle=0):
if angle > 0:
a = angle
else:
a = min(self.A_MAX, self.angle_cur + self.angle_acc)
self.set_angle(a)
def streeting_left(self, angle=0):
if angle < 0:
a = angle
else:
a = max(-1 * self.A_MAX, self.angle_cur - self.angle_acc)
self.set_angle(a)
def set_angle(self, angle):
self.angle_cur = angle
if not self.is_testmode_activated:
self.streeting.set_angle(angle)
def stop(self):
self.speed_cur = 0
self.angle_cur = 0
def symulate(self):
if self.is_testmode_activated:
if not self.is_testmode_servo_active:
self.speed_last = self.speed_cur
self.speed_cur = min(0, self.speed_cur - calculate_friction(self.speed_cur, self.frict))

257
Versuch 3/keyikt_main (copy).py Executable file
View File

@@ -0,0 +1,257 @@
#!/usr/bin/env python2
#######################################################################
# Aufgabe 1 #
#######################################################################
import pygame
import math
import argparse, sys
import servo_ctrl
width = 400
height = 200
freq = 50 # Sets the frequency of input procession
delta = 1.0 / freq # time per step
acc = 2.6 # Max acceleration of the car (per sec.)
dec = 4.5 # Max deceleration of the car (per sec.)
frict = -1 # max friction
angle_acc = 300 # max change of angle (per sec.)
speed_cur = 0
angle_cur = 0
V_MAX = 11 # Maximale Geswindigkeit 11 m/s
ANGLE_MAX = 45
mouse_ctrl = False
v_step = True # Stufenweises regel der Beschlunigung in beide Richtungen
m_step = True
# vars for testmode
is_testmode_enabled = True
testmode_vars = {'is_servo_active': False}
# vars for car
motor = None
streeting = None
# start main pygame event processing loop here
pygame.display.init()
# set up the pygame screen enviroment
screen = pygame.display.set_mode((width, height))
# get a clock to generate frequent behaviour
clock = pygame.time.Clock()
# States of the keys
keystates = {
'quit': False,
'accelerate': False,
'decelerate': False,
'reset': False,
'accelerate_angle_right': False,
'accelerate_angle_left': False,
'acceleration_mouse': False,
'mouse_ctrl': False,
}
running = True
# Functions for exercises
def calculate_acceleration(speed, acc):
sigma = 2.5
mu = V_MAX / 2
return acc * (1 - 0.5 * (1 + (math.erf((abs(speed) - mu) / (math.sqrt(2 * sigma**2))))))
def calculate_friction(speed, f):
# TODO hier stimmt was nicht
sigma = 4
mu = V_MAX / 2
return (f/2) * (1 + (math.erf((abs(speed) - mu) / (math.sqrt(2 * sigma**2)))))
def toggle_mouse_ctrl():
global mouse_ctrl
mouse_ctrl= not mouse_ctrl
def mouse_ctrl_calc_velocity(pos):
global height, V_MAX
x, y = pos
rel_y = y - (height/2)
return -1 * (rel_y / (height/2.0)) * V_MAX
def mouse_ctrl_calc_angle(pos):
global width, ANGLE_MAX
x, y = pos
rel_x = x - (width/2.0)
return (rel_x / (width/2)) * ANGLE_MAX
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='IoT Lab script')
parser.add_argument('--notest', action='store_true', default=False)
args = parser.parse_args()
if not args.notest:
print "Running in testmode."
else:
is_testmode_enabled = False
motor = Motor(1)
streeting = Steering(2)
try:
while running:
# set clock frequency
clock.tick(freq);
# save the last speed 4 analysis
last = speed_cur
# process input events
for event in pygame.event.get():
# exit on quit
if event.type == pygame.QUIT:
running = False
# check for key down events (press)
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_q:
keystates['quit'] = True
elif event.key == pygame.K_w:
keystates['accelerate'] = True
elif event.key == pygame.K_s:
keystates['decelerate'] = True
elif event.key == pygame.K_d:
keystates['accelerate_angle_right'] = True
elif event.key == pygame.K_a:
keystates['accelerate_angle_left'] = True
elif event.key == pygame.K_m:
keystates['mouse_ctrl'] = True
elif event.key == pygame.K_r:
keystates['reset'] = True
# check for key up events (release)
if event.type == pygame.KEYUP:
if event.key == pygame.K_q:
keystates['quit'] = False
elif event.key == pygame.K_w:
v_step = True
testmode_vars['is_servo_active'] = False
keystates['accelerate'] = False
elif event.key == pygame.K_s:
v_step = True
testmode_vars['is_servo_active'] = False
keystates['decelerate'] = False
elif event.key == pygame.K_d:
keystates['accelerate_angle_right'] = False
elif event.key == pygame.K_a:
keystates['accelerate_angle_left'] = False
elif event.key == pygame.K_m:
keystates['mouse_ctrl'] = False
m_step = True
elif event.key == pygame.K_r:
v_step = True
keystates['reset'] = False
if event.type == pygame.MOUSEBUTTONDOWN:
# linke maustaste
if event.button == 1:
testmode_vars['is_servo_active'] = True
keystates['acceleration_mouse'] = True
if event.type == pygame.MOUSEBUTTONUP:
if event.button == 1:
testmode_vars['is_servo_active'] = False
keystates['acceleration_mouse'] = False
# do something about the key states here, now that the event queue has been processed
if keystates['quit']:
running = False
if keystates['reset']:
# reset
speed_cur = 0
angle_cur = 0
mouse_ctrl = False
if keystates['mouse_ctrl'] and m_step:
# beim umstellen zur maus steuerung und geschw. zuruecksetzen
speed_cur = 0
angle_cur = 0
toggle_mouse_ctrl()
m_step = False
print "Is mouse control enabled?: " + str(mouse_ctrl)
# Calculate valeues for control types
if mouse_ctrl:
pos = pygame.mouse.get_pos()
angle_cur = mouse_ctrl_calc_angle(pos)
if keystates['acceleration_mouse']:
speed_cur = mouse_ctrl_calc_velocity(pos)
elif not is_testmode_enabled:
speed_cur = 0
else:
if keystates['accelerate']: # and v_step:
# Beschleunigen
v_step = False
if is_testmode_enabled:
calc_acc = calculate_acceleration(speed_cur, acc)
speed_cur = min(V_MAX, speed_cur + calc_acc)
testmode_vars['is_servo_active'] = True
else:
speed_cur = min(V_MAX, speed_cur + acc)
if keystates['decelerate']: # and v_step:
# abbremsen
v_step = False
if is_testmode_enabled:
calc_dec = calculate_acceleration(speed_cur, dec)
speed_cur = max(-1 * V_MAX, speed_cur - calc_dec)
testmode_vars['is_servo_active'] = True
else:
speed_cur = max(-1 * V_MAX, speed_cur - dec)
if keystates['accelerate_angle_right']:
# ist das so richtig angle_acc = 300? TODO
angle_cur = min(ANGLE_MAX, angle_cur + angle_acc)
if keystates['accelerate_angle_left']:
# ist das so richtig angle_acc = 300? TODO
angle_cur = max(-1 * ANGLE_MAX, angle_cur - angle_acc)
if not keystates['accelerate_angle_left'] and not keystates['accelerate_angle_right']:
angle_cur = 0
if not is_testmode_enabled and (not keystates['accelerate'] and not keystates['decelerate']):
# Wenn nicht im testmode und weger beschl noch gebremst wird
speed_cur = 0
if is_testmode_enabled:
if not testmode_vars['is_servo_active']:
speed_cur = min(0, speed_cur - calculate_friction(speed_cur, -1))
else:
motor.set_speed(speed_cur)
steering.set_angle(angle_cur)
# Hier denn Wete an servos geben TODO
print("({},{} --> {})".format(speed_cur, angle_cur, (speed_cur - last) / delta)) + ", Servo_active: " + str(testmode_vars['is_servo_active'])
except KeyboardInterrupt:
print "Exiting through keyboard event (CTRL + C)"
# gracefully exit pygame here
pygame.quit()

205
Versuch 3/keyikt_main.py Executable file
View File

@@ -0,0 +1,205 @@
#!/usr/bin/env python2
#######################################################################
# Aufgabe 1 #
#######################################################################
import pygame
import math
import argparse, sys
#import servo_ctrl,
from iot_car import Iot_car
width = 400
height = 200
freq = 50 # Sets the frequency of input procession
delta = 1.0 / freq # time per step
frict = -1 # max friction
mouse_ctrl = False
v_step = True # Stufenweises regel der Beschlunigung in beide Richtungen
m_step = True
# vars for testmode
is_testmode_enabled = True
testmode_vars = {'is_servo_active': False}
# vars for car
motor = None
streeting = None
# start main pygame event processing loop here
pygame.display.init()
# set up the pygame screen enviroment
screen = pygame.display.set_mode((width, height))
# get a clock to generate frequent behaviour
clock = pygame.time.Clock()
# States of the keys
keystates = {
'quit': False,
'accelerate': False,
'decelerate': False,
'reset': False,
'accelerate_angle_right': False,
'accelerate_angle_left': False,
'acceleration_mouse': False,
'mouse_ctrl': False,
}
running = True
# Functions for exercises
def toggle_mouse_ctrl():
global mouse_ctrl
mouse_ctrl= not mouse_ctrl
def mouse_ctrl_calc_velocity(pos):
global height
V_MAX = 11
x, y = pos
rel_y = y - (height/2)
return -1 * (rel_y / (height/2.0)) * V_MAX
def mouse_ctrl_calc_angle(pos):
global width
ANGLE_MAX = 45
x, y = pos
rel_x = x - (width/2.0)
return (rel_x / (width/2)) * ANGLE_MAX
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='IoT Lab script')
parser.add_argument('--notest', action='store_true', default=False)
args = parser.parse_args()
if not args.notest:
print "Running in testmode."
car = Iot_car(testmode= not args.notest)
try:
while running:
# set clock frequency
clock.tick(freq);
# process input events
for event in pygame.event.get():
# exit on quit
if event.type == pygame.QUIT:
running = False
# check for key down events (press)
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_q:
keystates['quit'] = True
elif event.key == pygame.K_w:
keystates['accelerate'] = True
elif event.key == pygame.K_s:
keystates['decelerate'] = True
elif event.key == pygame.K_d:
keystates['accelerate_angle_right'] = True
elif event.key == pygame.K_a:
keystates['accelerate_angle_left'] = True
elif event.key == pygame.K_m:
keystates['mouse_ctrl'] = True
elif event.key == pygame.K_r:
keystates['reset'] = True
# check for key up events (release)
if event.type == pygame.KEYUP:
if event.key == pygame.K_q:
keystates['quit'] = False
elif event.key == pygame.K_w:
v_step = True
car.is_testmode_servo_active = False
keystates['accelerate'] = False
elif event.key == pygame.K_s:
v_step = True
car.is_testmode_servo_active = False
keystates['decelerate'] = False
elif event.key == pygame.K_d:
keystates['accelerate_angle_right'] = False
elif event.key == pygame.K_a:
keystates['accelerate_angle_left'] = False
elif event.key == pygame.K_m:
keystates['mouse_ctrl'] = False
m_step = True
elif event.key == pygame.K_r:
v_step = True
keystates['reset'] = False
if event.type == pygame.MOUSEBUTTONDOWN:
# linke maustaste
if event.button == 1:
car.is_testmode_servo_active = True
keystates['acceleration_mouse'] = True
if event.type == pygame.MOUSEBUTTONUP:
if event.button == 1:
car.is_testmode_servo_active = False
keystates['acceleration_mouse'] = False
# do something about the key states here, now that the event queue has been processed
if keystates['quit']:
running = False
if keystates['reset']:
# reset
car.stop()
mouse_ctrl = False
if keystates['mouse_ctrl'] and m_step:
# beim umstellen zur maus steuerung und geschw. zuruecksetzen
car.stop()
toggle_mouse_ctrl()
m_step = False
print "Is mouse control enabled?: " + str(mouse_ctrl)
# Calculate valeues for control types
# MOUSE
if mouse_ctrl:
pos = pygame.mouse.get_pos()
angle_cur = mouse_ctrl_calc_angle(pos)
car.set_angle(mouse_ctrl_calc_angle(pos))
if keystates['acceleration_mouse']:
speed_cur = mouse_ctrl_calc_velocity(pos)
car.accelerate(speed=speed_cur, direct=True)
# Keyboard
else:
if keystates['accelerate']: # and v_step:
# Beschleunigen
v_step = False
car.accelerate(speed=1)
if keystates['decelerate']: # and v_step:
# abbremsen
car.accelerate(speed=-1)
if keystates['accelerate_angle_right']:
car.streeting_right()
if keystates['accelerate_angle_left']:
car.streeting_left()
if not keystates['accelerate_angle_left'] and not keystates['accelerate_angle_right']:
car.reset_streeting()
print("({},{} --> {})".format(car.speed_cur, car.angle_cur, (car.speed_cur - car.speed_last) / delta)) + ", Servo_active: " + str(car.is_testmode_servo_active)
car.symulate()
except KeyboardInterrupt:
print "Exiting through keyboard event (CTRL + C)"
# gracefully exit pygame here
pygame.quit()

562
Versuch 3/linuxWiimoteLib.py Normal file
View File

@@ -0,0 +1,562 @@
# LICENSE: MIT (X11) License which follows:
#
# Copyright (c) 2008 Stephane Duchesneau
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
#
# Modified by Pere Negre and Pietro Pilolli
#
#
import bluetooth
import code
from datetime import datetime
from math import atan, asin, acos #ZDR
import threading
from time import time,sleep
import time
def i2bs(val):
lst = []
while val:
lst.append(val&0xff)
val = val >> 8
lst.reverse()
return lst
class WiimoteState:
Battery = None
class ButtonState:
A = False
B = False
Down = False
Home = False
Left = False
Minus = False
One = False
Plus = False
Right = False
Two = False
Up = False
class GyroState:
calibrated = False
int_yaw = 0
int_roll = 0
int_pitch = 0
zero_yaw = 8192
zero_roll = 8159
zero_pitch = 8208
class AccelState:
X = 0
Y = 0
Z = 0
RawX = 0
RawY = 0
RawZ = 0
zeroX = 0
zeroY = 0
zeroZ = 0
class IRState:
RawX1 = 0
RawX2 = 0
RawX3 = 0
RawX4 = 0
RawY1 = 0
RawY2 = 0
RawY3 = 0
RawY4 = 0
Found1 = 0
Found2 = 0
Found3 = 0
Found4 = 0
Size1 = 0
Size2 = 0
Size3 = 0
Size4 = 0
X1 = X2 = X3 = X4 = 0.0
Y1 = Y2 = Y3 = Y4 = 0.0
#Mode = None
MidX = 0
MidY = 0
RawMidX = 0
RawMidY = 0
class LEDState:
LED1 = False
LED2 = False
LED3 = False
LED4 = False
class Parser:
""" Sets the values contained in a signal """
A = 0x0008
B = 0x0004
Down = 0x0400
Home = 0x0080
Left = 0x0100
Minus = 0x0010
One = 0x0002
Plus = 0x1000
Right = 0x0200
Two = 0x0001
Up = 0x0800
# ZDR
gyro_deg_s = 1./8192./200.
def parseButtons(self,signal, ButtonState): #signal is 16bit long intl
ButtonState.A = bool(signal&self.A)
ButtonState.B = bool(signal&self.B)
ButtonState.Down = bool(signal&self.Down)
ButtonState.Home = bool(signal&self.Home)
ButtonState.Left = bool(signal&self.Left)
ButtonState.Minus = bool(signal&self.Minus)
ButtonState.One = bool(signal&self.One)
ButtonState.Plus = bool(signal&self.Plus)
ButtonState.Right = bool(signal&self.Right)
ButtonState.Two = bool(signal&self.Two)
ButtonState.Up = bool(signal&self.Up)
#ZDR
def parseAccelerometer(self, signal, AccelState):
AccelState.RawX = signal[0] - 512
AccelState.RawY = signal[1] - 512
AccelState.RawZ = signal[2] - 512
#ZDR
def parseGyro(self, signal, slow, GyroState):
if GyroState.calibrated==False:
return
yaw_slow = 440 if slow[0]==1 else 2000
roll_slow = 440 if slow[1]==1 else 2000
pitch_slow = 440 if slow[2]==1 else 2000
GyroState.int_yaw += (signal[0] - GyroState.zero_yaw)*yaw_slow # *self.gyro_deg_s
GyroState.int_roll += (signal[1] - GyroState.zero_roll)*roll_slow
GyroState.int_pitch += (signal[2] - GyroState.zero_pitch)*pitch_slow
#print '%d\t\t%d\t\t%d' % (GyroState.int_yaw, GyroState.int_roll, GyroState.int_pitch )
def parseIR(self,signal,irstate):
irstate.RawX1 = signal[0] + ((signal[2] & 0x30) >>4 << 8)
irstate.RawY1 = signal[1] + (signal[2] >> 6 << 8)
irstate.Size1 = signal[2] & 0x0f
if irstate.RawY1 == 1023: irstate.Found1 = False
else: irstate.Found1 = True
irstate.RawX2 = signal[3] + ((signal[5] & 0x30) >>4 << 8)
irstate.RawY2 = signal[4] + (signal[5] >> 6 << 8)
irstate.Size2 = signal[5] & 0x0f
if irstate.RawY2 == 1023: irstate.Found2 = False
else: irstate.Found2 = True
irstate.RawX3 = signal[6] + ((signal[8] & 0x30) >>4 << 8)
irstate.RawY3 = signal[7] + (signal[8] >> 6 << 8)
irstate.Size3 = signal[8] & 0x0f
if irstate.RawY3 == 1023: irstate.Found3 = False
else: irstate.Found3 = True
irstate.RawX4 = signal[9] + ((signal[11] & 0x30) >>4 << 8)
irstate.RawY4 = signal[10] + (signal[11] >> 6 << 8)
irstate.Size4 = signal[11] & 0x0f
if irstate.RawY4 == 1023: irstate.Found4 = False
else: irstate.Found4 = True
if irstate.Found1:
if irstate.Found2:
irstate.RawMidX = (irstate.RawX1 + irstate.RawX2) / 2
irstate.RawMidY = (irstate.RawY1 + irstate.RawY2) / 2
else:
irstate.RawMidX = irstate.RawX1
irstate.RawMidY = irstate.RawY1
irstate.MidX = float(irstate.RawMidX) / 1024
irstate.MidY = float(irstate.RawMidY) / 768
else: irstate.MidX = irstate.MidY = 0
class Setter:
"""The opposite from the Parser class: returns the signal needed to set the values in the wiimote"""
LED1 = 0x10
LED2 = 0x20
LED3 = 0x40
LED4 = 0x80
def SetLEDs(self,ledstate):
signal = 0
if ledstate.LED1: signal += self.LED1
if ledstate.LED2: signal += self.LED2
if ledstate.LED3: signal += self.LED3
if ledstate.LED4: signal += self.LED4
return signal
class InputReport:
Buttons = 2 #2 to 8 not implemented yet !!! only IR is implemented
Status = 4
ReadData = 5
ButtonsExtension = 6
class Wiimote(threading.Thread):
state = None
running = False
WiimoteState = WiimoteState
InputReport = InputReport
gyroCalibration = [] # ZDR
packet_number = 0
packet_time = 0
def __init__(self):
threading.Thread.__init__(self)
self.parser = Parser()
self.setter = Setter()
self.IRCallback = None
def Connect(self, device):
self.bd_addr = device[0]
self.name = device[1]
self.controlsocket = bluetooth.BluetoothSocket(bluetooth.L2CAP)
try:
self.controlsocket.connect((self.bd_addr,17))
except:
return False
self.datasocket = bluetooth.BluetoothSocket(bluetooth.L2CAP)
self.datasocket.connect((self.bd_addr,19))
self.sendsocket = self.controlsocket
self.CMD_SET_REPORT = 0x52
if self.name == "Nintendo RVL-CNT-01-TR":
self.CMD_SET_REPORT = 0xa2
self.sendsocket = self.datasocket
try:
self.datasocket.settimeout(1)
except NotImplementedError:
print "socket timeout not implemented with this bluetooth module"
print "Connected to ", self.bd_addr
self._get_battery_status()
self.start_time = datetime.now()
#self.f = open('wiimote.log', 'w')
#self.measurement_ended = False
self.start() #start this thread
return True
def char_to_binary_string(self,char):
ascii = ord(char)
bin = []
while (ascii > 0):
if (ascii & 1) == 1:
bin.append("1")
else:
bin.append("0")
ascii = ascii >> 1
bin.reverse()
binary = "".join(bin)
zerofix = (8 - len(binary)) * '0'
return zerofix + binary
def SetLEDs(self, led1,led2,led3,led4):
self.WiimoteState.LEDState.LED1 = led1
self.WiimoteState.LEDState.LED2 = led2
self.WiimoteState.LEDState.LED3 = led3
self.WiimoteState.LEDState.LED4 = led4
self._send_data((0x11,self.setter.SetLEDs(self.WiimoteState.LEDState)))
def run(self):
print "starting"
#i = 0
self.running = True
while self.running:
try:
x= map(ord,self.datasocket.recv(32))
#t = datetime.now() - self.start_time
#self.packet_time = t.seconds * 1000.0 + t.microseconds / 1000.0
#if t_millis <= 60000.0:
# self.f.write("Packet %i Zeit %f\n" % (self.i, t_millis))
#elif not self.measurement_ended:
# self.f.close()
# print "Measurement ended"
# self.measurement_ended = True
#print "New Data %i" % self.i
#self.packet_number = self.packet_number + 1
except bluetooth.BluetoothError:
continue
self.state = ""
for each in x[:17]:
self.state += self.char_to_binary_string(chr(each)) + " "
if len(x) >= 4:
self.parser.parseButtons((x[2]<<8) + x[3], self.WiimoteState.ButtonState)
#ZDR xyz = x[4:7]
# 0x31: core buttons and accelerometer
# 0x35: buttens accelerometer and extension data
if len(x)>4 and (x[1]==0x31 or x[1]==0x35):
# assume 10 bit precision
ax,ay,az = [i<<2 for i in x[4:7]]
#print ax-512,ay-512,az-512
# add LSB from button values
ax += x[2]>>5 & 0x3
ay += x[3]>>4 & 0x2
az += x[3]>>5 & 0x2
self.parser.parseAccelerometer([ax,ay,az], self.WiimoteState.AccelState)
# ZDR gyroscope data
if len(x)>7 and x[1]==0x35:
data = x[7:13]
yaw = (data[3] & 0xfc)<<6 | data[0]
roll = (data[4] & 0xfc)<<6 | data[1]
pitch = (data[5] & 0xfc)<<6 | data[2]
slow_mode_yaw = (data[3] & 0x2)>>1
slow_mode_pitch = (data[3] & 0x1)
slow_mode_roll = (data[4] & 0x2)>>1
clen = 3000.0
self.WiimoteState.GyroState.calibrated = True
if self.WiimoteState.GyroState.calibrated==False and len(self.gyroCalibration)!=clen:
self.gyroCalibration.append([yaw,roll,pitch])
if len(self.gyroCalibration)==clen:
# gc = self.gyroCalibration
self.WiimoteState.GyroState.calibrated = True
self.WiimoteState.GyroState.zero_yaw = sum([y for y,r,p in self.gyroCalibration])/clen
self.WiimoteState.GyroState.zero_roll = sum([r for y,r,p in self.gyroCalibration])/clen
self.WiimoteState.GyroState.zero_pitch = sum([p for y,r,p in self.gyroCalibration])/clen
print self.WiimoteState.GyroState.zero_yaw
print self.WiimoteState.GyroState.zero_roll
print self.WiimoteState.GyroState.zero_pitch
code.interact(local=locals())
#print yaw, roll, pitch, slow_mode_yaw, slow_mode_roll, slow_mode_pitch,
self.parser.parseGyro([yaw,roll,pitch], [slow_mode_yaw,slow_mode_roll,slow_mode_pitch], self.WiimoteState.GyroState)
#print roll
#if len(x) >= 19:
# self.parser.parseIR(x[7:19],self.WiimoteState.IRState)
# self.doIRCallback()
#sleep(0.01)
self.datasocket.close()
self.controlsocket.close()
print "Bluetooth socket closed succesfully."
self.Dispose()
print "stopping"
def Dispose(self):
self.Disconnect()
def Disconnect(self):
self.running = False
self.WiimoteState.Battery = None
def join(self):#will be called last...
self.Dispose()
def _send_data(self,data):
str_data = ""
for each in data:
str_data += chr(each)
self.sendsocket.send(chr(self.CMD_SET_REPORT) + str_data)
def _write_to_mem(self, address, value):
val = i2bs(value)
val_len=len(val)
val += [0]*(16-val_len)
msg = [0x16] + i2bs(address) + [val_len] +val
self._send_data(msg)
def SetRumble(self,on):
if on: self._send_data((0x11,0x01))
else: self._send_data((0x11,0x00))
def activate_IR(self, sens=6):
self._send_data(i2bs(0x120033)) #mode IR
self._send_data(i2bs(0x1304))#enable transmission
self._send_data(i2bs(0x1a04))#enable transmission
self.setIRSensitivity(sens)
# ZDR
def SetAccelerometerMode(self, mode=0x0):
# see: http://wiibrew.org/wiki/Wiimote#Data_Reporting
# set data reporting mode 0x31: (a1) 31 BB BB AA AA AA
self._send_data((0x12,0x0,0x31))
# ZDR
def SetGyroMode(self):
""" initialize motion plus the extension """
self._write_to_mem(0x4A600FE,0x04)
self._send_data((0x12,0x4,0x35))
# ZDR
def getAccelState(self):
return (self.WiimoteState.AccelState.RawX - WiimoteState.AccelState.zeroX,
self.WiimoteState.AccelState.RawY - WiimoteState.AccelState.zeroY,
self.WiimoteState.AccelState.RawZ - WiimoteState.AccelState.zeroZ)
# ZDR
def calibrateAccelerometer(self):
WiimoteState.AccelState.zeroX = self.WiimoteState.AccelState.RawX
WiimoteState.AccelState.zeroY = self.WiimoteState.AccelState.RawY
WiimoteState.AccelState.zeroZ = self.WiimoteState.AccelState.RawZ
# ZDR
def getGyroState(self, deg_scale=1./8192./200):
return (self.WiimoteState.GyroState.int_yaw*deg_scale,
self.WiimoteState.GyroState.int_roll*deg_scale,
self.WiimoteState.GyroState.int_pitch*deg_scale)
def calibrateGyro(self, yaw=None, roll=None, pitch=None):
if yaw is not None:
self.WiimoteState.GyroState.int_yaw = yaw
if roll is not None:
self.WiimoteState.GyroState.int_roll = roll
if pitch is not None:
self.WiimoteState.GyroState.int_pitch = pitch
def setIRSensitivity(self, n):
if n < 1 or n > 6:
return
self._write_to_mem(0x04b00030,0x08)
time.sleep(0.1)
if n == 1:
self._write_to_mem(0x04b00000,0x0200007101006400fe)
time.sleep(0.1)
self._write_to_mem(0x04b0001a,0xfd05)
elif n == 2:
self._write_to_mem(0x04b00000,0x0200007101009600b4)
time.sleep(0.1)
self._write_to_mem(0x04b0001a,0xb304)
elif n == 3:
self._write_to_mem(0x04b00000,0x020000710100aa0064)
time.sleep(0.1)
self._write_to_mem(0x04b0001a,0x6303)
elif n == 4:
self._write_to_mem(0x04b00000,0x020000710100c80036)
time.sleep(0.1)
self._write_to_mem(0x04b0001a,0x3503)
elif n == 5:
self._write_to_mem(0x04b00000,0x070000710100720020)
time.sleep(0.1)
self._write_to_mem(0x04b0001a,0x1f03)
# MAX
elif n == 6:
self._write_to_mem(0x04b00000,0x000000000000900041)
time.sleep(0.1)
self._write_to_mem(0x04b0001a,0x4000)
time.sleep(0.1)
self._write_to_mem(0x04b00033,0x33)
def _get_battery_status(self):
self._send_data((0x15,0x00))
self.running2 = True
while self.running2:
try:
x= map(ord,self.datasocket.recv(32))
except bluetooth.BluetoothError:
continue
self.state = ""
for each in x[:17]:
if len(x) >= 7:
self.running2 = False
battery_level = x[7]
self.WiimoteState.Battery = float(battery_level) / float(208)
def setIRCallBack(self, func):
self.IRCallback = func
def doIRCallback(self):
if self.IRCallback == None: return
irstate = self.WiimoteState.IRState
if irstate.Found1:
self.IRCallback(irstate.RawX1, irstate.RawY1)
elif irstate.Found2:
self.IRCallback(irstate.RawX2, irstate.RawY2)
elif irstate.Found3:
self.IRCallback(irstate.RawX3, irstate.RawY3)
elif irstate.Found4:
self.IRCallback(irstate.RawX4, irstate.RawY4)
if __name__ == "__main__":
device = ('40:F4:07:C2:08:B9', 'Nintendo RVL-CNT-01-TR')
print device
connected = False
wiimote = Wiimote()
print "Press SYNC on wiimote to make it discoverable"
while (not connected):
connected = wiimote.Connect(device)
leds = [False]*4
for i in xrange(16):
leds[i&0x3] = not leds[i&0x3]
wiimote.SetLEDs(*leds);
#time.sleep(0.1)
#ZDR
# see: http://wiibrew.org/wiki/Wiimote#Data_Reporting
# If bit 2 (0x04) is set, the Wii Remote will send reports
# whether there has been any change to the data or
# not. Otherwise, the Wii Remote will only send an output
# report when the data has changed. set data reporting mode
# 0x31: (a1) 31 BB BB AA AA AA
wiimote._send_data((0x12,0x4,0x31))
#time.sleep(0.1)
print wiimote.WiimoteState.AccelState.RawX, wiimote.WiimoteState.AccelState.RawY, wiimote.WiimoteState.AccelState.RawZ, wiimote.WiimoteState.ButtonState.A
# 0x35: Core Buttons and Accelerometer with 16 Extension Bytes
# (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
# init motion plus: the extension is initialised by writing 0x55 to 0x(4)a600f0
wiimote._write_to_mem(0x4A600FE,0x04)
wiimote._send_data((0x12,0x4,0x35))
code.interact(local=locals())
while 1:
print "%.2f\t%.2f\t%.2f" % wiimote.getGyroState()
if wiimote.WiimoteState.ButtonState.A:
wiimote.calibrateGyro(yaw=0, roll=0)
#time.sleep(0.1)
# wiimote._send_data((0x12,0x4,0x30))
# wiimote.activate_IR()
# while 1:
# time.sleep(0.1)
# #print wiimote.state
# print wiimote.WiimoteState.ButtonState.A, wiimote.WiimoteState.ButtonState.B, wiimote.WiimoteState.ButtonState.Up, wiimote.WiimoteState.ButtonState.Down, wiimote.WiimoteState.ButtonState.Left, wiimote.WiimoteState.ButtonState.Right, wiimote.WiimoteState.ButtonState.Minus, wiimote.WiimoteState.ButtonState.Plus, wiimote.WiimoteState.ButtonState.Home, wiimote.WiimoteState.ButtonState.One, wiimote.WiimoteState.ButtonState.Two, wiimote.WiimoteState.IRState.RawX1, wiimote.WiimoteState.IRState.RawY1, wiimote.WiimoteState.IRState.Size1, wiimote.WiimoteState.IRState.RawX2, wiimote.WiimoteState.IRState.RawY2, wiimote.WiimoteState.IRState.Size2
# #print wiimote.IRState.Found1

62
Versuch 3/servo_ctrl.py Normal file
View File

@@ -0,0 +1,62 @@
#!/usr/bin/env python
#######################################################################
# Aufgabe 1.3 #
#######################################################################
import gpio_class
def write(servo, pulse):
gpio_class.write(servo, pulse)
class Motor(object):
PWM_PIN = 1 # GPIO pin 11
min_pulse = 100 #ms -> -11 m/s
max_pulse = 200 #ms -> 11 m/s
servo = None
__is_active = False
v_max = 11.0
def __init__(self, servo):
self.servo = servo
def set_speed(self, speed):
if abs(speed) > self.v_max:
speed = self.v_max
if speed == 0:
self.__is_active = False
else:
self.__is_active = True
pulse = 150 + ((speed / self.v_max) * ((self.max_pulse - self.min_pulse)/2))
write(self.servo, pulse)
def stop(self):
self.set_speed(0)
# Aufgabe 1d
def is_active(self):
return self.__is_active
class Steering(object):
PWM_PIN = 2 # GPIO pin 12
min_pulse = 100 # -45
max_pulse = 200 # 45
servo = None
angle_max = 45
def __init__(self, servo):
self.servo = servo
def set_angle(self, angle):
if abs(angle) > self.angle_max:
angle = self.angle_max
pulse = 150 + ((angle / self.angle_max) * ((self.max_pulse - self.min_pulse)/2))
write(self.servo, pulse)
def stop(self):
self.set_angel(0)

BIN
Versuch 3/servo_ctrl.pyc Normal file
View File

Binary file not shown.

38
Versuch 3/wiikt_main.py Normal file
View File

@@ -0,0 +1,38 @@
#!/usr/bin/env python
#######################################################################
# Aufgabe 2 #
#######################################################################
from linuxWiimoteLib import *
# initialize wiimote
wiimote = Wiimote()
#Insert address and name of device here
# hcitool scan
#
device = ('', '')
connected = False
try:
print "Press any key on wiimote to connect"
while (not connected):
connected = wiimote.Connect(device)
print "succesfully connected"
wiimote.SetAccelerometerMode()
wiistate = wiimote.WiimoteState
while True:
# re-calibrate accelerometer
if (wiistate.ButtonState.Home):
print 're-calibrating'
wiimote.calibrateAccelerometer()
sleep(0.1)
except KeyboardInterrupt:
print "Exiting through keyboard event (CTRL + C)"
exit(wiimote)