# Odrive S1 config - Firmware 0.6.8
# For the NEO V1.1 motor.
# https://www.revrobotics.com/rev-21-1650/

CAN_NODE_ID = 6
CAN_BAUD_RATE = 1000000
ENABLE_BRAKE_RESISTOR = True

odrv = odrv0
odrv.config.dc_bus_overvoltage_trip_level = 30
odrv.config.dc_bus_undervoltage_trip_level = 19.2
odrv.config.dc_max_positive_current = 40
odrv.config.dc_max_negative_current = -7
odrv.axis0.config.motor.motor_type = MotorType.HIGH_CURRENT

# 8.27 / Motor KV (rpm/V)
odrv.axis0.config.motor.torque_constant = 8.27 / 473

odrv.axis0.config.motor.pole_pairs = 7
odrv.axis0.config.motor.current_soft_max = 40
odrv.axis0.config.motor.current_hard_max = 80

# Half the continuous current rating of the motor
odrv.axis0.config.motor.calibration_current = 20
odrv.axis0.config.calibration_lockin.current = 20

# calib_max_voltage < 0.5 * vbus_voltage &&
# calib_max_voltage > calibration_current * phase_resistance
odrv.axis0.config.motor.resistance_calib_max_voltage = 5

odrv.axis0.controller.config.control_mode = ControlMode.VELOCITY_CONTROL
odrv.axis0.controller.config.input_mode = InputMode.VEL_RAMP
odrv.axis0.controller.config.vel_ramp_rate = 100
odrv.axis0.controller.config.vel_limit = 100
odrv.axis0.controller.config.vel_limit_tolerance = 1.2

# Disable spinout detection
odrv.axis0.controller.config.spinout_electrical_power_threshold = 9999
odrv.axis0.controller.config.spinout_mechanical_power_threshold = -9999

# Brake Resistor
odrv.config.brake_resistor0.enable = ENABLE_BRAKE_RESISTOR
odrv.config.brake_resistor0.resistance = 2
odrv.config.brake_resistor0.enable_dc_bus_voltage_feedback = True
odrv.config.brake_resistor0.dc_bus_voltage_feedback_ramp_start = 40
odrv.config.brake_resistor0.dc_bus_voltage_feedback_ramp_end = 46

odrv.axis0.config.torque_soft_min = -1.25
odrv.axis0.config.torque_soft_max = 1.25

#odrv.axis0.config.torque_soft_min = -0.25
#odrv.axis0.config.torque_soft_max = 0.25


odrv.axis0.config.enable_watchdog = True
odrv.axis0.config.watchdog_timeout = 2

odrv.axis0.config.encoder_bandwidth = 100
odrv.hall_encoder0.config.enabled = True
odrv.axis0.config.load_encoder = EncoderId.HALL_ENCODER0
odrv.axis0.config.commutation_encoder = EncoderId.HALL_ENCODER0

# PID Loop
odrv.axis0.controller.config.vel_gain = 0.04
odrv.axis0.controller.config.vel_integrator_gain = 0.5 * 10 * odrv.axis0.controller.config.vel_gain
# The '10' above being in Hz from encoder_bandwidth (1/100ms = 1/0.1s = 10Hz)

odrv.can.config.protocol = Protocol.SIMPLE

# Add can bus functionality
# CAN node ID must be different for each device.
odrv.config.enable_can_a = True
odrv.can.config.protocol = Protocol.SIMPLE
odrv.axis0.config.can.node_id = CAN_NODE_ID
odrv.can.config.baud_rate = CAN_BAUD_RATE

# Enable cyclical data to reach over CAN
odrv = odrv0
odrv.axis0.config.can.heartbeat_msg_rate_ms = 100
odrv.axis0.config.can.encoder_msg_rate_ms = 10
odrv.axis0.config.can.iq_msg_rate_ms = 0
odrv.axis0.config.can.torques_msg_rate_ms = 0
odrv.axis0.config.can.error_msg_rate_ms = 0
odrv.axis0.config.can.temperature_msg_rate_ms = 0
odrv.axis0.config.can.bus_voltage_msg_rate_ms = 0

odrv.save_configuration()