131 lines
3.0 KiB
Go
131 lines
3.0 KiB
Go
package megapi
|
|
|
|
import (
|
|
"bytes"
|
|
"encoding/binary"
|
|
"log"
|
|
"sync"
|
|
"time"
|
|
|
|
"gobot.io/x/gobot/v2"
|
|
"gobot.io/x/gobot/v2/drivers/serial"
|
|
)
|
|
|
|
var _ gobot.Driver = (*MotorDriver)(nil)
|
|
|
|
type megapiMotorSerialAdaptor interface {
|
|
gobot.Adaptor
|
|
serial.SerialWriter
|
|
}
|
|
|
|
// MotorDriver represents a motor
|
|
type MotorDriver struct {
|
|
*serial.Driver
|
|
port byte
|
|
halted bool
|
|
writeBytesChannel chan []byte
|
|
finalizeChannel chan struct{}
|
|
syncRoot *sync.Mutex
|
|
}
|
|
|
|
// NewMotorDriver creates a new MotorDriver at the given port
|
|
func NewMotorDriver(a megapiMotorSerialAdaptor, port byte, opts ...serial.OptionApplier) *MotorDriver {
|
|
d := &MotorDriver{
|
|
port: port,
|
|
halted: true,
|
|
syncRoot: &sync.Mutex{},
|
|
writeBytesChannel: make(chan []byte),
|
|
finalizeChannel: make(chan struct{}),
|
|
}
|
|
d.Driver = serial.NewDriver(a, "MegaPiMotor", d.initialize, d.shutdown, opts...)
|
|
|
|
return d
|
|
}
|
|
|
|
// Speed sets the motors speed to the specified value
|
|
func (d *MotorDriver) Speed(speed int16) error {
|
|
d.syncRoot.Lock()
|
|
defer d.syncRoot.Unlock()
|
|
|
|
if d.halted {
|
|
return nil
|
|
}
|
|
return d.speedHelper(speed)
|
|
}
|
|
|
|
// initialize implements the Driver interface
|
|
func (d *MotorDriver) initialize() error {
|
|
d.syncRoot.Lock()
|
|
defer d.syncRoot.Unlock()
|
|
|
|
// sleeping is required to give the board a chance to reset after connection is done
|
|
time.Sleep(2 * time.Second)
|
|
|
|
// kick off thread to send bytes to the board
|
|
go func() {
|
|
for {
|
|
select {
|
|
case bytes := <-d.writeBytesChannel:
|
|
if _, err := d.adaptor().SerialWrite(bytes); err != nil {
|
|
panic(err)
|
|
}
|
|
time.Sleep(10 * time.Millisecond)
|
|
case <-d.finalizeChannel:
|
|
d.finalizeChannel <- struct{}{}
|
|
return
|
|
default:
|
|
time.Sleep(10 * time.Millisecond)
|
|
}
|
|
}
|
|
}()
|
|
|
|
d.halted = false
|
|
return d.speedHelper(0)
|
|
}
|
|
|
|
// Halt terminates the Driver interface
|
|
func (d *MotorDriver) shutdown() error {
|
|
d.syncRoot.Lock()
|
|
defer d.syncRoot.Unlock()
|
|
|
|
d.finalizeChannel <- struct{}{}
|
|
<-d.finalizeChannel
|
|
|
|
d.halted = true
|
|
return d.speedHelper(0)
|
|
}
|
|
|
|
// there is some sort of bug on the hardware such that you cannot
|
|
// send the exact same speed to 2 different motors consecutively
|
|
// hence we ensure we always alternate speeds
|
|
func (d *MotorDriver) speedHelper(speed int16) error {
|
|
if err := d.sendSpeed(speed - 1); err != nil {
|
|
return err
|
|
}
|
|
return d.sendSpeed(speed)
|
|
}
|
|
|
|
// sendSpeed sets the motors speed to the specified value
|
|
func (d *MotorDriver) sendSpeed(speed int16) error {
|
|
bufOut := new(bytes.Buffer)
|
|
|
|
// byte sequence: 0xff, 0x55, id, action, device, port
|
|
bufOut.Write([]byte{0xff, 0x55, 0x6, 0x0, 0x2, 0xa, d.port})
|
|
if err := binary.Write(bufOut, binary.LittleEndian, speed); err != nil {
|
|
return err
|
|
}
|
|
bufOut.Write([]byte{0xa})
|
|
d.writeBytesChannel <- bufOut.Bytes()
|
|
|
|
return nil
|
|
}
|
|
|
|
func (d *MotorDriver) adaptor() megapiMotorSerialAdaptor {
|
|
if a, ok := d.Connection().(megapiMotorSerialAdaptor); ok {
|
|
return a
|
|
}
|
|
|
|
log.Printf("%s has no MegaPi serial connector\n", d.Name())
|
|
return nil
|
|
}
|