diff --git a/robocon/brain/greengiant.py b/robocon/brain/greengiant.py index 9671d7a..b3020ce 100644 --- a/robocon/brain/greengiant.py +++ b/robocon/brain/greengiant.py @@ -385,11 +385,14 @@ def pwm(self): else: if self._mode is not PWM_SERVO: raise IOError(f"Attempt to read PWM property from pin configured as {self._mode}") - raw = read_high_low_data(self._bus, self._analog_base) + if self._pwm_base is None or self._pwm_base >= _GG_GPIO_PWM_BASE: + # the firmware can't report the PWM value of GPIO pins + return self._set_to + raw = read_high_low_data(self._bus, self._pwm_base) if self._version < 10: return ((raw - _GG_GG_PWM_CENTER) / _GG_GG_PWM_PERCENT_HALF_RANGE) * 100 else: - return ((raw - _GG_PiLow_PWM_CENTER) / _GG_PiLow_PWM_PERCENT_HALF_RANGE) * 100 + return ((raw - _GG_PiLow_PWM_CENTER) / _GG_PiLow_PWM_HALF_RANGE) * _GG_PiLow_PWM_PERCENT_HALF_RANGE @pwm.setter def pwm(self, percent): if self._pwm_base is not None: @@ -561,6 +564,10 @@ def __init__(self, bus, max_motor_voltage): # because we care about heating effects in the motors, we have to scale self.power_scaling_factor = max_motor_voltage / _SYSTEM_VOLTAGE + # the firmware truncates the direction register to a byte when it is + # read, so it always returns 0, remember the direction here instead + self._reversed = [False, False] + # should we set up the state of 12v power and enable here? self._bus.write_byte_data(_GG_I2C_ADDR, _GG_ENABLE_MOTORS, 0) # disable the motor controller @@ -575,7 +582,11 @@ def __getitem__(self, index): f"motor index must be in (0,1) but instead got {index}") hex_mag = self._bus.read_byte_data(_GG_I2C_ADDR, _GG_MOTOR_MAG_START + index) - return hex_mag * (100.0 / 256.0) * self.power_scaling_factor + if self.power_scaling_factor == 0: + return 0 + + percent = hex_mag * (100.0 / 256.0) / self.power_scaling_factor + return -percent if self._reversed[index] else percent def __setitem__(self, index, percent): """Clamps input value, converts from percentage to wiring pi format and @@ -586,6 +597,7 @@ def __setitem__(self, index, percent): percent = clamp(percent, -100, 100) direction = (percent < 0) + self._reversed[index] = direction self._bus.write_byte_data(_GG_I2C_ADDR, _GG_MOTOR_DIR_START + index, direction) # Scale such that 50% with a motor limit of 6V is really 3V