No puede seleccionar más de 25 temas Los temas deben comenzar con una letra o número, pueden incluir guiones ('-') y pueden tener hasta 35 caracteres de largo.

442 líneas
15KB

  1. (
  2. var imageDist, micronsPerStep, automation, imgPositions, curPos, tarPos,
  3. netAddress, serialPort, serialListener,
  4. moveTo, jogControl, jogHorizontal, jogVertical,
  5. imgSelect, imgCalibrate, automate;
  6. // init global vars
  7. imageDist = 100; // in steps
  8. micronsPerStep = 0.0977;
  9. automation = false;
  10. imgPositions = 9.collect({nil});
  11. curPos = Point.new(0, 0);
  12. tarPos = Point.new(0, 0);
  13. netAddress = NetAddr.new("127.0.0.1", 7777);
  14. serialPort = SerialPort("/dev/ttyACM0", baudrate: 115200, crtscts: true);
  15. // recieve motor feedback
  16. serialListener = Routine({
  17. var byte, str, res, valArray,
  18. stepper, limitSwitchNeg, limitSwitchPos, safeMode, limitPos;
  19. safeMode = false;
  20. loop{
  21. byte = serialPort.read;
  22. if(byte==13, {
  23. if(str[1].asString == "[", {
  24. valArray = str.asString.interpret.postln;
  25. curPos = Point.new(valArray[0], valArray[1]);
  26. limitSwitchNeg = valArray[2];
  27. limitSwitchPos = valArray[3];
  28. if(safeMode && (limitSwitchNeg == limitSwitchPos), {
  29. safeMode = false;
  30. fork {
  31. netAddress.sendMsg("/STATE/SET", "{message: \"all clear\"}");
  32. 2.wait;
  33. netAddress.sendMsg("/STATE/SET", "{message: \"\"}");
  34. }
  35. });
  36. if(automation, {
  37. if((curPos.x - tarPos.x).abs < 100, {tarPos.x = imageDist.rand2});
  38. if((curPos.y - tarPos.y).abs < 100, {tarPos.y = imageDist.rand2});
  39. moveTo.value(tarPos);
  40. });
  41. }, {
  42. if(str[1..3].asString == "!!!", {
  43. netAddress.sendMsg("/STATE/SET", "{message: \"!!! limit switch still on after 1000 steps, this should NEVER happen\"}");
  44. }, {
  45. automation = false;
  46. safeMode = true;
  47. netAddress.sendMsg("/STATE/SET", "{message: \"!! limit hit, move the other direction\"}");
  48. });
  49. });
  50. str = "";
  51. }, {str = str++byte.asAscii});
  52. };
  53. }).play(AppClock);
  54. // send new coordinates to the arduino / motors
  55. moveTo = {arg point;
  56. serialPort.putAll(point.x.asInteger.asString ++ " " ++ point.y.asInteger.asString);
  57. serialPort.put(10);
  58. };
  59. jogControl = {arg axis;
  60. var jog, count = 0, jogRate= 0, jogDirection = 1;
  61. jog = Task({
  62. loop{
  63. count = (count + 0.01).clip(0, 1);
  64. jogRate = pow(count, 2) * 500;
  65. if(axis == '/jog_horizontal', {
  66. tarPos.x = curPos.x + (jogRate * jogDirection);
  67. }, {
  68. tarPos.y = curPos.y + (jogRate * jogDirection);
  69. });
  70. moveTo.value(tarPos);
  71. 0.1.wait
  72. };
  73. });
  74. OSCFunc({arg msg;
  75. //tarPos.x = curPos.x + (1000 * msg[1]);
  76. //moveTo.value(tarPos);
  77. if(msg[1] == 0, {count = 0; jogRate = 0; jog.pause()}, {jogDirection = msg[1]; jog.play(AppClock)});
  78. automation = false;
  79. netAddress.sendMsg("/STATE/SET", "{automate: 0}");
  80. }, axis, netAddress)
  81. };
  82. jogHorizontal = jogControl.value('/jog_horizontal');
  83. jogVertical = jogControl.value('/jog_vertical');
  84. imgSelect = {
  85. var lastSelect = nil;
  86. OSCFunc({arg msg;
  87. var imgIndex;
  88. if(msg[1] > 0, {
  89. imgIndex = msg[1] - 1;
  90. if(imgPositions[imgIndex] != nil, {tarPos = imgPositions[imgIndex].deepCopy; moveTo.value(tarPos)});
  91. 9.do({arg i; if(imgIndex != i, {
  92. netAddress.sendMsg("/STATE/SET", "{img_" ++ (i + 1).asString ++ "_select: " ++ (i + 1).neg ++ "}")})});
  93. automation = false;
  94. netAddress.sendMsg("/STATE/SET", "{automate: 0}");
  95. lastSelect = imgIndex;
  96. }, {
  97. imgIndex = msg[1].neg - 1;
  98. if(imgIndex == lastSelect, {
  99. if(imgPositions[imgIndex] != nil, {tarPos = imgPositions[imgIndex].deepCopy; moveTo.value(tarPos)});
  100. netAddress.sendMsg("/STATE/SET", "{img_" ++ (imgIndex + 1).asInteger.asString ++ "_select: " ++ (imgIndex + 1) ++ "}")});
  101. });
  102. }, '/img_select', netAddress)
  103. }.value;
  104. imgCalibrate = {
  105. var calibrateHold, imgIndex, setPos;
  106. calibrateHold = Routine({
  107. 20.do({0.1.wait});
  108. imgPositions[imgIndex] = setPos.deepCopy;
  109. netAddress.sendMsg("/STATE/SET", "{message: \"image calibrated\"}");
  110. });
  111. OSCFunc({ arg msg;
  112. imgIndex = msg[1] - 1;
  113. if(imgIndex >= 0, {
  114. setPos = curPos.deepCopy;
  115. calibrateHold.play(AppClock);
  116. }, {
  117. calibrateHold.stop; calibrateHold.reset; netAddress.sendMsg("/STATE/SET", "{message: \"\"}");
  118. });
  119. }, '/img_calibrate', netAddress);
  120. }.value;
  121. automate = OSCFunc({arg msg;
  122. if(msg[1] == 1, {
  123. automation = true;
  124. }, {
  125. automation = false;
  126. tarPos = curPos.deepCopy;
  127. moveTo.value(tarPos);
  128. });
  129. 9.do({arg i; netAddress.sendMsg("/STATE/SET", "{img_" ++ (i + 1).asString ++ "_select: " ++ (i + 1).neg ++ "}")});
  130. }, '/automate', netAddress);
  131. )
  132. (
  133. // TODO:
  134. // set position to 0
  135. // limit switch warnings
  136. // More clean up and testing
  137. var imageDist, rotation, micronsPerStep, curPos, tarPos, automate, imagePositions,
  138. serialPort, serialListener, moveTo,
  139. window, xOffset, yOffset,
  140. userView, imageButtonRects,
  141. dirKeyBlockTasks, jogTasks, jogRates,
  142. moveButtons, curPosFields, tarPosFields,
  143. calibrationSteps, wizardButtons, wizMoveBlock, curWizardStep, curWizardText;
  144. // init global vars
  145. imageDist = 25; // in microns
  146. rotation = 0; // in degrees
  147. micronsPerStep = 0.0977;
  148. curPos = Point.new(0, 0);
  149. tarPos = Point.new(0, 0);
  150. automate = false;
  151. imagePositions = 3.collect({arg r; 3.collect({arg c; Point(imageDist * (c - 1), imageDist * (r - 1))})}).reverse.flat;
  152. // connect to arduino
  153. serialPort = SerialPort(
  154. "/dev/ttyACM0", //edit to match the port (SerialPort.listDevice)
  155. baudrate: 115200, //check that baudrate is the same as in arduino sketch
  156. crtscts: true);
  157. // recieve motor feedback
  158. serialListener = Routine({
  159. var byte, str, res, valArray,
  160. stepper, limitSwitchPos, limitSwitchNeg, safeMode, limitPos;
  161. loop{
  162. byte = serialPort.read;
  163. if(byte==13, {
  164. if(str[1].asString == "[", {
  165. valArray = str.asString.interpret;
  166. stepper = valArray[0];
  167. if(stepper == 1, {curPos.x = valArray[1]}, {curPos.y = valArray[1]});
  168. //tarPos = valArray[2];
  169. limitSwitchPos = valArray[3];
  170. limitSwitchNeg = valArray[4];
  171. safeMode = valArray[5];
  172. limitPos = valArray[6];
  173. // update all the curPos fields
  174. if(stepper == 2, {
  175. //curPos = curPos.rotate(rotation.neg * (pi / 180.0)) * micronsPerStep;
  176. curPos = curPos * micronsPerStep;
  177. curPosFields[0].string = (curPos.x).round(0.1).asString;
  178. curPosFields[1].string = (curPos.y).round(0.1).asString;
  179. curPosFields[2].string = (curPos.rho).round(0.1).asString;
  180. curPosFields[3].string = (if(curPos.theta >= 0, {0}, {360}) + (curPos.theta * (180 / pi))).round(0.1).asString;
  181. userView.refresh;
  182. // automate mode: select new point before the motor comes to a stop
  183. if(automate, {
  184. if((curPos.x - tarPos.x).abs < 5.0, {tarPos.x = imageDist.rand2.round(0.1)});
  185. if((curPos.y - tarPos.y).abs < 5.0, {tarPos.y = imageDist.rand2.round(0.1)});
  186. moveTo.value(tarPos);
  187. });
  188. });
  189. }, {
  190. (str).postln;
  191. });
  192. str = "";
  193. }, {str = str++byte.asAscii});
  194. };
  195. });
  196. // send new coordinates to the arduino / motors
  197. moveTo = {arg point;
  198. var rotatedPoint, xMove, yMove;
  199. tarPosFields[0].string = tarPos.x.round(0.1).asString;
  200. tarPosFields[1].string = tarPos.y.round(0.1).asString;
  201. tarPosFields[2].string = tarPos.rho.round(0.1).asString;
  202. tarPosFields[3].string = (if(tarPos.theta >= 0, {0}, {360}) + (tarPos.theta * (180 / pi))).round(0.1).asString;
  203. //rotatedPoint = point.rotate(rotation * (pi / 180.0));
  204. rotatedPoint = point;
  205. xMove = (rotatedPoint.x / micronsPerStep).round(1).asInteger;
  206. yMove = (rotatedPoint.y / micronsPerStep).round(1).asInteger;
  207. serialPort.putAll(xMove.asString ++ " " ++ yMove.asString);
  208. serialPort.put(10);
  209. };
  210. // generate the gui
  211. window = Window.new("", Rect(400, 400, 480, 650)).front;
  212. xOffset = 240;
  213. yOffset = 220;
  214. // drawing and window key commands
  215. userView = UserView(window, Rect(0, 0, 800, 600));
  216. imageButtonRects = (({arg r; ({arg c; Rect.aboutPoint(Point(xOffset + (120 * (r - 1)), yOffset + (120 * (c - 1))), 5, 5)}) ! 3}) ! 3).flat;
  217. userView.drawFunc = ({
  218. imageButtonRects.do({ arg rect, i;
  219. Pen.addOval(rect);
  220. Pen.color = Color.blue;
  221. Pen.draw;
  222. });
  223. Pen.addOval(Rect.aboutPoint(Point(xOffset + (curPos.x * (120 / imageDist)), yOffset + (curPos.y.neg * (120 / imageDist))), 5, 5));
  224. Pen.color = Color.black;
  225. Pen.draw;
  226. Pen.line(Point(xOffset, yOffset + 150), Point(xOffset, yOffset + 250));
  227. Pen.stroke;
  228. });
  229. userView.keyDownAction = ({arg view, char, mod, unicode, keycode, key;
  230. switch(key,
  231. 16r1000012, {moveButtons[0].focus; dirKeyBlockTasks[0].stop; jogTasks[0].pause; jogTasks[0].play(AppClock)},
  232. 16r1000013, {moveButtons[1].focus; dirKeyBlockTasks[1].stop; jogTasks[1].pause; jogTasks[1].play(AppClock)},
  233. 16r1000014, {moveButtons[2].focus; dirKeyBlockTasks[2].stop; jogTasks[2].pause; jogTasks[2].play(AppClock)},
  234. 16r1000015, {moveButtons[3].focus; dirKeyBlockTasks[3].stop; jogTasks[3].pause; jogTasks[3].play(AppClock)})
  235. });
  236. // create all the jog buttons and logic
  237. dirKeyBlockTasks = [];
  238. jogTasks = [];
  239. jogRates = [0, 0, 0, 0, 0, 0, 0, 0];
  240. moveButtons = ([[-1, 0], [0, -1], [1, 0], [0, 1], [-1, 0], [0, -1], [1, 0], [0, 1]].collect({arg m, i;
  241. var icons = ["◄", "▲", "►", "▼", "↻", "+", "↺", "-"], button;
  242. // speeds up the jog based on how long the button was pressed
  243. jogTasks = jogTasks.add(
  244. Task({
  245. dirKeyBlockTasks[i].stop;
  246. loop{
  247. jogRates[i] = (jogRates[i] + 0.1).clip(0, 10);
  248. if(i < 4, {
  249. // cartesian horizontal movement
  250. if(m[0].abs == 1, {tarPos.x = tarPos.x + (jogRates[i] * m[0])});
  251. // cartesian vertical movement
  252. if(m[1].abs == 1, {tarPos.y = tarPos.y + (jogRates[i] * m[1].neg);});
  253. }, {// polar change theta (rotate)
  254. if(m[0].abs == 1, {tarPos.theta = ((tarPos.theta * (180 / pi)) + (jogRates[i] * m[0])) * (pi / 180.0)});
  255. // polar change magnitude
  256. if(m[1].abs == 1, {tarPos.rho = tarPos.rho + (jogRates[i] * m[1].neg)});
  257. });
  258. moveTo.value(tarPos);
  259. 0.2.wait
  260. };
  261. })
  262. );
  263. // hack to acount for a key held down
  264. dirKeyBlockTasks = dirKeyBlockTasks.add(Task({0.1.wait; jogRates[i] = 0;jogTasks[i].stop}));
  265. // create buttons
  266. button = Button(window, Rect(xOffset - 12.5 + (25 * m[0]) + if(i < 4, {-175}, {175}), yOffset + 187.5 + (25 * m[1]), 25, 25))
  267. .states_([[icons[i]]])
  268. .mouseDownAction_({jogRates[i] = 0; jogTasks[i].play(AppClock)})
  269. .action_({jogTasks[i].stop(AppClock)})
  270. .enabled_(false)
  271. .keyDownAction_({arg butt, char, mod, unicode, keycode, key;
  272. switch(key,
  273. 16r1000012, {moveButtons[0].focus; dirKeyBlockTasks[0].stop; jogTasks[0].pause; jogTasks[0].play(AppClock); true},
  274. 16r1000013, {moveButtons[1].focus; dirKeyBlockTasks[1].stop; jogTasks[1].pause; jogTasks[1].play(AppClock); true},
  275. 16r1000014, {moveButtons[2].focus; dirKeyBlockTasks[2].stop; jogTasks[2].pause; jogTasks[2].play(AppClock); true},
  276. 16r1000015, {moveButtons[3].focus; dirKeyBlockTasks[3].stop; jogTasks[3].pause; jogTasks[3].play(AppClock); true},
  277. {false})})
  278. .keyUpAction_({arg butt, char, mod, unicode, keycode, key;
  279. switch(key,
  280. 16r1000012, {dirKeyBlockTasks[0].start(AppClock); true},
  281. 16r1000013, {dirKeyBlockTasks[1].start(AppClock); true},
  282. 16r1000014, {dirKeyBlockTasks[2].start(AppClock); true},
  283. 16r1000015, {dirKeyBlockTasks[3].start(AppClock); true},
  284. {false})})
  285. }));
  286. // position text fields
  287. StaticText(window, Rect(xOffset - 82, yOffset + 150, 300, 20)).string_("cartesian");
  288. StaticText(window, Rect(xOffset + 39, yOffset + 150, 300, 20)).string_("polar");
  289. curPosFields = [];
  290. tarPosFields = ["x", "y", "ρ", "θ"].collect({arg v, i;
  291. StaticText(window, Rect(xOffset + 22.5 + (55 * (i - 2)), yOffset + 170, 50, 20)).string_(v);
  292. curPosFields = curPosFields.add(StaticText(window, Rect(xOffset + 5 + (55 * (i - 2)), yOffset + 220, 50, 20)).string_("0.0"));
  293. TextField(window, Rect(xOffset + 2.5 + (55 * (i - 2)), yOffset + 190, 50, 20))
  294. .string_("0.0")
  295. .enabled_(false)
  296. .action_({arg field;
  297. if(i < 2, {
  298. tarPos.x = tarPosFields[0].string.asFloat;
  299. tarPos.y = tarPosFields[1].string.asFloat;
  300. tarPosFields[2].string = tarPos.rho.round(0.1).asString;
  301. tarPosFields[3].string = (if(tarPos.theta >= 0, {0}, {360}) + (tarPos.theta * (180 / pi))).round(0.1).asString;
  302. }, {
  303. tarPos.rho = tarPosFields[2].string.asFloat;
  304. tarPos.theta = tarPosFields[3].string.asFloat * (pi / 180);
  305. tarPosFields[0].string = tarPos.x.round(0.1).asString;
  306. tarPosFields[1].string = tarPos.y.round(0.1).asString;
  307. });
  308. moveTo.value(tarPos)})
  309. });
  310. // calibration wizard
  311. calibrationSteps = [
  312. "1) find center image",
  313. "2) find northwest image \ntry first by using only the ↻ ↺ buttons to change θ",
  314. "3) compute all other points \nthis will erase previously saved points unless skipped",
  315. "4) find north image",
  316. "5) find northeast image",
  317. "6) find east image",
  318. "7) find southeast image",
  319. "8) find south image",
  320. "9) find southwest image",
  321. "10) find west image"
  322. ];
  323. // disables everything till the point is reached between each step in the wizard
  324. wizMoveBlock = Task({
  325. while({curPos.dist(tarPos) > 1}, {
  326. moveButtons.do({arg button; button.enabled = false});
  327. wizardButtons.do({arg button; button.enabled = false});
  328. tarPosFields.do({arg field; field.enabled = false});
  329. 0.1.wait;
  330. });
  331. wizardButtons.do({arg button; button.enabled = true});
  332. wizardButtons[2].focus;
  333. moveButtons.do({arg button; button.enabled = true});
  334. tarPosFields.do({arg field; field.enabled = true});
  335. });
  336. // automate / calibrate button
  337. Button.new(window, Rect.aboutPoint(Point(xOffset, yOffset + 270), 75, 12.5))
  338. .states_([["calibrate"], ["automate"]])
  339. .action_({arg button;
  340. if(button.value == 0, {
  341. automate = true;
  342. curWizardText.string = "";
  343. wizardButtons.do({arg button; button.visible = false});
  344. }, {
  345. automate = false;
  346. curWizardText.string = calibrationSteps[0];
  347. tarPos = imagePositions[4].deepCopy;
  348. moveTo.value(tarPos);
  349. wizMoveBlock.start(AppClock);
  350. curWizardStep = 0;
  351. wizardButtons.do({arg button; button.visible = true});
  352. });
  353. moveButtons.do({arg button; button.enabled = automate.not});
  354. tarPosFields.do({arg field; field.enabled = automate.not});
  355. });
  356. // wizard button logic
  357. curWizardStep = 0;
  358. curWizardText = StaticText(window, Rect.aboutPoint(Point(xOffset, yOffset + 310), 200, 20)).string_("").align_(\center);
  359. wizardButtons = ["back", "skip", "next"].collect({arg t, i;
  360. var pointSeq, button;
  361. pointSeq = [4, 0, 0, 1, 2, 5, 8, 7, 6, 3, 4];
  362. button = Button(window, Rect.aboutPoint(Point(xOffset - 60 + (60 * i), yOffset + 350), 25, 12.5))
  363. .states_([[t]])
  364. .action_({arg button;
  365. // code to automate populate all the points based on relation between two of the points
  366. if((curWizardStep == 2) && (i == 2), {
  367. if(imagePositions[0].rho == imageDist, {
  368. }, {
  369. });
  370. rotation = imagePositions[0].theta - (0.75 * pi);
  371. imagePositions[1].theta = (0.5 * pi) + rotation;
  372. imagePositions[2].theta = (0.25 * pi) + rotation;
  373. imagePositions[3].theta = pi + rotation;
  374. imagePositions[5].theta = rotation;
  375. imagePositions[6].theta = (1.25 * pi) + rotation;
  376. imagePositions[7].theta = (1.5 * pi) + rotation;
  377. imagePositions[8].theta = (1.75 * pi) + rotation;
  378. });
  379. if((curWizardStep == 0) && (i == 2), {serialPort.putAll("c")});
  380. if(i == 2, {imagePositions[pointSeq[curWizardStep]] = if(curWizardStep == 0, {Point(0, 0)}, {curPos.deepCopy})});
  381. curWizardStep = (curWizardStep + if(i == 0, {-1}, {1})) % 10;
  382. tarPos = imagePositions[pointSeq[curWizardStep]].deepCopy;
  383. moveTo.value(tarPos);
  384. wizMoveBlock.start(AppClock);
  385. //wizardButtons.do({arg button; button.enabled = true});
  386. //moveButtons.do({arg button; button.enabled = true});
  387. //tarPosFields.do({arg field; field.enabled = true});
  388. curWizardText.string = calibrationSteps[curWizardStep];
  389. //wizardButtons[1].visible = if(curWizardStep == 2, {true}, {false});
  390. })
  391. .visible_(false)
  392. });
  393. serialListener.play(AppClock);
  394. )