import minimalmodbus
import time

SERIAL_PORT = '/dev/servo'
SLAVE_ID = 1
BAUDRATE = 115200
PARITY = minimalmodbus.serial.PARITY_NONE

REG_ENABLE      = 0x62    # Enable-Register
REG_SPEED_CMD   = 0x100E  # Drehzahlvorgabe (r/min)
REG_START_CMD   = 0x1010  # Start/Stopp-Befehl
REG_POS_LOW     = 0x1003  # Positionsvorgabe low 16 Bit
REG_POS_HIGH    = 0x1004  # Positionsvorgabe high 16 Bit

# Positionskommandos verwenden die gleiche Skalierung wie in servo_modbus.py
PULSES_PER_DEG = 160

CMD_ENABLE  = 0x1234
CMD_FORWARD = 0x2222
CMD_REVERSE = 0x1111
CMD_STOP    = 0x0000

REGISTERS = {
    "Drehzahl (RPM)":      (0x0000, True),
    "Motorstrom (A)":      (0x000B, False),
    "Fehlercode":          (0x001A, False),
    "DC-Busspannung (V)":  (0x0028, False),
}

def read_and_show(instrument, label=""):
    print(f"\n==== Status {label} ====")
    for name, (reg, signed) in REGISTERS.items():
        try:
            val = instrument.read_register(reg, 0, signed=signed, functioncode=4)
            if name.startswith("Motorstrom"):
                val = val / 10.0
            print(f"{name:22}: {val}")
        except Exception as e:
            print(f"{name:22}: Fehler ({e})")
    print("")

instrument = minimalmodbus.Instrument(SERIAL_PORT, SLAVE_ID, mode='rtu')
instrument.serial.baudrate = BAUDRATE
instrument.serial.timeout = 0.5
instrument.serial.bytesize = 8
instrument.serial.parity = PARITY
instrument.serial.stopbits = 1

try:
    instrument.write_register(REG_ENABLE, 1, functioncode=6)
    instrument.write_register(REG_START_CMD, CMD_ENABLE, functioncode=6)
    print("Servo ENABLE gesetzt (Register 0x62 = 1)")
except Exception as e:
    print(f"Fehler beim Setzen von ENABLE: {e}")

read_and_show(instrument, "Initial")
time.sleep(2)

for rpm in [200, 300, 0]:
    try:
        instrument.write_register(REG_SPEED_CMD, rpm, functioncode=6)
        if rpm > 0:
            cmd = CMD_FORWARD
        elif rpm < 0:
            cmd = CMD_REVERSE
        else:
            cmd = CMD_STOP
        instrument.write_register(REG_START_CMD, cmd, functioncode=6)
        print(f"Setze Sollwert: {rpm} U/min")
    except Exception as e:
        print(f"Fehler beim Schreiben der Drehzahl: {e}")
    read_and_show(instrument, f"nach {rpm} U/min")
    time.sleep(2)

# Winkelpositionen testen
try:
    pos_90 = int(90 * PULSES_PER_DEG)
    instrument.write_register(REG_POS_LOW, pos_90 & 0xFFFF, functioncode=6)
    instrument.write_register(REG_POS_HIGH, (pos_90 >> 16) & 0xFFFF, functioncode=6)
    instrument.write_register(REG_START_CMD, CMD_FORWARD, functioncode=6)
    print("Drehe 90 Grad CW")
except Exception as e:
    print(f"Fehler beim Positionieren auf 90 Grad: {e}")
read_and_show(instrument, "nach 90°")
time.sleep(2)

try:
    pos_180 = int(180 * PULSES_PER_DEG)
    instrument.write_register(REG_POS_LOW, pos_180 & 0xFFFF, functioncode=6)
    instrument.write_register(REG_POS_HIGH, (pos_180 >> 16) & 0xFFFF, functioncode=6)
    instrument.write_register(REG_START_CMD, CMD_REVERSE, functioncode=6)
    print("Drehe 180 Grad CCW")
except Exception as e:
    print(f"Fehler beim Positionieren auf 180 Grad: {e}")
read_and_show(instrument, "nach 180°")
time.sleep(2)

try:
    instrument.write_register(REG_ENABLE, 0, functioncode=6)
    instrument.write_register(REG_START_CMD, CMD_STOP, functioncode=6)
    print("Servo DISABLE gesetzt (Register 0x62 = 0)")
except Exception as e:
    print(f"Fehler beim Zurücksetzen von ENABLE: {e}")

print("Programm Ende.")