jabrena / gopigo3-java Goto Github PK

    def spi_read_16(self, MessageType):
        """
        Read a 16-bit value over SPI
        Keyword arguments:
        MessageType -- the SPI message type
        Returns touple:
        value, error
        """
        outArray = [self.SPI_Address, MessageType, 0, 0, 0, 0]
        reply = self.spi_transfer_array(outArray)
        if(reply[3] == 0xA5):
            return int((reply[4] << 8) | reply[5])
        raise IOError("No SPI response")
        return 0

    def get_voltage_5v(self):
        """
        Get the 5v circuit voltage
        Returns touple:
        5v circuit voltage, error
        """
        value = self.spi_read_16(self.SPI_MESSAGE_TYPE.GET_VOLTAGE_5V)
        return (value / 1000.0)

    def get_voltage_battery(self):
        """
        Get the battery voltage
        Returns touple:
        battery voltage, error
        """
        value = self.spi_read_16(self.SPI_MESSAGE_TYPE.GET_VOLTAGE_VCC)
        return (value / 1000.0)

    WHEEL_BASE_WIDTH         = 117  # distance (mm) from left wheel to right wheel. This works with the initial GPG3 prototype. Will need to be adjusted.
    WHEEL_DIAMETER           = 66.5 # wheel diameter (mm)
    WHEEL_BASE_CIRCUMFERENCE = WHEEL_BASE_WIDTH * math.pi # The circumference of the circle the wheels will trace while turning (mm)
    WHEEL_CIRCUMFERENCE      = WHEEL_DIAMETER   * math.pi # The circumference of the wheels (mm)

    MOTOR_GEAR_RATIO           = 120 # Motor gear ratio # 220 for Nicole's prototype
    ENCODER_TICKS_PER_ROTATION = 6   # Encoder ticks per motor rotation (number of magnet positions) # 16 for early prototypes
    MOTOR_TICKS_PER_DEGREE = ((MOTOR_GEAR_RATIO * ENCODER_TICKS_PER_ROTATION) / 360.0) # encoder ticks per output shaft rotation degree

def set_motor_position(self, port, position):
        """
        Set the motor target position in degrees
        Keyword arguments:
        port -- The motor port(s). MOTOR_LEFT and/or MOTOR_RIGHT.
        position -- The target position
        """
        position_raw = int(position * self.MOTOR_TICKS_PER_DEGREE)
        outArray = [self.SPI_Address, self.SPI_MESSAGE_TYPE.SET_MOTOR_POSITION, port,\
                    ((position_raw >> 24) & 0xFF), ((position_raw >> 16) & 0xFF),\
                    ((position_raw >> 8) & 0xFF), (position_raw & 0xFF)]
        reply = self.spi_transfer_array(outArray)

    def set_motor_dps(self, port, dps):
        """
        Set the motor target speed in degrees per second
        Keyword arguments:
        port -- The motor port(s). MOTOR_LEFT and/or MOTOR_RIGHT.
        dps -- The target speed in degrees per second
        """
        dps = int(dps * self.MOTOR_TICKS_PER_DEGREE)
        outArray = [self.SPI_Address, self.SPI_MESSAGE_TYPE.SET_MOTOR_DPS, int(port),\
                    ((dps >> 8) & 0xFF), (dps & 0xFF)]
        self.spi_transfer_array(outArray)

    def set_motor_limits(self, port, power = 0, dps = 0):
        """
        Set the motor speed limit
        Keyword arguments:
        port -- The motor port(s). MOTOR_LEFT and/or MOTOR_RIGHT.
        power -- The power limit in percent (0 to 100), with 0 being no limit (100)
        dps -- The speed limit in degrees per second, with 0 being no limit
        """
        dps = int(dps * self.MOTOR_TICKS_PER_DEGREE)
        outArray = [self.SPI_Address, self.SPI_MESSAGE_TYPE.SET_MOTOR_LIMITS, int(port), int(power),\
                    ((dps >> 8) & 0xFF), (dps & 0xFF)]
        self.spi_transfer_array(outArray)

    def get_motor_status(self, port):
        """
        Read a motor status
        Keyword arguments:
        port -- The motor port (one at a time). MOTOR_LEFT or MOTOR_RIGHT.
        Returns a list:
            flags -- 8-bits of bit-flags that indicate motor status:
                bit 0 -- LOW_VOLTAGE_FLOAT - The motors are automatically disabled because the battery voltage is too low
                bit 1 -- OVERLOADED - The motors aren't close to the target (applies to position control and dps speed control).
            power -- the raw PWM power in percent (-100 to 100)
            encoder -- The encoder position
            dps -- The current speed in Degrees Per Second
        """
        if port == self.MOTOR_LEFT:
            message_type = self.SPI_MESSAGE_TYPE.GET_MOTOR_STATUS_LEFT
        elif port == self.MOTOR_RIGHT:
            message_type = self.SPI_MESSAGE_TYPE.GET_MOTOR_STATUS_RIGHT
        else:
            raise IOError("get_motor_status error. Must be one motor port at a time. MOTOR_LEFT or MOTOR_RIGHT.")
            return

        outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        reply = self.spi_transfer_array(outArray)
        if(reply[3] == 0xA5):
            power = int(reply[5])
            if power & 0x80:
                power = power - 0x100

            encoder = int((reply[6] << 24) | (reply[7] << 16) | (reply[8] << 8) | reply[9])
            if encoder & 0x80000000:
                encoder = int(encoder - 0x100000000)

            dps = int((reply[10] << 8) | reply[11])
            if dps & 0x8000:
                dps = dps - 0x10000

            return [reply[4], power, int(encoder / self.MOTOR_TICKS_PER_DEGREE), int(dps / self.MOTOR_TICKS_PER_DEGREE)]
        raise IOError("No SPI response")
        return

    def get_motor_encoder(self, port):
        """
        Read a motor encoder in degrees
        Keyword arguments:
        port -- The motor port (one at a time). MOTOR_LEFT or MOTOR_RIGHT.
        Returns the encoder position in degrees
        """
        if port == self.MOTOR_LEFT:
            message_type = self.SPI_MESSAGE_TYPE.GET_MOTOR_ENCODER_LEFT
        elif port == self.MOTOR_RIGHT:
            message_type = self.SPI_MESSAGE_TYPE.GET_MOTOR_ENCODER_RIGHT
        else:
            raise IOError("Port(s) unsupported. Must be one at a time.")
            return 0

        encoder = self.spi_read_32(message_type)
        if encoder & 0x80000000:
            encoder = int(encoder - 0x100000000)
        return int(encoder / self.MOTOR_TICKS_PER_DEGREE)

    def offset_motor_encoder(self, port, offset):
        """
        Offset a motor encoder
        Keyword arguments:
        port -- The motor port(s). MOTOR_LEFT and/or MOTOR_RIGHT.
        offset -- The encoder offset
        Zero the encoder by offsetting it by the current position
        """
        offset = int(offset * self.MOTOR_TICKS_PER_DEGREE)
        outArray = [self.SPI_Address, self.SPI_MESSAGE_TYPE.OFFSET_MOTOR_ENCODER, int(port),\
                    ((offset >> 24) & 0xFF), ((offset >> 16) & 0xFF), ((offset >> 8) & 0xFF), (offset & 0xFF)]
        self.spi_transfer_array(outArray)