robocars/robocar-arduino
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

First implementation

Browse Source
This commit is contained in:
Cyrille Nofficial
2019-12-01 22:35:19 +01:00
parent 832459ee7c
commit bdba5127bd
369 changed files with 240807 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
arduino/arduino.go Normal file
View File

@@ -0,0 +1,190 @@
package arduino
import (
"bufio"
"github.com/cyrilix/robocar-base/mode"
"github.com/cyrilix/robocar-base/mqttdevice"
"github.com/tarm/serial"
"io"
"log"
"regexp"
"strconv"
"strings"
"sync"
"time"
)
const (
MinPwmAngle = 960.0
MaxPwmAngle = 1980.0
MinPwmThrottle = 972.0
MaxPwmThrottle = 1954.0
)
var (
serialLineRegex = regexp.MustCompile(`(?P<timestamp>\d+),(?P<channel_1>\d+),(?P<channel_2>\d+),(?P<channel_3>\d+),(?P<channel_4>\d+),(?P<channel_5>\d+),(?P<channel_6>\d+),(?P<frequency>\d+),(?P<distance_cm>\d+)?`)
)
type ArduinoPart struct {
pub mqttdevice.Publisher
topicBase string
pubFrequency float64
serial io.Reader
mutex sync.Mutex
steering float32
throttle float32
distanceCm int
ctrlRecord bool
driveMode mode.DriveMode
}
func NewArduinoPart(name string, baud int, pub mqttdevice.Publisher, topicBase string, pubFrequency float64) *ArduinoPart {
c := &serial.Config{Name: name, Baud: baud}
s, err := serial.OpenPort(c)
if err != nil {
log.Panicf("unable to open serial port: %v", err)
}
return &ArduinoPart{serial: s, pub: pub, topicBase: topicBase, pubFrequency: pubFrequency}
}
func (a *ArduinoPart) Start() {
go a.publishLoop()
for {
buff := bufio.NewReader(a.serial)
line, err := buff.ReadString('\n')
if err == io.EOF || line == "" {
log.Println("remote connection closed")
break
}
if ! serialLineRegex.MatchString(line) {
log.Printf("invalid line: '%v'", line)
continue
}
values := strings.Split(strings.TrimSuffix(strings.TrimSuffix(line, "\n"), "\r"), ",")
a.updateValues(values)
}
}
func (a *ArduinoPart) updateValues(values []string) {
a.mutex.Lock()
defer a.mutex.Unlock()
a.processChannel1(&values[1])
a.processChannel2(&values[2])
a.processChannel3(&values[3])
a.processChannel4(&values[4])
a.processChannel5(&values[5])
a.processChannel6(&values[6])
a.processDistanceCm(&values[8])
}
func (a *ArduinoPart) Stop() {
log.Printf("Stop ArduinoPart")
switch s := a.serial.(type) {
case io.ReadCloser:
if err := s.Close(); err != nil {
log.Fatalf("unable to close serial port: %v", err)
}
}
}
func (a *ArduinoPart) processChannel1(v *string) {
value, err := strconv.Atoi(*v)
if err != nil {
log.Printf("invalid value for channel1, should be an int: %v", err)
}
if value < MinPwmAngle {
value = MinPwmAngle
} else if value > MaxPwmAngle {
value = MaxPwmAngle
}
a.steering = ((float32(value)-MinPwmAngle)/(MaxPwmAngle-MinPwmAngle))*2.0 - 1.0
}
func (a *ArduinoPart) processChannel2(v *string) {
value, err := strconv.Atoi(*v)
if err != nil {
log.Printf("invalid value for channel2, should be an int: %v", err)
}
if value < MinPwmThrottle {
value = MinPwmThrottle
} else if value > MaxPwmThrottle {
value = MaxPwmThrottle
}
a.throttle = ((float32(value)-MinPwmThrottle)/(MaxPwmThrottle-MinPwmThrottle))*2.0 - 1.0
}
func (a *ArduinoPart) processChannel3(values *string) {
}
func (a *ArduinoPart) processChannel4(values *string) {
}
func (a *ArduinoPart) processChannel5(v *string) {
value, err := strconv.Atoi(*v)
if err != nil {
log.Printf("invalid value for channel5, should be an int: %v", err)
}
if value < 1800 {
if ! a.ctrlRecord {
log.Printf("Update channel 5 with value %v, record: %v", true, false)
a.ctrlRecord = true
}
} else {
if a.ctrlRecord {
log.Printf("Update channel 5 with value %v, record: %v", false, true)
a.ctrlRecord = false
}
}
}
func (a *ArduinoPart) processChannel6(v *string) {
value, err := strconv.Atoi(*v)
if err != nil {
log.Printf("invalid value for channel6, should be an int: %v", err)
return
}
if value > 1800 {
if a.driveMode != mode.DriveModePilot {
log.Printf("Update channel 6 with value %v, new user_mode: %v", value, mode.DriveModeUser)
a.driveMode = mode.DriveModePilot
}
} else {
if a.driveMode != mode.DriveModeUser {
log.Printf("Update channel 6 with value %v, new user_mode: %v", value, mode.DriveModeUser)
}
a.driveMode = mode.DriveModeUser
}
}
func (a *ArduinoPart) processDistanceCm(v *string) {
value, err := strconv.Atoi(*v)
if err != nil {
log.Printf("invalid value for distanceCm, should be an int: %v", err)
return
}
a.distanceCm = value
}
func (a *ArduinoPart) publishLoop() {
prefix := strings.TrimSuffix(a.topicBase, "/")
for {
a.publishValues(prefix)
time.Sleep(time.Second / time.Duration(int(a.pubFrequency)))
}
}
func (a *ArduinoPart) publishValues(prefix string) {
a.mutex.Lock()
defer a.mutex.Unlock()
a.pub.Publish(prefix+"/throttle", mqttdevice.NewMqttValue(a.throttle))
a.pub.Publish(prefix+"/steering", mqttdevice.NewMqttValue(a.steering))
a.pub.Publish(prefix+"/drive_mode", mqttdevice.NewMqttValue(a.driveMode))
a.pub.Publish(prefix+"/switch_record", mqttdevice.NewMqttValue(a.ctrlRecord))
a.pub.Publish(prefix+"/distance_cm", mqttdevice.NewMqttValue(a.distanceCm))
}

99
arduino/arduino_test.go
View File

@@ -3,8 +3,10 @@ package arduino
import (
"bufio"
"fmt"
"github.com/cyrilix/robocar-base/mode"
"github.com/cyrilix/robocar-base/mqttdevice"
"net"
"robocar/mode"
"sync"
"testing"
"time"
)
@@ -28,8 +30,8 @@ func TestArduinoPart_Update(t *testing.T) {
}
defer conn.Close()
a := ArduinoPart{serial: conn}
go a.Run()
a := ArduinoPart{serial: conn, pubFrequency: 100, pub: &fakePublisher{msg: make(map[string]interface{})}}
go a.Start()
channel1, channel2, channel3, channel4, channel5, channel6, distanceCm := 678, 910, 1112, 1678, 1910, 112, 128
cases := []struct {
@@ -112,6 +114,9 @@ func TestArduinoPart_Update(t *testing.T) {
{"Distance cm",
fmt.Sprintf("12445,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, channel6, 43),
-1., -1., mode.DriveModeUser, false, 43},
{"Distance cm with \r",
fmt.Sprintf("12450,%d,%d,%d,%d,%d,%d,50,%d\r\n", channel1, channel2, channel3, channel4, channel5, channel6, 43),
-1., -1., mode.DriveModeUser, false, 43},
}
for _, c := range cases {
@@ -125,7 +130,7 @@ func TestArduinoPart_Update(t *testing.T) {
t.Error("unable to flush content")
}
time.Sleep(1* time.Millisecond)
time.Sleep(5 * time.Millisecond)
a.mutex.Lock()
if fmt.Sprintf("%0.2f", a.throttle) != fmt.Sprintf("%0.2f", c.expectedThrottle) {
t.Errorf("%s: bad throttle value, expected: %0.2f, actual: %.2f", c.name, c.expectedThrottle, a.throttle)
@@ -140,9 +145,93 @@ func TestArduinoPart_Update(t *testing.T) {
t.Errorf("%s: bad switch record, expected: %v, actual:%v", c.name, c.expectedSwitchRecord, a.ctrlRecord)
}
if a.distanceCm != c.expectedDistanceCm {
t.Errorf("%s: bad distanceCm, expected: %v" +
t.Errorf("%s: bad distanceCm, expected: %v"+
", actual:%v", c.name, c.expectedDistanceCm, a.distanceCm)
}
a.mutex.Unlock()
}
}
type fakePublisher struct {
muMsg sync.Mutex
msg map[string]interface{}
}
func (f *fakePublisher) Publish(topic string, payload mqttdevice.MqttValue) {
f.muMsg.Lock()
defer f.muMsg.Unlock()
f.msg[topic] = payload
}
func TestPublish(t *testing.T) {
ln, err := net.Listen("tcp", ":8080")
if err != nil {
t.Fatalf("unable to init connection for test")
}
defer ln.Close()
client, err := net.Dial("tcp", "localhost:8080")
if err != nil {
t.Fatalf("unable to init connection for test")
}
defer client.Close()
conn, err := ln.Accept()
if err != nil {
t.Fatalf("unable to init connection for test")
}
defer conn.Close()
pubFrequency := 100.
p := fakePublisher{msg: make(map[string]interface{})}
a := ArduinoPart{serial: conn, pub: &p, pubFrequency: pubFrequency, topicBase: "car/part/arduino/"}
go a.Start()
defer a.Stop()
cases := []struct {
throttle, steering float32
driveMode mode.DriveMode
switchRecord bool
distanceCm int
expectedThrottle, expectedSteering, expectedDriveMode, expectedSwitchRecord, expectedDistance mqttdevice.MqttValue
}{
{-1, 1, mode.DriveModeUser, false, 55,
"-1.00", "1.00", "user", "OFF", "55"},
{0, 0, mode.DriveModePilot, true, 43,
"0.00", "0.00", "pilot", "ON", "43"},
{0.87, -0.58, mode.DriveModePilot, true, 21,
"0.87", "-0.58", "pilot", "ON", "21"},
}
for _, c := range cases {
a.mutex.Lock()
a.throttle = c.throttle
a.steering = c.steering
a.driveMode = c.driveMode
a.ctrlRecord = c.switchRecord
a.distanceCm = c.distanceCm
a.mutex.Unlock()
time.Sleep(time.Second / time.Duration(int(pubFrequency)))
time.Sleep(500 * time.Millisecond)
p.muMsg.Lock()
if v := p.msg["car/part/arduino/throttle"]; v != c.expectedThrottle {
t.Errorf("msg(car/part/arduino/throttle): %v, wants %v", v, c.expectedThrottle)
}
if v := p.msg["car/part/arduino/steering"]; v != c.expectedSteering {
t.Errorf("msg(car/part/arduino/steering): %v, wants %v", v, c.expectedSteering)
}
if v := p.msg["car/part/arduino/drive_mode"]; v != c.expectedDriveMode {
t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", v, c.expectedDriveMode)
}
if v := p.msg["car/part/arduino/switch_record"]; v != c.expectedSwitchRecord {
t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", v, c.expectedSwitchRecord)
}
if v := p.msg["car/part/arduino/distance_cm"]; v != c.expectedDistance {
t.Errorf("msg(car/part/arduino/distance_cm): %v, wants %v", v, c.expectedThrottle)
}
p.muMsg.Unlock()
}
}