
#vor Start (Fahre zur Mitte des Blattes)

fahre linear
wegpunkt1 start


# Hauptschleife
while (True):


#define Grenzen des Arbeitsbereichs (DinA3 Zeichnungsberreich)
x_min=-0.26
x_max=0.11
y_min=-0.34
y_max=-0.17

#define Startposition (Höhe des Stiftes)
          
z_position=-0.101
z_locked=False

#define Spannungsbereich (poti)
voltage_min=0.0
voltage_max=9.5

#Berechnung Grad-->Bogenmaß 
rotation_deg= 45
rotation_rad= (rotation_deg* 3.14159/ 180)


#Berechnung Rotationswinkel
cos_theta=cos(rotation_rad)
sin_theta=sin(rotation_rad)



#Schleife für Berechnung der Koordinaten und Bewegung

While (True):

    #Potentiometer -Spannung abfragen (Analog in 0-10V)
    poti_voltage_x = analog_in(0)
    poti_voltage_y = analog_in(1)

    # Rundung der Potentiometerwerte auf zwei Dezimalstellen (Floor-Methode) 
    potivolt_xround = floor(poti_voltage_x * 100 + 0.5) / 100
    potivolt_yround = floor(poti_voltage_y * 100 + 0.5) / 100

    # Skalierung der Potentiometer -Spannung auf den Arbeitsbereich
    x_position = ((potivolt_xround - voltage_min) / (voltage_max - voltage_min)) * (x_max - x_min) + x_min
    y_position = ((potivolt_yround - voltage_min) / (voltage_max - voltage_min)) * (y_max - y_min) + y_min      
         
                               
    # Aktuelle Pose beziehen
    current_pose = get_actual_tcp_pose()
        
   
    # Rotation der X- und Y-Koordinaten 
    rotated_x = (cos_theta * x_position -   y_position)
    rotated_y = (sin_theta * x_position +   y_position)

    # neue Koordinaten in der Pose anpassen
    target_pose = current_pose
    target_pose[0] = rotated_x   
    target_pose[1] = rotated_y   
        

    # Z-Position dauerhaft setzen, wenn Taster gedrückt wurde
        if digital_in[0]:
            target_pose[2] = z_position  # Speichert die aktuelle Z-Position
            z_locked = True              # Sperrflag setzen

    # Wenn z_locked aktiv ist, schreibe die Z-Position dauerhaft in target_pose[2]
        if z_locked:
            target_pose[2] = z_position
        
       
           
    # Bewegung der neuen Pose 
    movel(target_pose, a=0.5, v=0.1)
        

      
         





