gobot.io/x/gobot/v2@v2.1.0/platforms/megapi/motor_driver.go (about)

     1  package megapi
     2  
     3  import (
     4  	"bytes"
     5  	"encoding/binary"
     6  	"sync"
     7  
     8  	"gobot.io/x/gobot/v2"
     9  )
    10  
    11  var _ gobot.Driver = (*MotorDriver)(nil)
    12  
    13  // MotorDriver represents a motor
    14  type MotorDriver struct {
    15  	name     string
    16  	megaPi   *Adaptor
    17  	port     byte
    18  	halted   bool
    19  	syncRoot *sync.Mutex
    20  }
    21  
    22  // NewMotorDriver creates a new MotorDriver at the given port
    23  func NewMotorDriver(megaPi *Adaptor, port byte) *MotorDriver {
    24  	return &MotorDriver{
    25  		name:     "MegaPiMotor",
    26  		megaPi:   megaPi,
    27  		port:     port,
    28  		halted:   true,
    29  		syncRoot: &sync.Mutex{},
    30  	}
    31  }
    32  
    33  // Name returns the name of this motor
    34  func (m *MotorDriver) Name() string {
    35  	return m.name
    36  }
    37  
    38  // SetName sets the name of this motor
    39  func (m *MotorDriver) SetName(n string) {
    40  	m.name = n
    41  }
    42  
    43  // Start implements the Driver interface
    44  func (m *MotorDriver) Start() error {
    45  	m.syncRoot.Lock()
    46  	defer m.syncRoot.Unlock()
    47  	m.halted = false
    48  	m.speedHelper(0)
    49  	return nil
    50  }
    51  
    52  // Halt terminates the Driver interface
    53  func (m *MotorDriver) Halt() error {
    54  	m.syncRoot.Lock()
    55  	defer m.syncRoot.Unlock()
    56  	m.halted = true
    57  	m.speedHelper(0)
    58  	return nil
    59  }
    60  
    61  // Connection returns the Connection associated with the Driver
    62  func (m *MotorDriver) Connection() gobot.Connection {
    63  	return gobot.Connection(m.megaPi)
    64  }
    65  
    66  // Speed sets the motors speed to the specified value
    67  func (m *MotorDriver) Speed(speed int16) error {
    68  	m.syncRoot.Lock()
    69  	defer m.syncRoot.Unlock()
    70  	if m.halted {
    71  		return nil
    72  	}
    73  	m.speedHelper(speed)
    74  	return nil
    75  }
    76  
    77  // there is some sort of bug on the hardware such that you cannot
    78  // send the exact same speed to 2 different motors consecutively
    79  // hence we ensure we always alternate speeds
    80  func (m *MotorDriver) speedHelper(speed int16) {
    81  	m.sendSpeed(speed - 1)
    82  	m.sendSpeed(speed)
    83  }
    84  
    85  // sendSpeed sets the motors speed to the specified value
    86  func (m *MotorDriver) sendSpeed(speed int16) {
    87  	bufOut := new(bytes.Buffer)
    88  
    89  	// byte sequence: 0xff, 0x55, id, action, device, port
    90  	bufOut.Write([]byte{0xff, 0x55, 0x6, 0x0, 0x2, 0xa, m.port})
    91  	binary.Write(bufOut, binary.LittleEndian, speed)
    92  	bufOut.Write([]byte{0xa})
    93  	m.megaPi.writeBytesChannel <- bufOut.Bytes()
    94  }