# 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='can2', bustype="socketcan", bitrate=1000000)

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

# node.sdo[0x6040].raw = 0x0006
fault_register = node.sdo[0x200b][0x0B].raw
print("fault_register", fault_register)
print("can_id", node.sdo[0x2101].raw)
print("motor max voltage", node.sdo[0x200b][0x09].raw)
print("motor voltage protection lower limit", node.sdo[0x200b][0x0A].raw)


# Reset the fault register
node.sdo[0x6040].raw = 128 

network.disconnect()