From 08a5370c0ffec406572c6c5f734c560a991549c2 Mon Sep 17 00:00:00 2001 From: Langspielplatte Date: Fri, 11 Sep 2020 09:01:51 +0200 Subject: [PATCH] Endlich alles ins Git. --- .gitignore | 1 + Versuch 1/aufgabe_e.sh | 24 ++ Versuch 1/aufgabe_f.sh | 12 + Versuch 1/setup.sh | 8 + Versuch 2/aufgabe_a.py | 35 ++ Versuch 2/aufgabe_b.py | 58 ++++ Versuch 2/aufgabe_c.py | 41 +++ Versuch 2/aufgabe_g.py | 26 ++ Versuch 2/servoblaster_ctl.py | 0 Versuch 3/Pipfile | 12 + Versuch 3/Pipfile.lock | 51 +++ Versuch 3/gpio_class.py | 32 ++ Versuch 3/gpio_class.pyc | Bin 0 -> 1271 bytes Versuch 3/iot_car.py | 101 ++++++ Versuch 3/keyikt_main (copy).py | 257 +++++++++++++++ Versuch 3/keyikt_main.py | 205 ++++++++++++ Versuch 3/linuxWiimoteLib.py | 562 ++++++++++++++++++++++++++++++++ Versuch 3/servo_ctrl.py | 62 ++++ Versuch 3/servo_ctrl.pyc | Bin 0 -> 3317 bytes Versuch 3/wiikt_main.py | 38 +++ 20 files changed, 1525 insertions(+) create mode 100644 .gitignore create mode 100644 Versuch 1/aufgabe_e.sh create mode 100644 Versuch 1/aufgabe_f.sh create mode 100644 Versuch 1/setup.sh create mode 100644 Versuch 2/aufgabe_a.py create mode 100644 Versuch 2/aufgabe_b.py create mode 100644 Versuch 2/aufgabe_c.py create mode 100644 Versuch 2/aufgabe_g.py create mode 100644 Versuch 2/servoblaster_ctl.py create mode 100644 Versuch 3/Pipfile create mode 100644 Versuch 3/Pipfile.lock create mode 100644 Versuch 3/gpio_class.py create mode 100644 Versuch 3/gpio_class.pyc create mode 100644 Versuch 3/iot_car.py create mode 100755 Versuch 3/keyikt_main (copy).py create mode 100755 Versuch 3/keyikt_main.py create mode 100644 Versuch 3/linuxWiimoteLib.py create mode 100644 Versuch 3/servo_ctrl.py create mode 100644 Versuch 3/servo_ctrl.pyc create mode 100644 Versuch 3/wiikt_main.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5207069 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +Versuch\ 3/.venv/ diff --git a/Versuch 1/aufgabe_e.sh b/Versuch 1/aufgabe_e.sh new file mode 100644 index 0000000..f2702f4 --- /dev/null +++ b/Versuch 1/aufgabe_e.sh @@ -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 + diff --git a/Versuch 1/aufgabe_f.sh b/Versuch 1/aufgabe_f.sh new file mode 100644 index 0000000..f581670 --- /dev/null +++ b/Versuch 1/aufgabe_f.sh @@ -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 diff --git a/Versuch 1/setup.sh b/Versuch 1/setup.sh new file mode 100644 index 0000000..16c347c --- /dev/null +++ b/Versuch 1/setup.sh @@ -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" diff --git a/Versuch 2/aufgabe_a.py b/Versuch 2/aufgabe_a.py new file mode 100644 index 0000000..3789142 --- /dev/null +++ b/Versuch 2/aufgabe_a.py @@ -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() diff --git a/Versuch 2/aufgabe_b.py b/Versuch 2/aufgabe_b.py new file mode 100644 index 0000000..3b741c4 --- /dev/null +++ b/Versuch 2/aufgabe_b.py @@ -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() diff --git a/Versuch 2/aufgabe_c.py b/Versuch 2/aufgabe_c.py new file mode 100644 index 0000000..6cd45ef --- /dev/null +++ b/Versuch 2/aufgabe_c.py @@ -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() diff --git a/Versuch 2/aufgabe_g.py b/Versuch 2/aufgabe_g.py new file mode 100644 index 0000000..34d76b5 --- /dev/null +++ b/Versuch 2/aufgabe_g.py @@ -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() diff --git a/Versuch 2/servoblaster_ctl.py b/Versuch 2/servoblaster_ctl.py new file mode 100644 index 0000000..e69de29 diff --git a/Versuch 3/Pipfile b/Versuch 3/Pipfile new file mode 100644 index 0000000..f072803 --- /dev/null +++ b/Versuch 3/Pipfile @@ -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" diff --git a/Versuch 3/Pipfile.lock b/Versuch 3/Pipfile.lock new file mode 100644 index 0000000..e56f070 --- /dev/null +++ b/Versuch 3/Pipfile.lock @@ -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": {} +} diff --git a/Versuch 3/gpio_class.py b/Versuch 3/gpio_class.py new file mode 100644 index 0000000..5814478 --- /dev/null +++ b/Versuch 3/gpio_class.py @@ -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 + + diff --git a/Versuch 3/gpio_class.pyc b/Versuch 3/gpio_class.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0a04e561ed3e9711b69780e2f634f7026edfc0a0 GIT binary patch literal 1271 zcmd5)-A)rh6h5=t7Fr?3pAfhqYXV7cU`h3Y3o#~0fQYSW)gsX}+ns@~-R?9yvq%*a zBl>P$`Yt|zerE~^2@jx~J!j9(oZs&|{_pwPX5-hpm@c0RzE2_M2b9DsC?z_A;n8;( zf~4;Zk0wi`h(7ovOU@|Kg-1ta>SnuCQWYB3D1T4lXoiF=)9#*{C0U^hLEi*rD{!e2 zj*dO&_?3hZoUTTy%uXT`bcpJm`V2Zr{@`iGKFkuN4B3X5EvU&dSp*6?ckQ0T@Hp)P z+7aDskxF)2W0S-epTxP3W?Xhcb`KevhR0r&TdT5G=Nbqg(#CR~k9lI1sMG#|q+`Hie624d#w?)jOvha<=6fj;wIrJ!hL;w`z4-vQG zY@dcDJ_qf%TG-u_z%iS~l!;>o*J2K@#SCu5fH=T~ipX>UZa7t)$!I2wVuF84enOf1 zP%pbLf>jyl!Up;{$N{E$kZ|vg#Wr))X8NYTJW!h%bSyaauc>JBP&J6T3q@j1EQ+d_ z@vgq+4o*XItufTbenqTB*+aVhPJ$hTIeL;x9Ph9oPT> literal 0 HcmV?d00001 diff --git a/Versuch 3/iot_car.py b/Versuch 3/iot_car.py new file mode 100644 index 0000000..b15f903 --- /dev/null +++ b/Versuch 3/iot_car.py @@ -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)) + diff --git a/Versuch 3/keyikt_main (copy).py b/Versuch 3/keyikt_main (copy).py new file mode 100755 index 0000000..f522cca --- /dev/null +++ b/Versuch 3/keyikt_main (copy).py @@ -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() diff --git a/Versuch 3/keyikt_main.py b/Versuch 3/keyikt_main.py new file mode 100755 index 0000000..2a34094 --- /dev/null +++ b/Versuch 3/keyikt_main.py @@ -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() diff --git a/Versuch 3/linuxWiimoteLib.py b/Versuch 3/linuxWiimoteLib.py new file mode 100644 index 0000000..55a5f33 --- /dev/null +++ b/Versuch 3/linuxWiimoteLib.py @@ -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 + + + diff --git a/Versuch 3/servo_ctrl.py b/Versuch 3/servo_ctrl.py new file mode 100644 index 0000000..58f2ae7 --- /dev/null +++ b/Versuch 3/servo_ctrl.py @@ -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) diff --git a/Versuch 3/servo_ctrl.pyc b/Versuch 3/servo_ctrl.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2c2c331bdb51e04fe41b44906b18eea4c9794a96 GIT binary patch literal 3317 zcmd5;-EJF26h6Cl>^MI_OVh#?AP};IlC4q+2?nr^c8T07&A zKzdc4fydxEcpn}BzVEEMw=>=@TrMu_9Oj5^}FGFoT+N4i;1PfbsxN}YScjPUkXy?YQ=dpyY4z0^Yd2TMoac{#m{~CluJJA+@aW8&9$_PldC2|=Zra%b`No`1k(@mt6 zPt5D+HyUHGLQd%^UC`D^uBz0F=*V=@fR)y3o}!%BS!fO}~E3y6QM5U33utqt8pZ9u7L zY6Gf8QyVr|5*RJzoXe6>>|*Ry%YpLAij@LI!yU$q6d=*xOThOS{?8Hs{(qU_wG14$ z!0_>c;k6o&`mZoNDRpp#+!dR|w6YsML;!M-wW7MgH*@??6s>Dlm7>+Sk72ndrj(*z zot&4%k5gT_iiD=b@)_N2KsD%ZqGPU8yh*{2*-N@IJM40)xkaHMU_sM!R*jqZz4Q^9 zCwpGurWnlB?Xz4OG|_K~Kv$~6W0wU(PBYxRVh$;le%ch2&w2GcjmGCg>*hOu0W(DE Ay8r+H literal 0 HcmV?d00001 diff --git a/Versuch 3/wiikt_main.py b/Versuch 3/wiikt_main.py new file mode 100644 index 0000000..4a7fa4f --- /dev/null +++ b/Versuch 3/wiikt_main.py @@ -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)