control.go 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244
  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.Normal, leftMotor, rightMotor); err != nil {
  39. return err
  40. }
  41. gamepadMotors := false
  42. var leftCruise, rightCruise int
  43. for {
  44. select {
  45. case e := <-g.LeftAnalog:
  46. if gamepadMotors {
  47. switch mode {
  48. case tank:
  49. if err := tankAnalogControl(e, "left", leftMotor, &leftCruise); err != nil {
  50. return err
  51. }
  52. }
  53. }
  54. case e := <-g.RightAnalog:
  55. if gamepadMotors {
  56. switch mode {
  57. case tank:
  58. if err := tankAnalogControl(e, "right", rightMotor, &rightCruise); err != nil {
  59. return err
  60. }
  61. }
  62. }
  63. case e := <-g.DPad:
  64. log.Println("dpad", e)
  65. case e := <-g.A:
  66. if e.IsButtonPress() {
  67. gamepadMotors = true
  68. log.Print("gamepad motor control enabled")
  69. }
  70. case e := <-g.B:
  71. if e.IsButtonPress() {
  72. gamepadMotors = false
  73. log.Print("gamepad motor control disabled")
  74. if err := stop(leftMotor, rightMotor); err != nil {
  75. return err
  76. }
  77. leftCruise, rightCruise = 0, 0
  78. }
  79. case e := <-g.X:
  80. log.Println("x", e)
  81. case e := <-g.Y:
  82. log.Println("y", e)
  83. case e := <-g.Left:
  84. if e.IsButtonPress() && gamepadMotors {
  85. if err := tankSetCruise("left", leftMotor, &leftCruise); err != nil {
  86. return err
  87. }
  88. }
  89. case e := <-g.Right:
  90. if e.IsButtonPress() && gamepadMotors {
  91. if err := tankSetCruise("right", rightMotor, &rightCruise); err != nil {
  92. return err
  93. }
  94. }
  95. case e := <-g.Back:
  96. if e.IsButtonPress() && gamepadMotors && rightCruise != 0 {
  97. if err := tankSetCruiseSpeed("left", leftMotor, &leftCruise, rightCruise); err != nil {
  98. return err
  99. }
  100. }
  101. case e := <-g.Start:
  102. if e.IsButtonPress() && gamepadMotors && leftCruise != 0 {
  103. if err := tankSetCruiseSpeed("right", rightMotor, &rightCruise, leftCruise); err != nil {
  104. return err
  105. }
  106. }
  107. case e := <-g.Logitech:
  108. if e.IsButtonPress() {
  109. mode++
  110. mode %= driveModeCount
  111. log.Printf("switched drive mode: %v", mode)
  112. }
  113. }
  114. }
  115. }
  116. func setPolarity(polarity ev3dev.Polarity, motors ...*ev3dev.TachoMotor) error {
  117. for _, motor := range motors {
  118. p, err := motor.Polarity()
  119. if err != nil {
  120. return err
  121. }
  122. if p != polarity {
  123. motor.SetPolarity(polarity)
  124. }
  125. if err := motor.Err(); err != nil {
  126. return err
  127. }
  128. }
  129. return nil
  130. }
  131. func stop(motors ...*ev3dev.TachoMotor) error {
  132. for _, motor := range motors {
  133. motor.SetSpeedSetpoint(0)
  134. motor.Command("stop")
  135. if err := motor.Err(); err != nil {
  136. return err
  137. }
  138. }
  139. return nil
  140. }
  141. func tankAnalogControl(e gamepad.Event, side string, motor *ev3dev.TachoMotor, cruise *int) error {
  142. if e.IsAxisY() {
  143. speed := -int(float64(motor.MaxSpeed()) * e.Fraction())
  144. if *cruise != 0 && !(*cruise > 0 && speed > 0 && speed > *cruise) && !(*cruise < 0 && speed < 0 && speed < *cruise) {
  145. speed = *cruise
  146. }
  147. currentSpeed, err := motor.Speed()
  148. if err != nil {
  149. return err
  150. }
  151. if speed != currentSpeed {
  152. motor.SetSpeedSetpoint(speed)
  153. if speed == 0 {
  154. motor.Command("stop")
  155. } else {
  156. motor.Command("run-forever")
  157. }
  158. if err := motor.Err(); err != nil {
  159. return err
  160. }
  161. }
  162. } else if e.IsAxisZ() {
  163. if *cruise != 0 {
  164. speed := int(float64(*cruise) * (1 - e.Fraction()))
  165. currentSpeed, err := motor.Speed()
  166. if err != nil {
  167. return err
  168. }
  169. if speed != currentSpeed {
  170. motor.SetSpeedSetpoint(speed)
  171. if speed == 0 {
  172. motor.Command("stop")
  173. } else {
  174. motor.Command("run-forever")
  175. }
  176. if err := motor.Err(); err != nil {
  177. return err
  178. }
  179. }
  180. }
  181. } else if e.IsButtonPress() {
  182. *cruise = 0
  183. log.Printf("%v cruise set: %v", side, *cruise)
  184. motor.SetSpeedSetpoint(*cruise)
  185. motor.Command("stop")
  186. if err := motor.Err(); err != nil {
  187. return err
  188. }
  189. }
  190. return nil
  191. }
  192. func tankSetCruise(side string, motor *ev3dev.TachoMotor, cruise *int) error {
  193. maxSpeed := motor.MaxSpeed()
  194. speed, err := motor.Speed()
  195. if err != nil {
  196. return err
  197. }
  198. if speed > 0 && speed > maxSpeed {
  199. *cruise = maxSpeed
  200. } else if speed < 0 && speed < -maxSpeed {
  201. *cruise = -maxSpeed
  202. } else {
  203. *cruise = speed
  204. }
  205. log.Printf("%v cruise set: %v", side, *cruise)
  206. return nil
  207. }
  208. func tankSetCruiseSpeed(side string, motor *ev3dev.TachoMotor, cruise *int, speed int) error {
  209. *cruise = speed
  210. log.Printf("%v cruise set: %v", side, *cruise)
  211. currentSpeed, err := motor.Speed()
  212. if err != nil {
  213. return err
  214. }
  215. if speed != currentSpeed {
  216. motor.SetSpeedSetpoint(speed)
  217. if speed == 0 {
  218. motor.Command("stop")
  219. } else {
  220. motor.Command("run-forever")
  221. }
  222. if err := motor.Err(); err != nil {
  223. return err
  224. }
  225. }
  226. return nil
  227. }