control.go 2.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158
  1. package main
  2. import (
  3. "fmt"
  4. "log"
  5. "douglasthrift.net/dtrobots/gamepad"
  6. "github.com/ev3go/ev3dev"
  7. )
  8. // drive modes
  9. const (
  10. tank = iota
  11. joystick
  12. )
  13. const driveModeCount = 2
  14. type driveMode int
  15. func (d driveMode) String() string {
  16. switch d {
  17. case tank:
  18. return "tank"
  19. case joystick:
  20. return "joystick"
  21. default:
  22. return "unknown"
  23. }
  24. }
  25. func (d *driveMode) Set(value string) error {
  26. switch value {
  27. case "tank":
  28. *d = tank
  29. case "joystick":
  30. *d = joystick
  31. default:
  32. return fmt.Errorf("unknown drive mode: %v", value)
  33. }
  34. return nil
  35. }
  36. func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.TachoMotor) error {
  37. log.Printf("control loop started (drive mode: %v)", mode)
  38. if err := setPolarity(ev3dev.Inversed, leftMotor, rightMotor); err != nil {
  39. return err
  40. }
  41. gamepadMotors := false
  42. for {
  43. select {
  44. case e := <-g.LeftAnalog:
  45. if gamepadMotors {
  46. switch mode {
  47. case tank:
  48. if err := tankAnalogControl(e, leftMotor); err != nil {
  49. return err
  50. }
  51. }
  52. }
  53. case e := <-g.RightAnalog:
  54. if gamepadMotors {
  55. switch mode {
  56. case tank:
  57. if err := tankAnalogControl(e, rightMotor); err != nil {
  58. return err
  59. }
  60. }
  61. }
  62. case e := <-g.DPad:
  63. log.Println("dpad", e)
  64. case e := <-g.A:
  65. if e.IsButtonPress() {
  66. gamepadMotors = true
  67. log.Print("gamepad motor control enabled")
  68. }
  69. case e := <-g.B:
  70. if e.IsButtonPress() {
  71. gamepadMotors = false
  72. log.Print("gamepad motor control disabled")
  73. if err := stop(leftMotor, rightMotor); err != nil {
  74. return err
  75. }
  76. }
  77. case e := <-g.X:
  78. log.Println("x", e)
  79. case e := <-g.Y:
  80. log.Println("y", e)
  81. case e := <-g.Left:
  82. log.Println("left", e)
  83. case e := <-g.Right:
  84. log.Println("right", e)
  85. case e := <-g.Back:
  86. log.Println("back", e)
  87. case e := <-g.Start:
  88. log.Println("start", e)
  89. case e := <-g.Logitech:
  90. if e.IsButtonPress() {
  91. mode++
  92. mode %= driveModeCount
  93. log.Printf("switched drive mode: %v", mode)
  94. }
  95. }
  96. }
  97. }
  98. func setPolarity(polarity ev3dev.Polarity, motors ...*ev3dev.TachoMotor) error {
  99. for _, motor := range motors {
  100. p, err := motor.Polarity()
  101. if err != nil {
  102. return err
  103. }
  104. if p != polarity {
  105. motor.SetPolarity(polarity)
  106. }
  107. if err := motor.Err(); err != nil {
  108. return err
  109. }
  110. }
  111. return nil
  112. }
  113. func stop(motors ...*ev3dev.TachoMotor) error {
  114. for _, motor := range motors {
  115. motor.SetSpeedSetpoint(0)
  116. motor.Command("stop")
  117. if err := motor.Err(); err != nil {
  118. return err
  119. }
  120. }
  121. return nil
  122. }
  123. func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor) error {
  124. if e.IsAxisY() {
  125. speed := int(float64(motor.MaxSpeed()) * e.Fraction())
  126. currentSpeed, err := motor.Speed()
  127. if err != nil {
  128. return err
  129. }
  130. if speed != currentSpeed {
  131. motor.SetSpeedSetpoint(speed)
  132. if speed == 0 {
  133. motor.Command("stop")
  134. } else {
  135. motor.Command("run-forever")
  136. }
  137. if err := motor.Err(); err != nil {
  138. return err
  139. }
  140. }
  141. }
  142. return nil
  143. }