import sys

sys.path.append("/var/lib/cloud9/vention-control/python-api")

import urllib.parse

from MachineMotion import *

mm = MachineMotionV2()

# When starting a program, one must remove the software stop before moving

print("--> Removing software stop")

mm.releaseEstop()

print("--> Resetting system")

mm.resetSystem()

# Detect all connected digital IO Modules

detectedIOModules = mm.detectIOModules()

home_position = 0 #deg

clockwise = DIRECTION.NORMAL #clockwise direction is positive

counter_clockwise = DIRECTION.NEGATIVE #counterclockwise direction is positive

max_rotation_distance = 360 #(deg) 1 full rotation

#The following variables can be changed based on your system set up

rotary_axis = 1 #can change depending on which drive port is used (drive 1, 2, 3 or 4)

speed_fast = 200 #deg/s, recommended max speed=300 deg/s

speed_slow = 10 #deg/s, recommended max speed = 10 deg/s for position accuracy

stop_accel = 400 #deg/s2, max 500deg/s2

IO_NetworkID = 4 #To be set depending on IO address

home_sensor_IOpin = 0 #read IO pin 0 (choose between 0,1,2,3)

motor_size=MOTOR_SIZE.LARGE #to change depending on existing motor (LARGE / MEDIUM / SMALL)

#defining sensor detected function (read pin and return pin value if sensor is triggered (triggered == 0))

def sensor_detected():

    pin_value = mm.digitalRead(IO_NetworkID, home_sensor_IOpin)

    print(f'Stage {stage_number}: pin {home_sensor_IOpin} on {IO_NetworkID} has value {pin_value}')

    return pin_value == 0

# config motor

# Note: make sure to add the correct positive direction (clockwise or counter_clockwise)

mm.configServo(rotary_axis, MECH_GAIN.indexer_v2_deg_turn, counter_clockwise, 10.0, motor_size)

# Stage 1: Move fast until sensor is triggered (locate sensor position)

stage_number = 1

#detect conecting IO module(s)

if detectedIOModules is not None :

    #WARNING: THIS WILL MAKE THE SERVO MOVES BETWEEN 0-360 deg until the sensor is detected, depending on the starting position

    #set speed to find home then start move in the positive direction

    mm.setSpeed(speed_fast)

    mm.moveRelative(rotary_axis, max_rotation_distance)

    #move until sensor is triggered

    while True:

        if sensor_detected():

            mm.stopContinuousMove(rotary_axis, stop_accel)

            break

       

    # Stage 2: Move backward slow, ensuring the senser trigger piece is on the sensor

    stage_number = 2

    # setspeed - very slow then start move in the negative direction

    mm.setSpeed(speed_slow)

    print("low speed")

    mm.moveRelative(rotary_axis, -max_rotation_distance)

    print("reverse move")

    # condition if trigger piece has stop pass sensor, continue move negative until the sensor is triggered

    if not sensor_detected():

        while True:

            if sensor_detected():

                break

    # Stage 3: verified the trigger piece is on top of the sensor

    # continue in the negative direction and stop at position right before trigger piece (accurate position), set current position to 0

    stage_number = 3

    while True:

        if not sensor_detected():

            mm.stopContinuousMove(rotary_axis, stop_accel)

            print("home_ready, position set to 0")

            mm.setPosition(rotary_axis,home_position)

            break

        time.sleep(0.1)

# Vention Homing with IO solution for Rotary Actuator V2