# Run the motor on 48v
import canopen # pip install canopen
import time

# 10 max, -90,
ROTATION_POSITION = -10
MAX_ROTATIONS_PER_SECOND = 200

safe_range = [-100, 20]
if ROTATION_POSITION < safe_range[0] or ROTATION_POSITION > safe_range[1]:
    print("out of safe range")
    exit

import sys

# Allow specifying the CAN channel as a command line argument, default to 'can0'
if len(sys.argv) > 1:
    can_channel = sys.argv[1]
else:
    can_channel = "can0"

network = canopen.Network()
network.connect(channel=can_channel, bustype="socketcan", bitrate=1000000)

# Default ID is 2
node = network.add_node(9, "../MMS760400-48-C2-1.eds")

# node.sdo[0x6040].raw = 0x0006


node.sdo[0x6040].raw = 0x0006

# Homing Mode
node.sdo[0x6060].raw = 0x06

# shutdown
node.sdo[0x6040].raw = 0x0006

# enable operation
node.sdo[0x6040].raw = 0x000F

# Set Homing Method to 1
node.sdo[0x6098].raw = -0x03
# max torque
node.sdo[0x2070][0x01].raw = 500

# Set homing speed
node.sdo[0x6099][0x01].raw = 65536
node.sdo[0x6099][0x02].raw = 65536

# Homing offset from top.
node.sdo[0x607C].raw = 2 * 65536

#  Homing acceleration
node.sdo[0x609A].raw = 3276800

node.sdo[0x6040].raw = 0x001F # enable operation to new set point

while True:
    status_word = node.sdo[0x6041].raw
    current_pos = node.sdo[0x6064].raw
    curr_torque = node.sdo[0x6077].raw
    if status_word & (1 << 10):
        print("Target reached", status_word, status_word & (1 << 10))
        print('curr', current_pos)
        print("final position", node.sdo[0x6064].raw)
        break
    else:
        print(status_word, status_word & (1 << 10))
        print('curr', current_pos, curr_torque)
    time.sleep(0.01)


node.sdo[0x6040].raw = 0x0006

network.disconnect()