123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158 |
- package main
- import (
- "fmt"
- "log"
- "douglasthrift.net/dtrobots/gamepad"
- "github.com/ev3go/ev3dev"
- )
- // drive modes
- const (
- tank = iota
- joystick
- )
- const driveModeCount = 2
- type driveMode int
- func (d driveMode) String() string {
- switch d {
- case tank:
- return "tank"
- case joystick:
- return "joystick"
- default:
- return "unknown"
- }
- }
- func (d *driveMode) Set(value string) error {
- switch value {
- case "tank":
- *d = tank
- case "joystick":
- *d = joystick
- default:
- return fmt.Errorf("unknown drive mode: %v", value)
- }
- return nil
- }
- func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.TachoMotor) error {
- log.Printf("control loop started (drive mode: %v)", mode)
- if err := setPolarity(ev3dev.Inversed, leftMotor, rightMotor); err != nil {
- return err
- }
- gamepadMotors := false
- for {
- select {
- case e := <-g.LeftAnalog:
- if gamepadMotors {
- switch mode {
- case tank:
- if err := tankAnalogControl(e, leftMotor); err != nil {
- return err
- }
- }
- }
- case e := <-g.RightAnalog:
- if gamepadMotors {
- 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:
- if e.IsButtonPress() {
- gamepadMotors = true
- log.Print("gamepad motor control enabled")
- }
- case e := <-g.B:
- if e.IsButtonPress() {
- gamepadMotors = false
- log.Print("gamepad motor control disabled")
- if err := stop(leftMotor, rightMotor); err != nil {
- return err
- }
- }
- case e := <-g.X:
- log.Println("x", e)
- case e := <-g.Y:
- log.Println("y", e)
- case e := <-g.Left:
- log.Println("left", e)
- case e := <-g.Right:
- log.Println("right", e)
- case e := <-g.Back:
- log.Println("back", e)
- case e := <-g.Start:
- log.Println("start", e)
- case e := <-g.Logitech:
- if e.IsButtonPress() {
- mode++
- mode %= driveModeCount
- log.Printf("switched drive mode: %v", mode)
- }
- }
- }
- }
- func setPolarity(polarity ev3dev.Polarity, motors ...*ev3dev.TachoMotor) error {
- for _, motor := range motors {
- p, err := motor.Polarity()
- if err != nil {
- return err
- }
- if p != polarity {
- motor.SetPolarity(polarity)
- }
- if err := motor.Err(); err != nil {
- return err
- }
- }
- return nil
- }
- func stop(motors ...*ev3dev.TachoMotor) error {
- for _, motor := range motors {
- motor.SetSpeedSetpoint(0)
- motor.Command("stop")
- if err := motor.Err(); err != nil {
- return err
- }
- }
- return nil
- }
- 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
- }
|