dtrobots/rover/control.go

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.Normal, leftMotor, rightMotor); err != nil {
		return err
	}

	gamepadMotors := false
	var leftCruise, rightCruise int

	for {
		select {
		case e := <-g.LeftAnalog:
			if gamepadMotors {
				switch mode {
				case tank:
					if err := tankAnalogControl(e, "left", leftMotor, &leftCruise); err != nil {
						return err
					}
				}
			}
		case e := <-g.RightAnalog:
			if gamepadMotors {
				switch mode {
				case tank:
					if err := tankAnalogControl(e, "right", rightMotor, &rightCruise); 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
				}
				leftCruise, rightCruise = 0, 0
			}
		case e := <-g.X:
			log.Println("x", e)
		case e := <-g.Y:
			log.Println("y", e)
		case e := <-g.Left:
			if e.IsButtonPress() && gamepadMotors {
				if err := tankSetCruise("left", leftMotor, &leftCruise); err != nil {
					return err
				}
			}
		case e := <-g.Right:
			if e.IsButtonPress() && gamepadMotors {
				if err := tankSetCruise("right", rightMotor, &rightCruise); err != nil {
					return err
				}
			}
		case e := <-g.Back:
			if e.IsButtonPress() && gamepadMotors && rightCruise != 0 {
				if err := tankSetCruiseSpeed("left", leftMotor, &leftCruise, rightCruise); err != nil {
					return err
				}
			}
		case e := <-g.Start:
			if e.IsButtonPress() && gamepadMotors && leftCruise != 0 {
				if err := tankSetCruiseSpeed("right", rightMotor, &rightCruise, leftCruise); err != nil {
					return err
				}
			}
		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, side string, motor *ev3dev.TachoMotor, cruise *int) error {
	if e.IsAxisY() {
		speed := -int(float64(motor.MaxSpeed()) * e.Fraction())
		if *cruise != 0 && !(*cruise > 0 && speed > 0 && speed > *cruise) && !(*cruise < 0 && speed < 0 && speed < *cruise) {
			speed = *cruise
		}
		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
			}
		}
	} else if e.IsAxisZ() {
		if *cruise != 0 {
			speed := int(float64(*cruise) * (1 - 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
				}
			}
		}
	} else if e.IsButtonPress() {
		*cruise = 0
		log.Printf("%v cruise set: %v", side, *cruise)
		motor.SetSpeedSetpoint(*cruise)
		motor.Command("stop")
		if err := motor.Err(); err != nil {
			return err
		}
	}
	return nil
}

func tankSetCruise(side string, motor *ev3dev.TachoMotor, cruise *int) error {
	maxSpeed := motor.MaxSpeed()
	speed, err := motor.Speed()
	if err != nil {
		return err
	}
	if speed > 0 && speed > maxSpeed {
		*cruise = maxSpeed
	} else if speed < 0 && speed < -maxSpeed {
		*cruise = -maxSpeed
	} else {
		*cruise = speed
	}
	log.Printf("%v cruise set: %v", side, *cruise)
	return nil
}

func tankSetCruiseSpeed(side string, motor *ev3dev.TachoMotor, cruise *int, speed int) error {
	*cruise = speed
	log.Printf("%v cruise set: %v", side, *cruise)
	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
}