initMotor

Denne funksjonsblokken skal initialisere motorene, slik at vi er sikker på at de starter opp riktig. Mer beskrivelse kommer senere.

Homing: kan egentlig være en egen funksjonsblokk.

1. Skru på motorene og sjekk om de er påkoblet riktig med MC_Power.status.
2. Kjør en og en motor sakte i "trekk inn" tau retning. 
3. Sjekk actual_torque, og når verdien når threshold stopp motoren, lagre posisjonen, gi ut litt tau igjen og gjør det samme steget bare enda saktere. 
4. Da har vi homet motorene, og de vil gå til akkurat dette punktet hver gang de starter.

INPUT

• SoftMotionAxis

OUTPUT

• BOOL (Startup finished or failed)