Browse Source

Tank mode works!

Douglas Thrift 5 years ago
parent
commit
2d380d6678
5 changed files with 126 additions and 17 deletions
  1. 27 8
      gamepad/gamepad.go
  2. 5 1
      go.mod
  3. 4 0
      go.sum
  4. 59 3
      rover/control.go
  5. 31 5
      rover/main.go

+ 27 - 8
gamepad/gamepad.go

@@ -3,6 +3,7 @@ package gamepad
 import (
 	"fmt"
 	"log"
+	"math"
 
 	"github.com/gvalkov/golang-evdev"
 )
@@ -18,6 +19,7 @@ const (
 type Event struct {
 	Type  int
 	Value int32
+	Max   int32
 }
 
 func (e Event) IsAxis() bool {
@@ -52,6 +54,17 @@ func (e Event) IsButtonPress() bool {
 	return false
 }
 
+func (e Event) Fraction() float64 {
+	if e.IsAxis() {
+		fraction := float64(e.Value) / float64(e.Max)
+		if math.Abs(fraction) < 0.01 {
+			return 0
+		}
+		return fraction
+	}
+	return float64(e.Value)
+}
+
 func (e Event) String() string {
 	var code string
 	switch e.Type {
@@ -84,6 +97,12 @@ const (
 	All         = 0xfff
 )
 
+const (
+	analogXYMax = 32767
+	analogZMax  = 255
+	dpadMax     = 1
+)
+
 const channelBufferSize = 16
 
 type Gamepad struct {
@@ -183,35 +202,35 @@ func (g *Gamepad) Read() error {
 			switch e.Code {
 			case leftAnalogX:
 				if g.channels&LeftAnalog != 0 {
-					g.LeftAnalog <- Event{Type: AxisX, Value: e.Value}
+					g.LeftAnalog <- Event{Type: AxisX, Value: e.Value, Max: analogXYMax}
 				}
 			case leftAnalogY:
 				if g.channels&LeftAnalog != 0 {
-					g.LeftAnalog <- Event{Type: AxisY, Value: e.Value}
+					g.LeftAnalog <- Event{Type: AxisY, Value: e.Value, Max: analogXYMax}
 				}
 			case leftAnalogZ:
 				if g.channels&LeftAnalog != 0 {
-					g.LeftAnalog <- Event{Type: AxisZ, Value: e.Value}
+					g.LeftAnalog <- Event{Type: AxisZ, Value: e.Value, Max: analogZMax}
 				}
 			case rightAnalogX:
 				if g.channels&RightAnalog != 0 {
-					g.RightAnalog <- Event{Type: AxisX, Value: e.Value}
+					g.RightAnalog <- Event{Type: AxisX, Value: e.Value, Max: analogXYMax}
 				}
 			case rightAnalogY:
 				if g.channels&RightAnalog != 0 {
-					g.RightAnalog <- Event{Type: AxisY, Value: e.Value}
+					g.RightAnalog <- Event{Type: AxisY, Value: e.Value, Max: analogXYMax}
 				}
 			case rightAnalogZ:
 				if g.channels&RightAnalog != 0 {
-					g.RightAnalog <- Event{Type: AxisZ, Value: e.Value}
+					g.RightAnalog <- Event{Type: AxisZ, Value: e.Value, Max: analogZMax}
 				}
 			case dPadX:
 				if g.channels&DPad != 0 {
-					g.DPad <- Event{Type: AxisX, Value: e.Value}
+					g.DPad <- Event{Type: AxisX, Value: e.Value, Max: dpadMax}
 				}
 			case dPadY:
 				if g.channels&DPad != 0 {
-					g.DPad <- Event{Type: AxisY, Value: e.Value}
+					g.DPad <- Event{Type: AxisY, Value: e.Value, Max: dpadMax}
 				}
 			}
 		case evdev.EV_KEY:

+ 5 - 1
go.mod

@@ -1,3 +1,7 @@
 module douglasthrift.net/dtrobots
 
-require github.com/gvalkov/golang-evdev v0.0.0-20180516222720-b6f418b1fe5a
+require (
+	github.com/ev3go/ev3dev v0.0.0-20180426205345-4ad40995118f
+	github.com/gvalkov/golang-evdev v0.0.0-20180516222720-b6f418b1fe5a
+	golang.org/x/sys v0.0.0-20180802203216-0ffbfd41fbef // indirect
+)

+ 4 - 0
go.sum

@@ -1,2 +1,6 @@
+github.com/ev3go/ev3dev v0.0.0-20180426205345-4ad40995118f h1:uZZwWMr9R7f6eGq60rypp3YA7DuFgszbASPgTtBs3Ao=
+github.com/ev3go/ev3dev v0.0.0-20180426205345-4ad40995118f/go.mod h1:xkqnu0U+159iVe5q0ABr7NJJCFX51cOae/Qy4/TDZqI=
 github.com/gvalkov/golang-evdev v0.0.0-20180516222720-b6f418b1fe5a h1:PpqPstphGuFEdiuje4K8NmWN+Qzvu15NN/oFa1w/lGM=
 github.com/gvalkov/golang-evdev v0.0.0-20180516222720-b6f418b1fe5a/go.mod h1:SAzVFKCRezozJTGavF3GX8MBUruETCqzivVLYiywouA=
+golang.org/x/sys v0.0.0-20180802203216-0ffbfd41fbef h1:ESfhYoBNk2UQGmavscFPKfwmc4ZTB2+UdQYsVw6Bq9M=
+golang.org/x/sys v0.0.0-20180802203216-0ffbfd41fbef/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY=

+ 59 - 3
rover/control.go

@@ -5,6 +5,8 @@ import (
 	"log"
 
 	"douglasthrift.net/dtrobots/gamepad"
+
+	"github.com/ev3go/ev3dev"
 )
 
 // drive modes
@@ -40,14 +42,46 @@ func (d *driveMode) Set(value string) error {
 	return nil
 }
 
-func control(mode driveMode, g *gamepad.Gamepad) {
+func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.TachoMotor) error {
 	log.Printf("control loop started (drive mode: %v)", mode)
+
+	polarity, err := leftMotor.Polarity()
+	if err != nil {
+		return err
+	}
+	if polarity != ev3dev.Inversed {
+		leftMotor.SetPolarity(ev3dev.Inversed)
+	}
+	if err = leftMotor.Err(); err != nil {
+		return err
+	}
+	polarity, err = rightMotor.Polarity()
+	if err != nil {
+		return err
+	}
+	if polarity != ev3dev.Inversed {
+		rightMotor.SetPolarity(ev3dev.Inversed)
+	}
+	if err = rightMotor.Err(); err != nil {
+		return err
+	}
+
 	for {
 		select {
 		case e := <-g.LeftAnalog:
-			log.Println("left analog", e)
+			switch mode {
+			case tank:
+				if err := tankAnalogControl(e, leftMotor); err != nil {
+					return err
+				}
+			}
 		case e := <-g.RightAnalog:
-			log.Println("right analog", e)
+			switch mode {
+			case tank:
+				if err := tankAnalogControl(e, rightMotor); err != nil {
+					return err
+				}
+			}
 		case e := <-g.DPad:
 			log.Println("dpad", e)
 		case e := <-g.A:
@@ -75,3 +109,25 @@ func control(mode driveMode, g *gamepad.Gamepad) {
 		}
 	}
 }
+
+func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor) error {
+	if e.IsAxisY() {
+		speed := int(float64(motor.MaxSpeed()) * e.Fraction())
+		currentSpeed, err := motor.Speed()
+		if err != nil {
+			return err
+		}
+		if speed != currentSpeed {
+			motor.SetSpeedSetpoint(speed)
+			if speed == 0 {
+				motor.Command("stop")
+			} else {
+				motor.Command("run-forever")
+			}
+			if err = motor.Err(); err != nil {
+				return err
+			}
+		}
+	}
+	return nil
+}

+ 31 - 5
rover/main.go

@@ -6,17 +6,27 @@ import (
 	"net/http"
 
 	"douglasthrift.net/dtrobots/gamepad"
+
+	"github.com/ev3go/ev3dev"
 )
 
 func main() {
 	var (
-		device string
-		listen string
-		mode   driveMode
+		device      string
+		listen      string
+		leftPort    string
+		leftDriver  string
+		rightPort   string
+		rightDriver string
+		mode        driveMode
 	)
 
 	flag.StringVar(&device, "gamepad", "/dev/input/event0", "the gamepad device path")
 	flag.StringVar(&listen, "listen", "localhost:8080", "the HTTP listen address and port")
+	flag.StringVar(&leftPort, "motor-left-port", "spi0.1:MC", "the left motor port")
+	flag.StringVar(&leftDriver, "motor-left-driver", "lego-nxt-motor", "the left motor driver")
+	flag.StringVar(&rightPort, "motor-right-port", "spi0.1:MB", "the right motor port")
+	flag.StringVar(&rightDriver, "motor-right-driver", "lego-nxt-motor", "the right motor driver")
 	flag.Var(&mode, "mode", "the drive mode (default \"tank\")")
 	flag.Parse()
 
@@ -25,8 +35,24 @@ func main() {
 		log.Fatalf("error creating gamepad: %v", err)
 	}
 
-	go control(mode, g)
-	go log.Fatalf("error reading from gamepad: %v", g.Read())
+	leftMotor, err := ev3dev.TachoMotorFor(leftPort, leftDriver)
+	if err != nil {
+		log.Fatalf("error detecting left motor: %v", err)
+	}
+	log.Printf("detected left motor: %v", leftMotor)
+
+	rightMotor, err := ev3dev.TachoMotorFor(rightPort, rightDriver)
+	if err != nil {
+		log.Fatalf("error detecting right motor: %v", err)
+	}
+	log.Printf("detected right motor: %v", rightMotor)
+
+	go func() {
+		log.Fatalf("error controlling rover: %v", control(mode, g, leftMotor, rightMotor))
+	}()
+	go func() {
+		log.Fatalf("error reading from gamepad: %v", g.Read())
+	}()
 
 	log.Print("HTTP server started")
 	log.Fatalf("error listening and serving: %v", http.ListenAndServe(listen, nil))