#!/usr/bin/python3
# Calibrate dual-isense P3
# Dependencies:
#   python3 -m pip install canopen
#
# Usage:
#   ./calp3.py <can_device> <can_id>

import sys       # Python Standard Library
import platform  # Python Standard Library
import time      # Python Standard Library
import canopen

def calibrate(can_device, can_id):
    network = canopen.Network() # Start with an empty network
    system = platform.system()      # Determine the operating system
    print("Connecting to {0} in {1}...".format(can_device, system))
    try: 
        if system == "Windows":
            network.connect(bustype='pcan', 
                            channel='PCAN_USBBUS'+str(int(can_device[-1:])+1), 
                            bitrate=1000000)
        elif system == "Linux":
            network.connect(bustype='socketcan', 
                            channel=can_device, 
                            bitrate=1000000)
    except:
        return "Failed to connect."
  
    print("Connection succeeded, adding CANopen node id {}...".format(can_id))
    node = network.add_node(can_id, None)
  
    # Reboot the node
    node.nmt.send_command(0x81)
    time.sleep(1)
    
    # Calibrate alpha/beta current offsets
    node.sdo.download(0x2385,1,b'\x01\x00') # Run current offset routine
    time.sleep(3)
    alpha = int.from_bytes(node.sdo.upload(0x2384,20), byteorder='little', signed=False)
    beta = int.from_bytes(node.sdo.upload(0x2384,21), byteorder='little', signed=False)
    print("Current offsets: Alpha = {0}, Beta = {1}".format(alpha, beta))

    # Calibrate alpha/beta current gains
    node.sdo.download(0x2385,2,b'\x2C\x01\x00\x00') # Set the calibration current in mA
    node.sdo.download(0x2385,1,b'\x03\x00') # Run current gain routine
    time.sleep(6)
    alpha = int.from_bytes(node.sdo.upload(0x2384,22), byteorder='little', signed=False)
    beta = int.from_bytes(node.sdo.upload(0x2384,23), byteorder='little', signed=False)
    print("Current gains: Alpha = {0}, Beta = {1}".format(alpha, beta))

    # Calibrate encoder offset
    node.sdo.download(0x2385,1,b'\x02\x00') # Run encoder offset routine
    time.sleep(3)
    offset = int.from_bytes(node.sdo.upload(0x2383,35), byteorder='little', signed=False)
    print("Encoder offset: {0}".format(offset))

    return "Calibration finished."

if __name__ == "__main__":
    # Read the command-line arguments
    can_device = sys.argv[1]  # Typically 'can0' for both Linux & Windows
    can_id = int(sys.argv[2]) # 1-127

    # Flash the new firmware
    result = calibrate(can_device, can_id)

    print(result)