Debug on car
This commit is contained in:
@ -26,7 +26,7 @@ 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 {
|
||||
type Part struct {
|
||||
pub mqttdevice.Publisher
|
||||
topicBase string
|
||||
pubFrequency float64
|
||||
@ -37,18 +37,20 @@ type ArduinoPart struct {
|
||||
distanceCm int
|
||||
ctrlRecord bool
|
||||
driveMode mode.DriveMode
|
||||
debug bool
|
||||
}
|
||||
|
||||
func NewArduinoPart(name string, baud int, pub mqttdevice.Publisher, topicBase string, pubFrequency float64) *ArduinoPart {
|
||||
func NewPart(name string, baud int, pub mqttdevice.Publisher, topicBase string, pubFrequency float64, debug bool) *Part {
|
||||
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}
|
||||
return &Part{serial: s, pub: pub, topicBase: topicBase, pubFrequency: pubFrequency, driveMode: mode.DriveModeInvalid, debug: debug}
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) Start() {
|
||||
func (a *Part) Start() error {
|
||||
log.Printf("start arduino part")
|
||||
go a.publishLoop()
|
||||
for {
|
||||
buff := bufio.NewReader(a.serial)
|
||||
@ -58,7 +60,7 @@ func (a *ArduinoPart) Start() {
|
||||
break
|
||||
}
|
||||
|
||||
if ! serialLineRegex.MatchString(line) {
|
||||
if !serialLineRegex.MatchString(line) {
|
||||
log.Printf("invalid line: '%v'", line)
|
||||
continue
|
||||
}
|
||||
@ -66,22 +68,23 @@ func (a *ArduinoPart) Start() {
|
||||
|
||||
a.updateValues(values)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) updateValues(values []string) {
|
||||
func (a *Part) 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])
|
||||
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")
|
||||
func (a *Part) Stop() {
|
||||
log.Printf("stop ArduinoPart")
|
||||
switch s := a.serial.(type) {
|
||||
case io.ReadCloser:
|
||||
if err := s.Close(); err != nil {
|
||||
@ -90,8 +93,11 @@ func (a *ArduinoPart) Stop() {
|
||||
}
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) processChannel1(v *string) {
|
||||
value, err := strconv.Atoi(*v)
|
||||
func (a *Part) processChannel1(v string) {
|
||||
if a.debug {
|
||||
log.Printf("channel1: %v", v)
|
||||
}
|
||||
value, err := strconv.Atoi(v)
|
||||
if err != nil {
|
||||
log.Printf("invalid value for channel1, should be an int: %v", err)
|
||||
}
|
||||
@ -103,8 +109,11 @@ func (a *ArduinoPart) processChannel1(v *string) {
|
||||
a.steering = ((float32(value)-MinPwmAngle)/(MaxPwmAngle-MinPwmAngle))*2.0 - 1.0
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) processChannel2(v *string) {
|
||||
value, err := strconv.Atoi(*v)
|
||||
func (a *Part) processChannel2(v string) {
|
||||
if a.debug {
|
||||
log.Printf("channel2: %v", v)
|
||||
}
|
||||
value, err := strconv.Atoi(v)
|
||||
if err != nil {
|
||||
log.Printf("invalid value for channel2, should be an int: %v", err)
|
||||
}
|
||||
@ -116,22 +125,30 @@ func (a *ArduinoPart) processChannel2(v *string) {
|
||||
a.throttle = ((float32(value)-MinPwmThrottle)/(MaxPwmThrottle-MinPwmThrottle))*2.0 - 1.0
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) processChannel3(values *string) {
|
||||
|
||||
func (a *Part) processChannel3(v string) {
|
||||
if a.debug {
|
||||
log.Printf("channel3: %v", v)
|
||||
}
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) processChannel4(values *string) {
|
||||
|
||||
func (a *Part) processChannel4(v string) {
|
||||
if a.debug {
|
||||
log.Printf("channel4: %v", v)
|
||||
}
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) processChannel5(v *string) {
|
||||
value, err := strconv.Atoi(*v)
|
||||
func (a *Part) processChannel5(v string) {
|
||||
if a.debug {
|
||||
log.Printf("channel5: %v", v)
|
||||
}
|
||||
|
||||
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 {
|
||||
if !a.ctrlRecord {
|
||||
log.Printf("Update channel 5 with value %v, record: %v", true, false)
|
||||
a.ctrlRecord = true
|
||||
}
|
||||
@ -143,8 +160,11 @@ func (a *ArduinoPart) processChannel5(v *string) {
|
||||
}
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) processChannel6(v *string) {
|
||||
value, err := strconv.Atoi(*v)
|
||||
func (a *Part) processChannel6(v string) {
|
||||
if a.debug {
|
||||
log.Printf("channel6: %v", v)
|
||||
}
|
||||
value, err := strconv.Atoi(v)
|
||||
if err != nil {
|
||||
log.Printf("invalid value for channel6, should be an int: %v", err)
|
||||
return
|
||||
@ -162,8 +182,8 @@ func (a *ArduinoPart) processChannel6(v *string) {
|
||||
}
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) processDistanceCm(v *string) {
|
||||
value, err := strconv.Atoi(*v)
|
||||
func (a *Part) processDistanceCm(v string) {
|
||||
value, err := strconv.Atoi(v)
|
||||
if err != nil {
|
||||
log.Printf("invalid value for distanceCm, should be an int: %v", err)
|
||||
return
|
||||
@ -171,7 +191,7 @@ func (a *ArduinoPart) processDistanceCm(v *string) {
|
||||
a.distanceCm = value
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) publishLoop() {
|
||||
func (a *Part) publishLoop() {
|
||||
prefix := strings.TrimSuffix(a.topicBase, "/")
|
||||
for {
|
||||
a.publishValues(prefix)
|
||||
@ -179,7 +199,7 @@ func (a *ArduinoPart) publishLoop() {
|
||||
}
|
||||
}
|
||||
|
||||
func (a *ArduinoPart) publishValues(prefix string) {
|
||||
func (a *Part) publishValues(prefix string) {
|
||||
a.mutex.Lock()
|
||||
defer a.mutex.Unlock()
|
||||
a.pub.Publish(prefix+"/throttle", mqttdevice.NewMqttValue(a.throttle))
|
||||
|
||||
@ -5,8 +5,8 @@ import (
|
||||
"fmt"
|
||||
"github.com/cyrilix/robocar-base/mode"
|
||||
"github.com/cyrilix/robocar-base/mqttdevice"
|
||||
"github.com/cyrilix/robocar-base/testtools"
|
||||
"net"
|
||||
"sync"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
@ -30,7 +30,7 @@ func TestArduinoPart_Update(t *testing.T) {
|
||||
}
|
||||
defer conn.Close()
|
||||
|
||||
a := ArduinoPart{serial: conn, pubFrequency: 100, pub: &fakePublisher{msg: make(map[string]interface{})}}
|
||||
a := Part{serial: conn, pubFrequency: 100, pub: testtools.NewFakePublisher()}
|
||||
go a.Start()
|
||||
|
||||
channel1, channel2, channel3, channel4, channel5, channel6, distanceCm := 678, 910, 1112, 1678, 1910, 112, 128
|
||||
@ -61,7 +61,6 @@ func TestArduinoPart_Update(t *testing.T) {
|
||||
fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1003, channel6, distanceCm),
|
||||
-1., -1., mode.DriveModeUser, true, distanceCm},
|
||||
|
||||
|
||||
{"DriveMode: user",
|
||||
fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 998, distanceCm),
|
||||
-1., -1., mode.DriveModeUser, false, distanceCm},
|
||||
@ -77,7 +76,6 @@ func TestArduinoPart_Update(t *testing.T) {
|
||||
fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1003, distanceCm),
|
||||
-1., -1., mode.DriveModeUser, false, distanceCm},
|
||||
|
||||
|
||||
{"Sterring: over left",
|
||||
fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,50,%d\n", 99, channel2, channel3, channel4, channel5, channel6, distanceCm),
|
||||
-1., -1., mode.DriveModeUser, false, distanceCm},
|
||||
@ -94,7 +92,6 @@ func TestArduinoPart_Update(t *testing.T) {
|
||||
fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,50,%d\n", 2998, channel2, channel3, channel4, channel5, channel6, distanceCm),
|
||||
-1., 1., mode.DriveModeUser, false, distanceCm},
|
||||
|
||||
|
||||
{"Throttle: over down",
|
||||
fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 99, channel3, channel4, channel5, channel6, distanceCm),
|
||||
-1., -1., mode.DriveModeUser, false, distanceCm},
|
||||
@ -152,17 +149,6 @@ func TestArduinoPart_Update(t *testing.T) {
|
||||
}
|
||||
}
|
||||
|
||||
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 {
|
||||
@ -183,8 +169,8 @@ func TestPublish(t *testing.T) {
|
||||
defer conn.Close()
|
||||
|
||||
pubFrequency := 100.
|
||||
p := fakePublisher{msg: make(map[string]interface{})}
|
||||
a := ArduinoPart{serial: conn, pub: &p, pubFrequency: pubFrequency, topicBase: "car/part/arduino/"}
|
||||
p := testtools.NewFakePublisher()
|
||||
a := Part{serial: conn, pub: p, pubFrequency: pubFrequency, topicBase: "car/part/arduino/"}
|
||||
go a.Start()
|
||||
defer a.Stop()
|
||||
|
||||
@ -196,11 +182,11 @@ func TestPublish(t *testing.T) {
|
||||
expectedThrottle, expectedSteering, expectedDriveMode, expectedSwitchRecord, expectedDistance mqttdevice.MqttValue
|
||||
}{
|
||||
{-1, 1, mode.DriveModeUser, false, 55,
|
||||
"-1.00", "1.00", "user", "OFF", "55"},
|
||||
mqttdevice.NewMqttValue("-1.00"), mqttdevice.NewMqttValue("1.00"), mqttdevice.NewMqttValue("user"), mqttdevice.NewMqttValue("OFF"), mqttdevice.NewMqttValue("55")},
|
||||
{0, 0, mode.DriveModePilot, true, 43,
|
||||
"0.00", "0.00", "pilot", "ON", "43"},
|
||||
mqttdevice.NewMqttValue("0.00"), mqttdevice.NewMqttValue("0.00"), mqttdevice.NewMqttValue("pilot"), mqttdevice.NewMqttValue("ON"), mqttdevice.NewMqttValue("43")},
|
||||
{0.87, -0.58, mode.DriveModePilot, true, 21,
|
||||
"0.87", "-0.58", "pilot", "ON", "21"},
|
||||
mqttdevice.NewMqttValue("0.87"), mqttdevice.NewMqttValue("-0.58"), mqttdevice.NewMqttValue("pilot"), mqttdevice.NewMqttValue("ON"), mqttdevice.NewMqttValue("21")},
|
||||
}
|
||||
|
||||
for _, c := range cases {
|
||||
@ -215,23 +201,20 @@ func TestPublish(t *testing.T) {
|
||||
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 {
|
||||
if v := p.PublishedEvent("car/part/arduino/throttle"); string(v) != string(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 {
|
||||
if v := p.PublishedEvent("car/part/arduino/steering"); string(v) != string(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 {
|
||||
if v := p.PublishedEvent("car/part/arduino/drive_mode"); string(v) != string(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 {
|
||||
if v := p.PublishedEvent("car/part/arduino/switch_record"); string(v) != string(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 {
|
||||
if v := p.PublishedEvent("car/part/arduino/distance_cm"); string(v) != string(c.expectedDistance) {
|
||||
t.Errorf("msg(car/part/arduino/distance_cm): %v, wants %v", v, c.expectedThrottle)
|
||||
}
|
||||
p.muMsg.Unlock()
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user