iot-lab2020/Versuch 3/keyikt_main (copy).py

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