#!/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()