legop3/MultiRoombaRover
1
0
Fork 0
mirror of https://github.com/legop3/MultiRoombaRover.git synced 2026-09-16 09:31:20 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity

ugh

Browse Source
This commit is contained in:
legop3
2026-01-15 12:15:51 -05:00
parent 6c01087738
commit 655fed4326
5 changed files with 184 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pi/roverd/camera_servo.go
+44 -10
View File
@@ -15,6 +15,8 @@ type CameraServo struct {
cfg CameraServoConfig
logger *log.Logger
pin rpio.Pin
pigpio *pigpioClient
usePigpio bool
mu sync.Mutex
currentAngle float64
closed bool
@@ -24,6 +26,30 @@ func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, er
if !cfg.Enabled {
return nil, fmt.Errorf("camera servo disabled")
}
servo := &CameraServo{
cfg: cfg,
logger: logger,
}
if client, err := newPigpioClient(defaultPigpioAddr); err == nil {
if err := setPigpioMode(client, cfg.Pin, piOutput); err == nil {
servo.pigpio = client
servo.usePigpio = true
} else {
_ = client.Close()
}
}
if servo.usePigpio {
if err := servo.setAngleLocked(cfg.HomeAngle); err != nil {
_ = servo.pigpio.Close()
return nil, err
}
logger.Printf("camera servo initialized on GPIO %d (pigpio, %.1f..%.1f deg, %d..%d us, invert=%v)", cfg.Pin, cfg.MinAngle, cfg.MaxAngle, cfg.MinPulseUs, cfg.MaxPulseUs, cfg.Invert)
return servo, nil
}
if err := rpio.Open(); err != nil {
return nil, fmt.Errorf("open gpio: %w", err)
}
@@ -32,12 +58,8 @@ func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, er
pin.Mode(rpio.Pwm)
targetClock := cfg.FreqHz * cfg.CycleLen
pin.Freq(targetClock)
servo.pin = pin
servo := &CameraServo{
cfg: cfg,
logger: logger,
pin: pin,
}
if err := servo.setAngleLocked(cfg.HomeAngle); err != nil {
rpio.Close()
return nil, err
@@ -52,8 +74,12 @@ func (s *CameraServo) Close() {
if s.closed {
return
}
s.applyPulseLocked(s.angleToPulse(s.cfg.HomeAngle))
rpio.Close()
_ = s.applyPulseLocked(s.angleToPulse(s.cfg.HomeAngle))
if s.usePigpio {
_ = s.pigpio.Close()
} else {
rpio.Close()
}
s.closed = true
}
@@ -68,7 +94,9 @@ func (s *CameraServo) setAngleLocked(angle float64) error {
return fmt.Errorf("servo closed")
}
clamped := clampFloat(angle, s.cfg.MinAngle, s.cfg.MaxAngle)
s.applyPulseLocked(s.angleToPulse(clamped))
if err := s.applyPulseLocked(s.angleToPulse(clamped)); err != nil {
return err
}
s.currentAngle = clamped
return nil
}
@@ -95,7 +123,9 @@ func (s *CameraServo) SetPulseWidth(micros int) error {
if micros <= 0 {
return fmt.Errorf("pulse width must be > 0")
}
s.applyPulseLocked(micros)
if err := s.applyPulseLocked(micros); err != nil {
return err
}
s.currentAngle = s.pulseToAngle(micros)
return nil
}
@@ -106,9 +136,13 @@ func (s *CameraServo) CurrentAngle() float64 {
return s.currentAngle
}
func (s *CameraServo) applyPulseLocked(micros int) {
func (s *CameraServo) applyPulseLocked(micros int) error {
micros = clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs)
if s.usePigpio {
return setPigpioServo(s.pigpio, s.cfg.Pin, micros)
}
s.pin.DutyCycle(uint32(micros), uint32(s.cfg.CycleLen))
return nil
}
func (s *CameraServo) angleToPulse(angle float64) int {
pi/roverd/ir_tx.go
+6 -108
View File
@@ -6,88 +6,17 @@ import (
"bytes"
"encoding/binary"
"fmt"
"io"
"log"
"net"
"sync"
"time"
)
const (
piCmdSetMode = 0
piCmdWrite = 4
piCmdWaveClear = 27
piCmdWaveAddGeneric = 28
piCmdWaveTxBusy = 32
piCmdWaveCreate = 49
piCmdWaveDelete = 50
piCmdWaveTxSend = 51
piOutput = 1
pigpioConnectRetries = 20
pigpioConnectDelay = 250 * time.Millisecond
)
type pigpioCmd struct {
Cmd uint32
P1 uint32
P2 uint32
P3 uint32
}
type gpioPulse struct {
GpioOn uint32
GpioOff uint32
DelayUs uint32
}
type pigpioClient struct {
conn net.Conn
mu sync.Mutex
}
func newPigpioClient(addr string) (*pigpioClient, error) {
conn, err := net.Dial("tcp", addr)
if err != nil {
return nil, err
}
if tcpConn, ok := conn.(*net.TCPConn); ok {
_ = tcpConn.SetNoDelay(true)
}
return &pigpioClient{conn: conn}, nil
}
func (c *pigpioClient) Close() error {
if c.conn == nil {
return nil
}
return c.conn.Close()
}
func (c *pigpioClient) command(cmd, p1, p2, p3 uint32, ext []byte) (int32, error) {
c.mu.Lock()
defer c.mu.Unlock()
var buf [16]byte
binary.LittleEndian.PutUint32(buf[0:], cmd)
binary.LittleEndian.PutUint32(buf[4:], p1)
binary.LittleEndian.PutUint32(buf[8:], p2)
binary.LittleEndian.PutUint32(buf[12:], p3)
if _, err := c.conn.Write(buf[:]); err != nil {
return -1, err
}
if len(ext) > 0 {
if _, err := c.conn.Write(ext); err != nil {
return -1, err
}
}
if _, err := io.ReadFull(c.conn, buf[:]); err != nil {
return -1, err
}
res := int32(binary.LittleEndian.Uint32(buf[12:]))
return res, nil
}
type IRTransmitter struct {
cfg IRConfig
logger *log.Logger
@@ -103,10 +32,7 @@ func NewIRTransmitter(cfg IRConfig, logger *log.Logger) (*IRTransmitter, error)
return nil, fmt.Errorf("ir disabled")
}
addr := cfg.PigpioAddr
if addr == "" {
addr = "localhost:8888"
}
addr := ensurePigpioAddr(cfg.PigpioAddr)
client, err := connectPigpioWithRetry(addr, logger)
if err != nil {
return nil, fmt.Errorf("connect pigpio: %w", err)
@@ -129,22 +55,6 @@ func NewIRTransmitter(cfg IRConfig, logger *log.Logger) (*IRTransmitter, error)
return tx, nil
}
func connectPigpioWithRetry(addr string, logger *log.Logger) (*pigpioClient, error) {
var lastErr error
for attempt := 1; attempt <= pigpioConnectRetries; attempt++ {
client, err := newPigpioClient(addr)
if err == nil {
return client, nil
}
lastErr = err
if attempt == 1 || attempt%4 == 0 {
logger.Printf("pigpio connect attempt %d/%d failed: %v", attempt, pigpioConnectRetries, err)
}
time.Sleep(pigpioConnectDelay)
}
return nil, lastErr
}
func (t *IRTransmitter) Close() {
t.mu.Lock()
defer t.mu.Unlock()
@@ -169,17 +79,12 @@ func (t *IRTransmitter) Send(code byte, repeat int) error {
if len(pulses) == 0 {
return nil
}
if err := t.writeWave(pulses, time.Duration(totalUs)*time.Microsecond); err != nil {
return err
}
return nil
return t.writeWave(pulses, time.Duration(totalUs)*time.Microsecond)
}
func (t *IRTransmitter) configureLine() error {
if res, err := t.pigpio.command(piCmdSetMode, uint32(t.cfg.Pin), piOutput, 0, nil); err != nil {
return fmt.Errorf("pigpio set mode: %w", err)
} else if res < 0 {
return fmt.Errorf("pigpio set mode: %d", res)
if err := setPigpioMode(t.pigpio, t.cfg.Pin, piOutput); err != nil {
return err
}
return t.setInactive()
}
@@ -189,14 +94,7 @@ func (t *IRTransmitter) setInactive() error {
if t.activeLow {
level = 1
}
res, err := t.pigpio.command(piCmdWrite, uint32(t.cfg.Pin), level, 0, nil)
if err != nil {
return fmt.Errorf("pigpio write: %w", err)
}
if res < 0 {
return fmt.Errorf("pigpio write: %d", res)
}
return nil
return writePigpio(t.pigpio, t.cfg.Pin, level)
}
func (t *IRTransmitter) buildWaveform(code byte, repeat int) ([]gpioPulse, int) {
@@ -303,7 +201,7 @@ func (t *IRTransmitter) writeWave(pulses []gpioPulse, duration time.Duration) er
_ = binary.Write(buf, binary.LittleEndian, pulse.DelayUs)
}
data := buf.Bytes()
if res, err := t.pigpio.command(piCmdWaveAddGeneric, 0, 0, uint32(len(data)), data); err != nil {
if res, err := t.pigpio.command(piCmdWaveAdd, 0, 0, uint32(len(data)), data); err != nil {
return fmt.Errorf("pigpio wave add: %w", err)
} else if res < 0 {
return fmt.Errorf("pigpio wave add: %d", res)
pi/roverd/pigpio_client.go
+134
View File
@@ -0,0 +1,134 @@
//go:build !dummy
package roverd
import (
"encoding/binary"
"fmt"
"io"
"log"
"net"
"sync"
"time"
)
const (
defaultPigpioAddr = "localhost:8888"
piCmdSetMode = 0
piCmdWrite = 4
piCmdServo = 8
piCmdWaveClear = 27
piCmdWaveAdd = 28
piCmdWaveTxBusy = 32
piCmdWaveCreate = 49
piCmdWaveDelete = 50
piCmdWaveTxSend = 51
piOutput = 1
pigpioConnectRetries = 20
pigpioConnectDelay = 250 * time.Millisecond
)
type pigpioClient struct {
conn net.Conn
mu sync.Mutex
}
func newPigpioClient(addr string) (*pigpioClient, error) {
conn, err := net.Dial("tcp", addr)
if err != nil {
return nil, err
}
if tcpConn, ok := conn.(*net.TCPConn); ok {
_ = tcpConn.SetNoDelay(true)
}
return &pigpioClient{conn: conn}, nil
}
func connectPigpioWithRetry(addr string, logger *log.Logger) (*pigpioClient, error) {
var lastErr error
for attempt := 1; attempt <= pigpioConnectRetries; attempt++ {
client, err := newPigpioClient(addr)
if err == nil {
return client, nil
}
lastErr = err
if attempt == 1 || attempt%4 == 0 {
logger.Printf("pigpio connect attempt %d/%d failed: %v", attempt, pigpioConnectRetries, err)
}
time.Sleep(pigpioConnectDelay)
}
return nil, lastErr
}
func (c *pigpioClient) Close() error {
if c.conn == nil {
return nil
}
return c.conn.Close()
}
func (c *pigpioClient) command(cmd, p1, p2, p3 uint32, ext []byte) (int32, error) {
c.mu.Lock()
defer c.mu.Unlock()
var buf [16]byte
binary.LittleEndian.PutUint32(buf[0:], cmd)
binary.LittleEndian.PutUint32(buf[4:], p1)
binary.LittleEndian.PutUint32(buf[8:], p2)
binary.LittleEndian.PutUint32(buf[12:], p3)
if _, err := c.conn.Write(buf[:]); err != nil {
return -1, err
}
if len(ext) > 0 {
if _, err := c.conn.Write(ext); err != nil {
return -1, err
}
}
if _, err := io.ReadFull(c.conn, buf[:]); err != nil {
return -1, err
}
res := int32(binary.LittleEndian.Uint32(buf[12:]))
return res, nil
}
func ensurePigpioAddr(addr string) string {
if addr != "" {
return addr
}
return defaultPigpioAddr
}
func setPigpioMode(client *pigpioClient, pin int, mode uint32) error {
res, err := client.command(piCmdSetMode, uint32(pin), mode, 0, nil)
if err != nil {
return fmt.Errorf("pigpio set mode: %w", err)
}
if res < 0 {
return fmt.Errorf("pigpio set mode: %d", res)
}
return nil
}
func writePigpio(client *pigpioClient, pin int, level uint32) error {
res, err := client.command(piCmdWrite, uint32(pin), level, 0, nil)
if err != nil {
return fmt.Errorf("pigpio write: %w", err)
}
if res < 0 {
return fmt.Errorf("pigpio write: %d", res)
}
return nil
}
func setPigpioServo(client *pigpioClient, pin int, pulseWidth int) error {
res, err := client.command(piCmdServo, uint32(pin), uint32(pulseWidth), 0, nil)
if err != nil {
return fmt.Errorf("pigpio servo: %w", err)
}
if res < 0 {
return fmt.Errorf("pigpio servo: %d", res)
}
return nil
}