mirror of
https://github.com/hybridgroup/gobot.git
synced 2025-04-24 13:48:49 +08:00
750 lines
19 KiB
Go
750 lines
19 KiB
Go
// Package gopigo3 is based on https://github.com/DexterInd/GoPiGo3/blob/master/Software/Python/gopigo3.py
|
|
// You will need to run the following commands if using a stock raspbian image before this library will work:
|
|
// sudo curl -kL dexterindustries.com/update_gopigo3 | bash
|
|
// sudo reboot
|
|
package gopigo3
|
|
|
|
import (
|
|
"bytes"
|
|
"encoding/binary"
|
|
"fmt"
|
|
"log"
|
|
"math"
|
|
"time"
|
|
|
|
"github.com/hashicorp/go-multierror"
|
|
|
|
"gobot.io/x/gobot/v2"
|
|
"gobot.io/x/gobot/v2/drivers/spi"
|
|
)
|
|
|
|
// spi address for gopigo3
|
|
const goPiGo3Address = 0x08
|
|
|
|
// register addresses for gopigo3
|
|
const (
|
|
NONE byte = iota
|
|
GET_MANUFACTURER
|
|
GET_NAME
|
|
GET_HARDWARE_VERSION
|
|
GET_FIRMWARE_VERSION
|
|
GET_ID
|
|
SET_LED
|
|
GET_VOLTAGE_5V
|
|
GET_VOLTAGE_VCC
|
|
SET_SERVO
|
|
SET_MOTOR_PWM
|
|
SET_MOTOR_POSITION
|
|
SET_MOTOR_POSITION_KP
|
|
SET_MOTOR_POSITION_KD
|
|
SET_MOTOR_DPS
|
|
SET_MOTOR_LIMITS
|
|
OFFSET_MOTOR_ENCODER
|
|
GET_MOTOR_ENCODER_LEFT
|
|
GET_MOTOR_ENCODER_RIGHT
|
|
GET_MOTOR_STATUS_LEFT
|
|
GET_MOTOR_STATUS_RIGHT
|
|
SET_GROVE_TYPE
|
|
SET_GROVE_MODE
|
|
SET_GROVE_STATE
|
|
SET_GROVE_PWM_DUTY
|
|
SET_GROVE_PWM_FREQUENCY
|
|
GET_GROVE_VALUE_1
|
|
GET_GROVE_VALUE_2
|
|
GET_GROVE_STATE_1_1
|
|
GET_GROVE_STATE_1_2
|
|
GET_GROVE_STATE_2_1
|
|
GET_GROVE_STATE_2_2
|
|
GET_GROVE_VOLTAGE_1_1
|
|
GET_GROVE_VOLTAGE_1_2
|
|
GET_GROVE_VOLTAGE_2_1
|
|
GET_GROVE_VOLTAGE_2_2
|
|
GET_GROVE_ANALOG_1_1
|
|
GET_GROVE_ANALOG_1_2
|
|
GET_GROVE_ANALOG_2_1
|
|
GET_GROVE_ANALOG_2_2
|
|
START_GROVE_I2C_1
|
|
START_GROVE_I2C_2
|
|
)
|
|
|
|
const (
|
|
// distance (mm) from left wheel to right wheel. This works with the initial GPG3 prototype. Will need to be adjusted.
|
|
WHEEL_BASE_WIDTH = 117
|
|
// wheel diameter (mm)
|
|
WHEEL_DIAMETER = 66.5
|
|
// circumference of the circle the wheels will trace while turning (mm)
|
|
WHEEL_BASE_CIRCUMFERENCE = WHEEL_BASE_WIDTH * math.Pi
|
|
// circumference of the wheels (mm)
|
|
WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * math.Pi
|
|
// motor gear ratio
|
|
MOTOR_GEAR_RATIO = 120
|
|
// encoder ticks per motor rotation (number of magnet positions)
|
|
ENCODER_TICKS_PER_ROTATION = 6
|
|
// encoder ticks per output shaft rotation degree
|
|
MOTOR_TICKS_PER_DEGREE = ((MOTOR_GEAR_RATIO * ENCODER_TICKS_PER_ROTATION) / 360.0)
|
|
GROVE_I2C_LENGTH_LIMIT = 16
|
|
MOTOR_FLOAT = -128
|
|
)
|
|
|
|
// GroveMode sets the mode of AD pins on the gopigo3.
|
|
type GroveMode byte
|
|
|
|
const (
|
|
GROVE_INPUT_DIGITAL GroveMode = 0
|
|
GROVE_OUTPUT_DIGITAL GroveMode = 1
|
|
GROVE_INPUT_DIGITAL_PULLUP GroveMode = 2
|
|
GROVE_INPUT_DIGITAL_PULLDOWN GroveMode = 3
|
|
GROVE_INPUT_ANALOG GroveMode = 4
|
|
GROVE_OUTPUT_PWM GroveMode = 5
|
|
GROVE_INPUT_ANALOG_PULLUP GroveMode = 6
|
|
GROVE_INPUT_ANALOG_PULLDOWN GroveMode = 7
|
|
)
|
|
|
|
// Servo contains the address for the 2 servo ports.
|
|
type Servo byte
|
|
|
|
const (
|
|
SERVO_1 Servo = 0x01
|
|
SERVO_2 Servo = 0x02
|
|
)
|
|
|
|
// Motor contains the address for the left and right motors.
|
|
type Motor byte
|
|
|
|
const (
|
|
MOTOR_LEFT Motor = 0x01
|
|
MOTOR_RIGHT Motor = 0x02
|
|
)
|
|
|
|
// Led contains the addresses for all leds on the board.
|
|
type Led byte
|
|
|
|
const (
|
|
LED_EYE_RIGHT Led = 0x01
|
|
LED_EYE_LEFT Led = 0x02
|
|
LED_BLINKER_LEFT Led = 0x04
|
|
LED_BLINKER_RIGHT Led = 0x08
|
|
LED_WIFI Led = 0x80
|
|
)
|
|
|
|
// Grove contains the addresses for pins/ports of the AD1/AD2 grove connector.
|
|
type Grove byte
|
|
|
|
const (
|
|
AD11 string = "AD_1_1"
|
|
AD12 string = "AD_1_2"
|
|
AD21 string = "AD_2_1"
|
|
AD22 string = "AD_2_2"
|
|
AD_1_1_G Grove = 0x01 // default pin for most grove devices, A0/D0
|
|
AD_1_2_G Grove = 0x02
|
|
AD_2_1_G Grove = 0x04 // default pin for most grove devices, A0/D0
|
|
AD_2_2_G Grove = 0x08
|
|
AD_1_G Grove = AD_1_1_G + AD_1_2_G
|
|
AD_2_G Grove = AD_2_1_G + AD_2_2_G
|
|
)
|
|
|
|
// GroveType represents the type of a grove device.
|
|
type GroveType int
|
|
|
|
const (
|
|
CUSTOM GroveType = 1
|
|
IR_DI_REMOTE GroveType = 2
|
|
IR_EV3_REMOTE GroveType = 3
|
|
US GroveType = 4
|
|
I2C GroveType = 5
|
|
)
|
|
|
|
// GroveState contains the state of a grove device.
|
|
type GroveState int
|
|
|
|
const (
|
|
VALID_DATA GroveState = iota
|
|
NOT_CONFIGURED
|
|
CONFIGURING
|
|
NO_DATA
|
|
I2C_ERROR
|
|
)
|
|
|
|
// Driver is a Gobot Driver for the GoPiGo3 board.
|
|
type Driver struct {
|
|
name string
|
|
connector spi.Connector
|
|
connection spi.Connection
|
|
spi.Config
|
|
}
|
|
|
|
// NewDriver creates a new Gobot Driver for the GoPiGo3 board.
|
|
//
|
|
// Params:
|
|
//
|
|
// a *Adaptor - the Adaptor to use with this Driver
|
|
//
|
|
// Optional params:
|
|
//
|
|
// spi.WithBusNumber(int): bus to use with this driver
|
|
// spi.WithChipNumber(int): chip to use with this driver
|
|
// spi.WithMode(int): mode to use with this driver
|
|
// spi.WithBitCount(int): number of bits to use with this driver
|
|
// spi.WithSpeed(int64): speed in Hz to use with this driver
|
|
func NewDriver(a spi.Connector, options ...func(spi.Config)) *Driver {
|
|
spiConfig := spi.NewConfig()
|
|
// use /dev/spidev0.1
|
|
spiConfig.SetBusNumber(0)
|
|
spiConfig.SetChipNumber(1)
|
|
d := &Driver{
|
|
name: gobot.DefaultName("GoPiGo3"),
|
|
connector: a,
|
|
Config: spiConfig,
|
|
}
|
|
for _, option := range options {
|
|
option(d)
|
|
}
|
|
return d
|
|
}
|
|
|
|
// Name returns the name of the device.
|
|
func (d *Driver) Name() string { return d.name }
|
|
|
|
// SetName sets the name of the device.
|
|
func (d *Driver) SetName(n string) { d.name = n }
|
|
|
|
// Connection returns the Connection of the device.
|
|
func (d *Driver) Connection() gobot.Connection {
|
|
if conn, ok := d.connection.(gobot.Connection); ok {
|
|
return conn
|
|
}
|
|
|
|
log.Printf("%s has no gobot connection\n", d.name)
|
|
return nil
|
|
}
|
|
|
|
// Halt stops the driver.
|
|
func (d *Driver) Halt() error {
|
|
err := d.resetAll()
|
|
time.Sleep(10 * time.Millisecond)
|
|
return err
|
|
}
|
|
|
|
// Start initializes the GoPiGo3
|
|
func (d *Driver) Start() error {
|
|
bus := d.GetBusNumberOrDefault(d.connector.SpiDefaultBusNumber())
|
|
chip := d.GetChipNumberOrDefault(d.connector.SpiDefaultChipNumber())
|
|
mode := d.GetModeOrDefault(d.connector.SpiDefaultMode())
|
|
bits := d.GetBitCountOrDefault(d.connector.SpiDefaultBitCount())
|
|
maxSpeed := d.GetSpeedOrDefault(d.connector.SpiDefaultMaxSpeed())
|
|
|
|
var err error
|
|
d.connection, err = d.connector.GetSpiConnection(bus, chip, mode, bits, maxSpeed)
|
|
return err
|
|
}
|
|
|
|
// GetManufacturerName returns the manufacturer from the firmware.
|
|
func (d *Driver) GetManufacturerName() (string, error) {
|
|
// read 24 bytes to get manufacturer name
|
|
response, err := d.readBytes(goPiGo3Address, GET_MANUFACTURER, 24)
|
|
if err != nil {
|
|
return "", err
|
|
}
|
|
if err := d.responseValid(response); err != nil {
|
|
return "", err
|
|
}
|
|
mf := response[4:23]
|
|
mf = bytes.Trim(mf, "\x00")
|
|
return string(mf), nil
|
|
}
|
|
|
|
// GetBoardName returns the board name from the firmware.
|
|
func (d *Driver) GetBoardName() (string, error) {
|
|
// read 24 bytes to get board name
|
|
response, err := d.readBytes(goPiGo3Address, GET_NAME, 24)
|
|
if err != nil {
|
|
return "", err
|
|
}
|
|
if err := d.responseValid(response); err != nil {
|
|
return "", err
|
|
}
|
|
mf := response[4:23]
|
|
mf = bytes.Trim(mf, "\x00")
|
|
return string(mf), nil
|
|
}
|
|
|
|
// GetHardwareVersion returns the hardware version from the firmware.
|
|
func (d *Driver) GetHardwareVersion() (string, error) {
|
|
response, err := d.readUint32(goPiGo3Address, GET_HARDWARE_VERSION)
|
|
if err != nil {
|
|
return "", err
|
|
}
|
|
major := response / 1000000
|
|
minor := response / 1000 % 1000
|
|
patch := response % 1000
|
|
return fmt.Sprintf("%d.%d.%d", major, minor, patch), nil
|
|
}
|
|
|
|
// GetFirmwareVersion returns the current firmware version.
|
|
func (d *Driver) GetFirmwareVersion() (string, error) {
|
|
response, err := d.readUint32(goPiGo3Address, GET_FIRMWARE_VERSION)
|
|
if err != nil {
|
|
return "", err
|
|
}
|
|
major := response / 1000000
|
|
minor := response / 1000 % 1000
|
|
patch := response % 1000
|
|
return fmt.Sprintf("%d.%d.%d", major, minor, patch), nil
|
|
}
|
|
|
|
// GetSerialNumber returns the 128-bit hardware serial number of the board.
|
|
func (d *Driver) GetSerialNumber() (string, error) {
|
|
// read 20 bytes to get the serial number
|
|
response, err := d.readBytes(goPiGo3Address, GET_ID, 20)
|
|
if err != nil {
|
|
return "", err
|
|
}
|
|
if err := d.responseValid(response); err != nil {
|
|
return "", err
|
|
}
|
|
return fmt.Sprintf("%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", response[4], response[5],
|
|
response[6], response[7], response[8], response[9], response[10], response[11], response[12], response[13],
|
|
response[14], response[15], response[16], response[17], response[18], response[19]), nil
|
|
}
|
|
|
|
// Get5vVoltage returns the current voltage on the 5v line.
|
|
func (d *Driver) Get5vVoltage() (float32, error) {
|
|
val, err := d.readUint16(goPiGo3Address, GET_VOLTAGE_5V)
|
|
return (float32(val) / 1000.0), err
|
|
}
|
|
|
|
// GetBatteryVoltage gets the battery voltage from the main battery pack (7v-12v).
|
|
func (d *Driver) GetBatteryVoltage() (float32, error) {
|
|
val, err := d.readUint16(goPiGo3Address, GET_VOLTAGE_VCC)
|
|
return (float32(val) / 1000.0), err
|
|
}
|
|
|
|
// SetLED sets rgb values from 0 to 255.
|
|
func (d *Driver) SetLED(led Led, red, green, blue uint8) error {
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_LED,
|
|
byte(led),
|
|
red,
|
|
green,
|
|
blue,
|
|
})
|
|
}
|
|
|
|
// SetServo sets a servo's position in microseconds (0-16666).
|
|
func (d *Driver) SetServo(srvo Servo, us uint16) error {
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_SERVO,
|
|
byte(srvo),
|
|
byte((us >> 8) & 0xFF),
|
|
byte(us & 0xFF),
|
|
})
|
|
}
|
|
|
|
// ServoWrite writes an angle (0-180) to the given servo (servo 1 or servo 2).
|
|
// Must implement the ServoWriter interface of gpio package.
|
|
func (d *Driver) ServoWrite(port string, angle byte) error {
|
|
srvo := SERVO_1 // default for unknown ports
|
|
if port == "2" || port == "SERVO_2" {
|
|
srvo = SERVO_2
|
|
}
|
|
|
|
pulseWidthRange := 1850
|
|
if angle > 180 {
|
|
angle = 180
|
|
}
|
|
pulseWidth := ((1500 - (pulseWidthRange / 2)) + ((pulseWidthRange / 180) * int(angle)))
|
|
return d.SetServo(srvo, uint16(pulseWidth)) //nolint:gosec // TODO: fix later
|
|
}
|
|
|
|
// SetMotorPower sets a motor's power from -128 to 127.
|
|
func (d *Driver) SetMotorPower(motor Motor, power int8) error {
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_MOTOR_PWM,
|
|
byte(motor),
|
|
byte(power),
|
|
})
|
|
}
|
|
|
|
// SetMotorPosition sets the motor's position in degrees.
|
|
func (d *Driver) SetMotorPosition(motor Motor, position int) error {
|
|
positionRaw := position * MOTOR_TICKS_PER_DEGREE
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_MOTOR_POSITION,
|
|
byte(motor),
|
|
byte((positionRaw >> 24) & 0xFF),
|
|
byte((positionRaw >> 16) & 0xFF),
|
|
byte((positionRaw >> 8) & 0xFF),
|
|
byte(positionRaw & 0xFF),
|
|
})
|
|
}
|
|
|
|
// SetMotorDps sets the motor target speed in degrees per second.
|
|
func (d *Driver) SetMotorDps(motor Motor, dps int) error {
|
|
mdps := dps * MOTOR_TICKS_PER_DEGREE
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_MOTOR_DPS,
|
|
byte(motor),
|
|
byte((mdps >> 8) & 0xFF),
|
|
byte(mdps & 0xFF),
|
|
})
|
|
}
|
|
|
|
// SetMotorLimits sets the speed limits for a motor.
|
|
func (d *Driver) SetMotorLimits(motor Motor, power int8, dps int) error {
|
|
dpsUint := dps * MOTOR_TICKS_PER_DEGREE
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_MOTOR_LIMITS,
|
|
byte(motor),
|
|
byte(power),
|
|
byte((dpsUint >> 8) & 0xFF),
|
|
byte(dpsUint & 0xFF),
|
|
})
|
|
}
|
|
|
|
// GetMotorStatus returns the status for the given motor.
|
|
//
|
|
//nolint:nonamedreturns // sufficient here
|
|
func (d *Driver) GetMotorStatus(motor Motor) (flags uint8, power uint16, encoder, dps int, err error) {
|
|
message := GET_MOTOR_STATUS_RIGHT
|
|
if motor == MOTOR_LEFT {
|
|
message = GET_MOTOR_STATUS_LEFT
|
|
}
|
|
response, err := d.readBytes(goPiGo3Address, message, 12)
|
|
if err != nil {
|
|
return flags, power, encoder, dps, err
|
|
}
|
|
if err := d.responseValid(response); err != nil {
|
|
return flags, power, encoder, dps, err
|
|
}
|
|
// get flags
|
|
flags = response[4]
|
|
// get power
|
|
power = uint16(response[5])
|
|
if power&0x80 == 0x80 {
|
|
power = power - 0x100
|
|
}
|
|
// get encoder
|
|
enc := make([]byte, 4)
|
|
enc[3] = response[6]
|
|
enc[2] = response[7]
|
|
enc[1] = response[8]
|
|
enc[0] = response[9]
|
|
e := binary.LittleEndian.Uint32(enc)
|
|
encoder = int(e)
|
|
if e&0x80000000 == 0x80000000 {
|
|
encoder = int(uint64(e) - 0x100000000) //nolint:gosec // TODO: fix later
|
|
}
|
|
// get dps
|
|
dpsRaw := make([]byte, 4)
|
|
dpsRaw[1] = response[10]
|
|
dpsRaw[0] = response[11]
|
|
ds := binary.LittleEndian.Uint32(dpsRaw)
|
|
dps = int(ds)
|
|
if ds&0x8000 == 0x8000 {
|
|
dps = int(ds - 0x10000)
|
|
}
|
|
return flags, power, encoder / MOTOR_TICKS_PER_DEGREE, dps / MOTOR_TICKS_PER_DEGREE, nil
|
|
}
|
|
|
|
// GetMotorEncoder reads a motor's encoder in degrees.
|
|
func (d *Driver) GetMotorEncoder(motor Motor) (int64, error) {
|
|
message := GET_MOTOR_ENCODER_RIGHT
|
|
if motor == MOTOR_LEFT {
|
|
message = GET_MOTOR_ENCODER_LEFT
|
|
}
|
|
response, err := d.readUint32(goPiGo3Address, message)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
encoder := int64(response)
|
|
if response&0x80000000 != 0 {
|
|
encoder = encoder - 0x100000000
|
|
}
|
|
encoder = encoder / MOTOR_TICKS_PER_DEGREE
|
|
return encoder, nil
|
|
}
|
|
|
|
// OffsetMotorEncoder offsets a motor's encoder for calibration purposes.
|
|
func (d *Driver) OffsetMotorEncoder(motor Motor, offset float64) error {
|
|
offsetUint := math.Float64bits(offset * MOTOR_TICKS_PER_DEGREE)
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
OFFSET_MOTOR_ENCODER,
|
|
byte(motor),
|
|
byte((offsetUint >> 24) & 0xFF),
|
|
byte((offsetUint >> 16) & 0xFF),
|
|
byte((offsetUint >> 8) & 0xFF),
|
|
byte(offsetUint & 0xFF),
|
|
})
|
|
}
|
|
|
|
// SetGroveType sets the given port to a grove device type.
|
|
func (d *Driver) SetGroveType(port Grove, gType GroveType) error {
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_GROVE_TYPE,
|
|
byte(port),
|
|
byte(gType),
|
|
})
|
|
}
|
|
|
|
// SetGroveMode sets the mode a given pin/port of the grove connector.
|
|
func (d *Driver) SetGroveMode(port Grove, mode GroveMode) error {
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_GROVE_MODE,
|
|
byte(port),
|
|
byte(mode),
|
|
})
|
|
}
|
|
|
|
// SetPWMDuty sets the pwm duty cycle for the given pin/port.
|
|
func (d *Driver) SetPWMDuty(port Grove, duty uint16) error {
|
|
if duty > 100 {
|
|
duty = 100
|
|
}
|
|
duty = duty * 10
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_GROVE_PWM_DUTY,
|
|
byte(port),
|
|
byte((duty >> 8) & 0xFF),
|
|
byte(duty & 0xFF),
|
|
})
|
|
}
|
|
|
|
// SetPWMFreq setst the pwm frequency for the given pin/port.
|
|
func (d *Driver) SetPWMFreq(port Grove, freq uint16) error {
|
|
if freq < 3 {
|
|
freq = 3
|
|
}
|
|
if freq > 48000 {
|
|
freq = 48000
|
|
}
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_GROVE_PWM_FREQUENCY,
|
|
byte(port),
|
|
byte((freq >> 8) & 0xFF),
|
|
byte(freq & 0xFF),
|
|
})
|
|
}
|
|
|
|
// PwmWrite implents the pwm interface for the gopigo3.
|
|
func (d *Driver) PwmWrite(pin string, val byte) error {
|
|
var (
|
|
grovePin, grovePort Grove
|
|
err error
|
|
)
|
|
if grovePin, grovePort, _, _, err = getGroveAddresses(pin); err != nil {
|
|
return err
|
|
}
|
|
if err := d.SetGroveType(grovePort, CUSTOM); err != nil {
|
|
return err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
if err = d.SetGroveMode(grovePin, GROVE_OUTPUT_PWM); err != nil {
|
|
return err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
if err = d.SetPWMFreq(grovePin, 24000); err != nil {
|
|
return err
|
|
}
|
|
val64 := math.Float64frombits(uint64(val))
|
|
dutyCycle := uint16(math.Float64bits((100.0 / 255.0) * val64)) //nolint:gosec // TODO: fix later
|
|
return d.SetPWMDuty(grovePin, dutyCycle)
|
|
}
|
|
|
|
// AnalogRead returns the analog value of the given pin.
|
|
func (d *Driver) AnalogRead(pin string) (int, error) {
|
|
grovePin, grovePort, analogCmd, _, err := getGroveAddresses(pin)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
if err := d.SetGroveType(grovePort, CUSTOM); err != nil {
|
|
return 0, err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
if err := d.SetGroveMode(grovePin, GROVE_INPUT_ANALOG); err != nil {
|
|
return 0, err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
response, err := d.readBytes(goPiGo3Address, analogCmd, 7)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
if err := d.responseValid(response); err != nil {
|
|
return 0, err
|
|
}
|
|
if err := d.valueValid(response); err != nil {
|
|
return 0, err
|
|
}
|
|
highBytes := uint16(response[5])
|
|
lowBytes := uint16(response[6])
|
|
return int((highBytes<<8)&0xFF00) | int(lowBytes&0xFF), nil
|
|
}
|
|
|
|
// DigitalWrite writes a 0/1 value to the given pin.
|
|
func (d *Driver) DigitalWrite(pin string, val byte) error {
|
|
var (
|
|
grovePin, grovePort Grove
|
|
err error
|
|
)
|
|
grovePin, grovePort, _, _, err = getGroveAddresses(pin)
|
|
if err != nil {
|
|
return err
|
|
}
|
|
if err := d.SetGroveType(grovePort, CUSTOM); err != nil {
|
|
return err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
if err := d.SetGroveMode(grovePin, GROVE_OUTPUT_DIGITAL); err != nil {
|
|
return err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
return d.writeBytes([]byte{
|
|
goPiGo3Address,
|
|
SET_GROVE_STATE,
|
|
byte(grovePin),
|
|
val,
|
|
})
|
|
}
|
|
|
|
// DigitalRead reads the 0/1 value from the given pin.
|
|
func (d *Driver) DigitalRead(pin string) (int, error) {
|
|
grovePin, grovePort, _, stateCmd, err := getGroveAddresses(pin)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
err = d.SetGroveType(grovePort, CUSTOM)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
err = d.SetGroveMode(grovePin, GROVE_INPUT_DIGITAL)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
response, err := d.readBytes(goPiGo3Address, stateCmd, 6)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
if err := d.responseValid(response); err != nil {
|
|
return 0, err
|
|
}
|
|
if err := d.valueValid(response); err != nil {
|
|
return 0, err
|
|
}
|
|
return int(response[5]), nil
|
|
}
|
|
|
|
//nolint:nonamedreturns // sufficient here
|
|
func getGroveAddresses(pin string) (gPin, gPort Grove, analog, state byte, err error) {
|
|
switch pin {
|
|
case "AD_1_1":
|
|
gPin = AD_1_1_G
|
|
gPort = AD_1_G
|
|
analog = GET_GROVE_ANALOG_1_1
|
|
state = GET_GROVE_STATE_1_1
|
|
case "AD_1_2":
|
|
gPin = AD_1_2_G
|
|
gPort = AD_1_G
|
|
analog = GET_GROVE_ANALOG_1_2
|
|
state = GET_GROVE_STATE_1_2
|
|
case "AD_2_1":
|
|
gPin = AD_2_1_G
|
|
gPort = AD_2_G
|
|
analog = GET_GROVE_ANALOG_2_1
|
|
state = GET_GROVE_STATE_2_1
|
|
case "AD_2_2":
|
|
gPin = AD_2_2_G
|
|
gPort = AD_2_G
|
|
analog = GET_GROVE_ANALOG_2_2
|
|
state = GET_GROVE_STATE_2_2
|
|
default:
|
|
err = fmt.Errorf("Invalid grove pin name")
|
|
}
|
|
return gPin, gPort, analog, state, err
|
|
}
|
|
|
|
func (d *Driver) responseValid(response []byte) error {
|
|
if response[3] != 0xA5 {
|
|
return fmt.Errorf("No SPI response, response not valid")
|
|
}
|
|
return nil
|
|
}
|
|
|
|
func (d *Driver) valueValid(value []byte) error {
|
|
if value[4] != byte(VALID_DATA) {
|
|
return fmt.Errorf("Invalid value")
|
|
}
|
|
return nil
|
|
}
|
|
|
|
func (d *Driver) readBytes(address byte, msg byte, numBytes int) ([]byte, error) {
|
|
w := make([]byte, numBytes)
|
|
w[0] = address
|
|
w[1] = msg
|
|
r := make([]byte, len(w))
|
|
if err := d.connection.ReadCommandData(w, r); err != nil {
|
|
return nil, err
|
|
}
|
|
return r, nil
|
|
}
|
|
|
|
func (d *Driver) readUint16(address, msg byte) (uint16, error) {
|
|
r, err := d.readBytes(address, msg, 8)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
if err := d.responseValid(r); err != nil {
|
|
return 0, err
|
|
}
|
|
return uint16(r[4])<<8 | uint16(r[5]), nil
|
|
}
|
|
|
|
func (d *Driver) readUint32(address, msg byte) (uint32, error) {
|
|
r, err := d.readBytes(address, msg, 8)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
if err := d.responseValid(r); err != nil {
|
|
return 0, err
|
|
}
|
|
return uint32(r[4])<<24 | uint32(r[5])<<16 | uint32(r[6])<<8 | uint32(r[7]), nil
|
|
}
|
|
|
|
func (d *Driver) writeBytes(w []byte) error {
|
|
return d.connection.WriteBytes(w)
|
|
}
|
|
|
|
func (d *Driver) resetAll() error {
|
|
err := d.SetGroveType(AD_1_G+AD_2_G, CUSTOM)
|
|
time.Sleep(10 * time.Millisecond)
|
|
if e := d.SetGroveMode(AD_1_G+AD_2_G, GROVE_INPUT_DIGITAL); e != nil {
|
|
err = multierror.Append(err, e)
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
if e := d.SetMotorPower(MOTOR_LEFT+MOTOR_RIGHT, 0.0); e != nil {
|
|
err = multierror.Append(err, e)
|
|
}
|
|
if e := d.SetMotorLimits(MOTOR_LEFT+MOTOR_RIGHT, 0, 0); e != nil {
|
|
err = multierror.Append(err, e)
|
|
}
|
|
if e := d.SetServo(SERVO_1+SERVO_2, 0); e != nil {
|
|
err = multierror.Append(err, e)
|
|
}
|
|
if e := d.SetLED(LED_EYE_LEFT+LED_EYE_RIGHT+LED_BLINKER_LEFT+LED_BLINKER_RIGHT, 0, 0, 0); e != nil {
|
|
err = multierror.Append(err, e)
|
|
}
|
|
|
|
return err
|
|
}
|