You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

chc.go 4.9KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212
  1. package chocococon
  2. import (
  3. "fmt"
  4. "io/ioutil"
  5. "log"
  6. "os"
  7. "strings"
  8. "git.timschuster.info/stephan.graf/Complete_hardware_Control"
  9. "github.com/pkg/errors"
  10. )
  11. func init() {
  12. cmds["move_to"] = moveTo
  13. helpm["move_to"] = "Moves the head to a position with given speed. [x,y,z,hspd,vspd]"
  14. cmds["m"] = moveTo
  15. helpm["m"] = "Alias for move_to"
  16. cmds["roll"] = roll
  17. helpm["roll"] = "Rolls the head. [angle]"
  18. cmds["rotate"] = rotate
  19. helpm["rotate"] = "Rotates the head. [angle]"
  20. cmds["motor"] = motor
  21. helpm["motor"] = "overrides the actual speed of the motor"
  22. cmds["connect"] = connect
  23. helpm["connect"] = "Connects to a Hardware Controller. [path]"
  24. cmds["deliver"] = deliver
  25. helpm["deliver"] = "Delivers item from reg=Column fch=Row. [reg,fch]"
  26. cmds["pickup"] = pickup
  27. helpm["pickup"] = "Pickup item at defined spot."
  28. cmds["putin"] = putin
  29. helpm["putin"] = "Put item into Column/Row. [reg,fch]"
  30. cmds["nolog"] = nolog
  31. helpm["nolog"] = "Disables the log for future connections"
  32. }
  33. var logger = log.New(os.Stderr, "CHC", log.Lshortfile|log.LUTC)
  34. func nolog(params []string) error {
  35. logger = log.New(ioutil.Discard, "", log.LUTC)
  36. return nil
  37. }
  38. func connect(params []string) error {
  39. var path string
  40. for k := range params {
  41. if strings.HasPrefix(params[k], "path=") {
  42. path = strings.TrimPrefix(params[k], "path=")
  43. }
  44. }
  45. if curCHC != nil {
  46. fmt.Println("Shutting down old controller...")
  47. curCHC.Shutdown()
  48. }
  49. fmt.Printf("Using Config Path %s\n", path)
  50. hc, err := HardwareControl.NewHardwareControl(logger, path)
  51. if err != nil {
  52. return errors.Wrap(err, "Could not connect to CHC")
  53. }
  54. curCHC = hc
  55. return nil
  56. }
  57. func moveTo(params []string) error {
  58. if curCHC == nil {
  59. fmt.Println("CHC not connected, aborting.")
  60. return nil
  61. }
  62. var (
  63. x, y, z float64
  64. xyspeed, zspeed int16
  65. )
  66. x, y, z = -1, -1, -1
  67. xyspeed, zspeed = -1, -1
  68. for k := range params {
  69. if err := parseParamToFloat64("x", &x, params[k]); err != nil {
  70. return errors.Wrap(err, "Error while parsing X")
  71. }
  72. if err := parseParamToFloat64("y", &y, params[k]); err != nil {
  73. return errors.Wrap(err, "Error while parsing Y")
  74. }
  75. if err := parseParamToFloat64("z", &z, params[k]); err != nil {
  76. return errors.Wrap(err, "Error while parsing Z")
  77. }
  78. if err := parseParamToInt16("hspd", &xyspeed, params[k]); err != nil {
  79. return errors.Wrap(err, "Error while parsing hspd")
  80. }
  81. if err := parseParamToInt16("vspd", &zspeed, params[k]); err != nil {
  82. return errors.Wrap(err, "Error while parsing vspd")
  83. }
  84. }
  85. fmt.Printf("Moving to %5.1f %5.1f %5.1f with speed horizontal=%d and vertical=%d\n", x, y, z, xyspeed, zspeed)
  86. return curCHC.MoveTo(x, y, z, xyspeed, zspeed)
  87. }
  88. func roll(params []string) error {
  89. if curCHC == nil {
  90. fmt.Println("CHC not connected, aborting.")
  91. return nil
  92. }
  93. var (
  94. angle int16
  95. )
  96. angle = -1
  97. for k := range params {
  98. if err := parseParamToInt16("angle", &angle, params[k]); err != nil {
  99. return errors.Wrap(err, "Error while parsing Angle")
  100. }
  101. }
  102. fmt.Printf("Rolling to angle %d\n", angle)
  103. return curCHC.Roll(angle)
  104. }
  105. func rotate(params []string) error {
  106. if curCHC == nil {
  107. fmt.Println("CHC not connected, aborting.")
  108. return nil
  109. }
  110. var (
  111. angle int16
  112. )
  113. angle = -1
  114. for k := range params {
  115. if err := parseParamToInt16("angle", &angle, params[k]); err != nil {
  116. fmt.Println("Err parsing: ", err)
  117. return nil
  118. }
  119. }
  120. fmt.Printf("Rotating to angle %d\n", angle)
  121. return curCHC.Rotate(angle)
  122. }
  123. func motor(params []string) error {
  124. if curCHC == nil {
  125. fmt.Println("CHC not connected, aborting.")
  126. return nil
  127. }
  128. var (
  129. angle int16
  130. )
  131. angle = -1
  132. for k := range params {
  133. if err := parseParamToInt16("angle", &angle, params[k]); err != nil {
  134. fmt.Println("Err parsing: ", err)
  135. return nil
  136. }
  137. }
  138. fmt.Printf("Setting speed to %d\n", angle)
  139. return curCHC.OverrideMotorSpeed(angle)
  140. }
  141. func deliver(params []string) error {
  142. if curCHC == nil {
  143. fmt.Println("CHC not connected, aborting.")
  144. return nil
  145. }
  146. var (
  147. regalNumber, fachNumber int32
  148. )
  149. for k := range params {
  150. if err := parseParamToInt32("reg", &regalNumber, params[k]); err != nil {
  151. return errors.Wrap(err, "Error while parsing hspd")
  152. }
  153. if err := parseParamToInt32("fch", &fachNumber, params[k]); err != nil {
  154. return errors.Wrap(err, "Error while parsing vspd")
  155. }
  156. }
  157. return curCHC.Deliver(regalNumber, fachNumber)
  158. }
  159. func pickup(_ []string) error {
  160. if curCHC == nil {
  161. fmt.Println("CHC not connected, aborting")
  162. return nil
  163. }
  164. return curCHC.PickUp()
  165. }
  166. func putin(params []string) error {
  167. if curCHC == nil {
  168. fmt.Println("CHC not connected, aborting.")
  169. return nil
  170. }
  171. var (
  172. regalNumber, fachNumber int32
  173. )
  174. for k := range params {
  175. if err := parseParamToInt32("reg", &regalNumber, params[k]); err != nil {
  176. return errors.Wrap(err, "Error while parsing hspd")
  177. }
  178. if err := parseParamToInt32("fch", &fachNumber, params[k]); err != nil {
  179. return errors.Wrap(err, "Error while parsing vspd")
  180. }
  181. }
  182. return curCHC.PutIn(regalNumber, fachNumber)
  183. }