123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244 |
- 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
- }
|