diff --git a/head/spine/appendages/i2c_encoder.py b/head/spine/appendages/i2c_encoder.py index 71c1632..e30c14b 100644 --- a/head/spine/appendages/i2c_encoder.py +++ b/head/spine/appendages/i2c_encoder.py @@ -39,7 +39,7 @@ def get_command_parameters(self): yield self.positionIndex, [self.POSITION, "i"] yield self.positionResultIndex, [self.POSITION_RESULT, "d"] yield self.rawPositionIndex, [self.RAW_POSITION, "i"] - yield self.rawPositionResultIndex, [self.RAW_POSITION_RESULT, "d"] + yield self.rawPositionResultIndex, [self.RAW_POSITION_RESULT, "l"] yield self.speedIndex, [self.SPEED, "i"] yield self.speedResultIndex, [self.SPEED_RESULT, "d"] yield self.velocityIndex, [self.VELOCITY, "i"] @@ -54,7 +54,7 @@ def get_position(self): return self.sim_position response = self.spine.send(self.devname, True, self.POSITION, self.index) - response = Angle(response[0], Angle.rev) + response = Angle(response[0] / 2, Angle.rev) return response def set_position(self, position): @@ -97,9 +97,9 @@ def zero(self): def pid_get(self): if self.pidSource == 'position': - return self.position() + return self.get_position() elif self.pidSource == 'velocity': - return self.velocity() + return self.get_velocity() def sim_update(self, tm_diff): self.sim_position += self.sim_velocity * tm_diff diff --git a/head/spine/appendages/pid.py b/head/spine/appendages/pid.py index 49a3a98..e2d331b 100644 --- a/head/spine/appendages/pid.py +++ b/head/spine/appendages/pid.py @@ -2,83 +2,68 @@ class Pid(Component): - PID_MODIFY_CONSTANT = "kModifyPidConstants" - PID_SET = "kSetPidSetpoint" + GET_PID_CONSTANTS = "kGetPidConstants" + GET_PID_CONSTANTS_RESULT = "kGetPidConstantsResult" + SET_PID_CONSTANTS = "kSetPidConstants" + SET_PID_SETPOINT = "kSetPidSetpoint" + GET_PID_VALUES = "kGetPidValues" + GET_PID_VALUES_RESULT = "kGetPidValuesResult" PID_OFF = "kPidOff" - PID_DISPLAY = "kPidDisplay" - PID_DISPLAY_RESULT = "kPidDisplayResult" - - VPID_MODIFY_CONSTANT = "kModifyVpidConstants" - VPID_SET = "kSetVpidSetpoint" - VPID_OFF = "kVpidOff" - VPID_DISPLAY = "kVpidDisplay" - VPID_DISPLAY_RESULT = "kVpidDisplayResult" def __init__(self, spine, devname, config, commands, sim): self.spine = spine self.devname = devname self.label = config['label'] self.index = config['index'] - self.vpid = config['vpid'] self.sim = sim if not self.sim: - if not self.vpid: - self.modifyConstantsIndex = commands[self.PID_MODIFY_CONSTANTS] - self.setIndex = commands[self.PID_SET] - self.offIndex = commands[self.PID_OFF] - self.displayIndex = commands[self.PID_DISPLAY] - self.displayResultIndex = commands[self.PID_DISPLAY_RESULT] - - self.MODIFY_CONSTANTS = self.PID_MODIFY_CONSTANTS - self.SET = self.PID_SET - self.OFF = self.PID_OFF - self.DISPLAY = self.PID_DISPLAY - self.DISPLAY_RESULT = self.PID_DISPLAY_RESULT - else: - self.modifyConstantsIndex = commands[self.VPID_MODIFY_CONSTANTS] - self.setIndex = commands[self.VPID_SET] - self.offIndex = commands[self.VPID_OFF] - self.displayIndex = commands[self.VPID_DISPLAY] - self.displayResultIndex = commands[self.VPID_DISPLAY_RESULT] - - self.MODIFY_CONSTANTS = self.VPID_MODIFY_CONSTANTS - self.SET = self.VPID_SET - self.OFF = self.VPID_OFF - self.DISPLAY = self.VPID_DISPLAY - self.DISPLAY_RESULT = self.VPID_DISPLAY_RESULT + self.getPidConstantsIndex = commands[self.GET_PID_CONSTANTS] + self.getPidConstantsResultIndex = commands[self.GET_PID_CONSTANTS_RESULT] + self.setPidConstantsIndex = commands[self.SET_PID_CONSTANTS] + self.setPidSetpointIndex = commands[self.SET_PID_SETPOINT] + self.getPidValuesIndex = commands[self.GET_PID_VALUES] + self.getPidValuesResultIndex = commands[self.GET_PID_VALUES_RESULT] + self.pidOffIndex = commands[self.PID_OFF] def get_command_parameters(self): - yield self.modifyConstantsIndex, [self.MODIFY_CONSTANTS, "iddd"] - yield self.setIndex, [self.SET, "id"] - yield self.offIndex, [self.OFF, "i"] - yield self.displayIndex, [self.DISPLAY, "i"] - yield self.displayResultIndex, [self.DISPLAY_RESULT, "iddd"] + yield self.getPidConstantsIndex, [self.GET_PID_CONSTANTS, "i"] + yield self.getPidConstantsResultIndex, [self.GET_PID_CONSTANTS_RESULT, "ddd"] + yield self.setPidConstantsIndex, [self.SET_PID_CONSTANTS, "iddd"] + yield self.setPidSetpointIndex, [self.SET_PID_SETPOINT, "id"] + yield self.getPidValuesIndex, [self.GET_PID_VALUES, "i"] + yield self.getPidValuesResultIndex, [self.GET_PID_VALUES_RESULT, "ddd"] + yield self.pidOffIndex, [self.PID_OFF, "i"] + + def get_constants(self): + if self.sim: + return + + return self.spine.send(self.devname, True, self.GET_PID_CONSTANTS, self.index) - def modify_constants(self, kp, ki, kd): + def set_constants(self, kp, ki, kd): if self.sim: return - self.spine.send(self.devname, False, self.MODIFY_CONSTANTS, self.index, kp, ki, kd) + self.spine.send(self.devname, False, self.SET_PID_CONSTANTS, self.index, kp, ki, kd) - def set(self, setpoint): + def set_setpoint(self, setpoint): if self.sim: return - self.spine.send(self.devname, False, self.SET, self.index, setpoint) + self.spine.send(self.devname, False, self.SET_PID_SETPOINT, self.index, setpoint) - def off(self): + def get_values(self): if self.sim: return - self.spine.send(self.devname, False, self.OFF, self.index) + return self.spine.send(self.devname, True, self.GET_PID_VALUES, self.index) - def display(self): + def off(self): if self.sim: - return 0 + return - response = self.spine.send(self.devname, True, self.DISPLAY, self.index) - return response + self.spine.send(self.devname, False, self.PID_OFF, self.index) def sim_update(self, tm_diff): pass diff --git a/head/spine/appendages/position_controlled_motor.py b/head/spine/appendages/position_controlled_motor.py new file mode 100644 index 0000000..4dba330 --- /dev/null +++ b/head/spine/appendages/position_controlled_motor.py @@ -0,0 +1,80 @@ +from .component import Component +from ...units import * + + +class PositionControlledMotor(Component): + SET_VOLTAGE = "kSetPCMVoltage" + STOP = "kStopPCM" + POSITION = "kGetPCMPosition" + POSITION_RESULT = "kGetPCMPositionResult" + SET = "kSetPCMPosition" + + def __init__(self, spine, devname, config, commands, sim): + self.spine = spine + self.devname = devname + self.label = config['label'] + self.index = config['index'] + self.sim = sim + self.motor = config['motor'] + self.encoder = config['encoder'] + self.pid = config['pid'] + + if self.sim: + self.sim_position = Constant(0) + if not self.sim: + self.setVoltageIndex = commands[self.SET_VOLTAGE] + self.stopIndex = commands[self.STOP] + self.positionIndex = commands[self.POSITION] + self.positionResultIndex = commands[self.POSITION_RESULT] + self.setIndex = commands[self.SET] + + def get_command_parameters(self): + yield self.setVoltageIndex, [self.SET_VOLTAGE, "ii"] + yield self.stopIndex, [self.STOP, "i"] + yield self.positionIndex, [self.POSITION, "i"] + yield self.positionResultIndex, [self.POSITION_RESULT, "d"] + yield self.setIndex, [self.SET, "id"] + + def set_voltage(self, value): + if self.sim: + # TODO: update sim_velocity based on sim_motor + return + + self.spine.send(self.devname, False, self.SET_VOLTAGE, self.index, value) + + def stop(self): + if self.sim: + self.sim_velocity = Constant(0) + return + + self.spine.send(self.devname, False, self.STOP, self.index) + + def get_position(self): + if self.sim: + return self.sim_position + + response = self.spine.send(self.devname, True, self.POSITION, self.index) + response = Angle(response[0], Angle.rev) + return response + + def set_position(self, position): + if self.sim: + self.sim_position = position + return + + self.spine.send(self.devname, False, self.SET, self.index, position.to(Angle.degree)) + + def get_dependency_update(self): + dependencies = {} + dependencies[self.motor]['sim_velocity'] = self.sim_velocity + dependencies[self.encoder]['sim_velocity'] = self.sim_velocity + return dependencies + + def sim_update(self, tm_diff): + self.sim_position += self.sim_velocity * tm_diff + + def get_hal_data(self): + hal_data = {} + hal_data['velocity'] = self.sim_velocity + hal_data['position'] = self.sim_position + return hal_data diff --git a/head/spine/appendages/vpid.py b/head/spine/appendages/vpid.py new file mode 100644 index 0000000..465e886 --- /dev/null +++ b/head/spine/appendages/vpid.py @@ -0,0 +1,108 @@ +from .component import Component + + +class Pid(Component): + PID_CONSTANTS = "kPidConstants" + PID_CONSTANTS_RESULT = "kPidConstantsResult" + PID_MODIFY_CONSTANTS = "kModifyPidConstants" + PID_SET = "kSetPidSetpoint" + PID_OFF = "kPidOff" + PID_DISPLAY = "kPidDisplay" + PID_DISPLAY_RESULT = "kPidDisplayResult" + + VPID_CONSTANTS = "kVPidConstants" + VPID_CONSTANTS_RESULT = "kVPidConstantsResult" + VPID_MODIFY_CONSTANTS = "kModifyVpidConstants" + VPID_SET = "kSetVpidSetpoint" + VPID_OFF = "kVpidOff" + VPID_DISPLAY = "kVpidDisplay" + VPID_DISPLAY_RESULT = "kVpidDisplayResult" + + def __init__(self, spine, devname, config, commands, sim): + self.spine = spine + self.devname = devname + self.label = config['label'] + self.index = config['index'] + self.vpid = config['vpid'] + self.sim = sim + + if not self.sim: + if not self.vpid: + self.constantsIndex = commands[self.PID_CONSTANTS] + self.constantsResultIndex = commands[self.PID_CONSTANTS_RESULT] + self.modifyConstantsIndex = commands[self.PID_MODIFY_CONSTANTS] + self.setIndex = commands[self.PID_SET] + self.offIndex = commands[self.PID_OFF] + self.displayIndex = commands[self.PID_DISPLAY] + self.displayResultIndex = commands[self.PID_DISPLAY_RESULT] + + self.CONSTANTS = self.PID_CONSTANTS + self.CONSTANTS_RESULT = self.PID_CONSTANTS_RESULT + self.MODIFY_CONSTANTS = self.PID_MODIFY_CONSTANTS + self.SET = self.PID_SET + self.OFF = self.PID_OFF + self.DISPLAY = self.PID_DISPLAY + self.DISPLAY_RESULT = self.PID_DISPLAY_RESULT + else: + self.constantsIndex = commands[self.VPID_CONSTANTS] + self.constantsResultIndex = commands[self.VPID_CONSTANTS_RESULT] + self.modifyConstantsIndex = commands[self.VPID_MODIFY_CONSTANTS] + self.setIndex = commands[self.VPID_SET] + self.offIndex = commands[self.VPID_OFF] + self.displayIndex = commands[self.VPID_DISPLAY] + self.displayResultIndex = commands[self.VPID_DISPLAY_RESULT] + + self.CONSTANTS = self.VPID_CONSTANTS + self.CONSTANTS_RESULT = self.VPID_CONSTANTS_RESULT + self.MODIFY_CONSTANTS = self.VPID_MODIFY_CONSTANTS + self.SET = self.VPID_SET + self.OFF = self.VPID_OFF + self.DISPLAY = self.VPID_DISPLAY + self.DISPLAY_RESULT = self.VPID_DISPLAY_RESULT + + def get_command_parameters(self): + yield self.constantsIndex, [self.CONSTANTS, "i"] + yield self.constantsResultIndex, [self.CONSTANTS, "iddd"] + yield self.modifyConstantsIndex, [self.MODIFY_CONSTANTS, "iddd"] + yield self.setIndex, [self.SET, "id"] + yield self.offIndex, [self.OFF, "i"] + yield self.displayIndex, [self.DISPLAY, "i"] + yield self.displayResultIndex, [self.DISPLAY_RESULT, "iddd"] + + def constants(self): + if self.sim: + return + + response = self.spine.send(self.devname, True, self.CONSTANTS, self.index) + return response + + def modify_constants(self, kp, ki, kd): + if self.sim: + return + + self.spine.send(self.devname, False, self.MODIFY_CONSTANTS, self.index, kp, ki, kd) + + def set(self, setpoint): + if self.sim: + return + + self.spine.send(self.devname, False, self.SET, self.index, setpoint) + + def off(self): + if self.sim: + return + + self.spine.send(self.devname, False, self.OFF, self.index) + + def display(self): + if self.sim: + return 0 + + response = self.spine.send(self.devname, True, self.DISPLAY, self.index) + return response + + def sim_update(self, tm_diff): + pass + + def get_hal_data(self): + return {} diff --git a/head/spine/core.py b/head/spine/core.py index 95be172..7c9a0dc 100644 --- a/head/spine/core.py +++ b/head/spine/core.py @@ -325,10 +325,15 @@ def configure_arduino(self, config): self.appendages[appendage['label']] = class_(self, devname, appendage, commands_config, self.sim) if not self.sim: - for i, command in self.appendages[appendage['label']].get_command_parameters(): - if commands[i] is None: - commands[i] = command - self.command_map[devname][i] = commands[i][0] + try: + for i, command in self.appendages[appendage['label']].get_command_parameters(): + if commands[i] is None: + commands[i] = command + self.command_map[devname][i] = commands[i][0] + except NotImplementedError: + print("Could not get commmand parameters for '{}' of type '{}'".format(appendage['label'], appendage['type'])) + print(self.appendages) + raise if not self.sim: self.messengers[devname] = CmdMessenger(self.arduinos[devname], commands)