API tools faq
paste
Advertisement
Guest User

DFMoco-Uno-CNCShield3-8825

a guest
May 1st, 2019
649
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 42.50 KB | None | 0 0
raw download clone embed print report
  1. #define DFMOCO_VERSION 1
  2. #define DFMOCO_VERSION_STRING "1.3.0"
  3.  
  4. /*
  5. DFMoco version 1.3.0
  6.  
  7. Multi-axis motion control.
  8. For use with the Arc motion control system in Dragonframe 4.
  9. Generates step and direction signals, which can be sent to stepper motor drivers.
  10.  
  11. Control up to four axes with an Uno, Duemilanove or 101 board.
  12. Control up to eight axes with a Mega or Mega 2560.
  13.  
  14. Version History
  15.  
  16. Version 1.3.0 Arduino 101 support. Remove non-Arduino support (chipKit, Maple).
  17. Version 1.2.7 Direction setup time.
  18. Version 1.2.6 Add PINOUT_VERSION option to use older pinout.
  19. Version 1.2.5 Fix jogging with low pulse rate.
  20. Version 1.2.4 Fix pin assignments
  21. Version 1.2.3 New Position command
  22. Version 1.2.2 Jog and Inch commands
  23. Version 1.2.1 Moved step/direction pins for motions 5-8.
  24. Detects board type automatically.
  25. Version 1.2.0 Basic go-motion capabilities
  26. Version 1.1.2 Smooth transitions when changing direction
  27. Version 1.1.1 Save/restore motor position
  28. Version 1.1.0 Major rework
  29. Version 1.0.2 Moved pulses into interrupt handler
  30. Version 1.0.1 Added delay for pulse widths
  31. Version 1.0.0 Initial public release.
  32.  
  33. Getting Started:
  34.  
  35. 1. Install IDE (Integrated Development Environment):
  36. Go to https://www.arduino.cc/en/Main/Software and download the Arduino Software for your OS.
  37. 2. Run the IDE you installed.
  38. 3. Open this file in the IDE.
  39. 4. Go to the Tools menu of the IDE and choose the Board type you are using.
  40. 5. Verify/Compile the sketch. (Command-R on Mac, Control-R on Windows.)
  41. 6. After this finishes, Upload the code to the board. (Command-U on Mac, Control-U on Windows.)
  42.  
  43.  
  44. Pin configuration:
  45.  
  46. channel 1
  47. PIN 4 step
  48. PIN 5 direction
  49. channel 2
  50. PIN 6 step
  51. PIN 7 direction
  52. channel 3
  53. PIN 8 step
  54. PIN 9 direction
  55. channel 4
  56. PIN 10 step
  57. PIN 11 direction
  58.  
  59. channel 5
  60. PIN 28 step
  61. PIN 29 direction
  62. channel 6
  63. PIN 30 step
  64. PIN 31 direction
  65. channel 7
  66. PIN 32 step
  67. PIN 33 direction
  68. channel 8
  69. PIN 34 step
  70. PIN 35 direction
  71. */
  72.  
  73. // change this to 1 if you want original pinout for channels 5-8
  74. #define PINOUT_VERSION 2
  75.  
  76. /*
  77. This is PINOUT_VERSION 1
  78.  
  79. channel 5
  80. PIN 22 step
  81. PIN 23 direction
  82. channel 6
  83. PIN 24 step
  84. PIN 25 direction
  85. channel 7
  86. PIN 26 step
  87. PIN 27 direction
  88. channel 8
  89. PIN 28 step
  90. PIN 29 direction
  91. */
  92.  
  93. // detect board type
  94. #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  95. #define BOARD_MEGA 1
  96. #elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega168__)
  97. #define BOARD_UNO 1
  98. #elif defined(ARDUINO_ARCH_ARC32) // Intel Curie/101
  99. #define BOARD_101 1
  100. #include "CurieTimerOne.h"
  101. #else
  102. #error Cannot identify board
  103. #endif
  104.  
  105. // USER: if you want a kill switch, uncomment out the next line by removing the // characters
  106. //#define KILL_SWITCH_INTERRUPT 0
  107.  
  108. #define SERIAL_DEVICE Serial
  109.  
  110. #if defined(BOARD_101)
  111. #define PIN_ON(port, pin) { digitalWrite(pin, 1); }
  112. #define PIN_OFF(port, pin) { digitalWrite(pin, 0); }
  113. #else
  114. #define PIN_ON(port, pin) { port |= pin; }
  115. #define PIN_OFF(port, pin) { port &= ~pin; }
  116. #endif
  117.  
  118. // Arduino Uno/Duemilanove -> 4 MOTORS MAX
  119. // Arduino Mega 2560 / Mega -> 8 MOTORS MAX
  120. #if defined(BOARD_UNO) || defined(BOARD_101)
  121. #define MOTOR_COUNT 4
  122. #else
  123. #define MOTOR_COUNT 8
  124. #endif
  125.  
  126. #define TIME_CHUNK 50
  127. #define SEND_POSITION_COUNT 20000
  128.  
  129. // update velocities 20 x second
  130. #define VELOCITY_UPDATE_RATE (50000 / TIME_CHUNK)
  131. #define VELOCITY_INC(maxrate) (max(1.0f, maxrate / 70.0f))
  132. #define VELOCITY_CONVERSION_FACTOR 0.30517578125f /* 20 / 65.536f */
  133.  
  134. // setup step and direction pins
  135. #if defined(BOARD_101)
  136.  
  137. #define MOTOR0_STEP_PORT 0
  138. #define MOTOR0_STEP_PIN 4
  139.  
  140. #define MOTOR1_STEP_PORT 0
  141. #define MOTOR1_STEP_PIN 6
  142.  
  143. #define MOTOR2_STEP_PORT 0
  144. #define MOTOR2_STEP_PIN 8
  145.  
  146. #define MOTOR3_STEP_PORT 0
  147. #define MOTOR3_STEP_PIN 10
  148.  
  149. #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  150.  
  151. #define MOTOR0_STEP_PORT PORTG
  152. #define MOTOR0_STEP_PIN B00100000
  153.  
  154. #define MOTOR1_STEP_PORT PORTH
  155. #define MOTOR1_STEP_PIN B00001000
  156.  
  157. #define MOTOR2_STEP_PORT PORTH
  158. #define MOTOR2_STEP_PIN B00100000
  159.  
  160. #define MOTOR3_STEP_PORT PORTB
  161. #define MOTOR3_STEP_PIN B00010000
  162.  
  163. #if ( PINOUT_VERSION == 2 )
  164.  
  165. #define MOTOR4_STEP_PORT PORTA
  166. #define MOTOR4_STEP_PIN B01000000
  167.  
  168. #define MOTOR5_STEP_PORT PORTC
  169. #define MOTOR5_STEP_PIN B10000000
  170.  
  171. #define MOTOR6_STEP_PORT PORTC
  172. #define MOTOR6_STEP_PIN B00100000
  173.  
  174. #define MOTOR7_STEP_PORT PORTC
  175. #define MOTOR7_STEP_PIN B00001000
  176.  
  177. #elif ( PINOUT_VERSION == 1 )
  178.  
  179. #define MOTOR4_STEP_PORT PORTA
  180. #define MOTOR4_STEP_PIN B00000001
  181.  
  182. #define MOTOR5_STEP_PORT PORTA
  183. #define MOTOR5_STEP_PIN B00000100
  184.  
  185. #define MOTOR6_STEP_PORT PORTA
  186. #define MOTOR6_STEP_PIN B00010000
  187.  
  188. #define MOTOR7_STEP_PORT PORTA
  189. #define MOTOR7_STEP_PIN B01000000
  190.  
  191. #endif
  192.  
  193. #elif defined(BOARD_UNO)
  194.  
  195. #define MOTOR0_STEP_PORT PORTD
  196. #define MOTOR0_STEP_PIN B00000100
  197.  
  198. #define MOTOR1_STEP_PORT PORTD
  199. #define MOTOR1_STEP_PIN B00001000
  200.  
  201. #define MOTOR2_STEP_PORT PORTD
  202. #define MOTOR2_STEP_PIN B00010000
  203.  
  204. #define MOTOR3_STEP_PORT PORTB
  205. #define MOTOR3_STEP_PIN B00010000
  206.  
  207. #endif
  208.  
  209.  
  210.  
  211. /**
  212. * Serial output specialization
  213. */
  214. #if defined(UBRRH)
  215. #define TX_UCSRA UCSRA
  216. #define TX_UDRE UDRE
  217. #define TX_UDR UDR
  218. #else
  219. #define TX_UCSRA UCSR0A
  220. #define TX_UDRE UDRE0
  221. #define TX_UDR UDR0
  222. #endif
  223.  
  224. char txBuf[32];
  225. char *txBufPtr;
  226.  
  227. #define TX_MSG_BUF_SIZE 16
  228.  
  229. #define MSG_STATE_START 0
  230. #define MSG_STATE_CMD 1
  231. #define MSG_STATE_DATA 2
  232. #define MSG_STATE_ERR 3
  233.  
  234. #define MSG_STATE_DONE 100
  235.  
  236. /*
  237. * Command codes from user
  238. */
  239. #define USER_CMD_ARGS 40
  240.  
  241. #define CMD_NONE 0
  242. #define CMD_HI 10
  243. #define CMD_MS 30
  244. #define CMD_NP 31
  245. #define CMD_MM 40 // move motor
  246. #define CMD_PR 41 // pulse rate
  247. #define CMD_SM 42 // stop motor
  248. #define CMD_MP 43 // motor position
  249. #define CMD_ZM 44 // zero motor
  250. #define CMD_SA 50 // stop all (hard)
  251. #define CMD_BF 60 // blur frame
  252. #define CMD_GO 61 // go!
  253.  
  254. #define CMD_JM 70 // jog motor
  255. #define CMD_IM 71 // inch motor
  256.  
  257.  
  258. #define MSG_HI 01
  259. #define MSG_MM 02
  260. #define MSG_MP 03
  261. #define MSG_MS 04
  262. #define MSG_PR 05
  263. #define MSG_SM 06
  264. #define MSG_SA 07
  265. #define MSG_BF 10
  266. #define MSG_GO 11
  267. #define MSG_JM 12
  268. #define MSG_IM 13
  269.  
  270.  
  271. struct UserCmd
  272. {
  273. byte command;
  274. byte argCount;
  275. int32_t args[USER_CMD_ARGS];
  276. } ;
  277.  
  278. /*
  279. * Message state machine variables.
  280. */
  281. byte lastUserData;
  282. int msgState;
  283. int msgNumberSign;
  284. UserCmd userCmd;
  285.  
  286.  
  287. struct txMsg
  288. {
  289. byte msg;
  290. byte motor;
  291. };
  292.  
  293. struct TxMsgBuffer
  294. {
  295. txMsg buffer[TX_MSG_BUF_SIZE];
  296. byte head;
  297. byte tail;
  298. };
  299.  
  300. TxMsgBuffer txMsgBuffer;
  301.  
  302.  
  303. /*
  304. Motor data.
  305. */
  306.  
  307. uint16_t motorAccumulator0;
  308. uint16_t motorAccumulator1;
  309. uint16_t motorAccumulator2;
  310. uint16_t motorAccumulator3;
  311. #if MOTOR_COUNT > 4
  312. uint16_t motorAccumulator4;
  313. uint16_t motorAccumulator5;
  314. uint16_t motorAccumulator6;
  315. uint16_t motorAccumulator7;
  316. #endif
  317. uint16_t* motorAccumulator[MOTOR_COUNT] =
  318. {
  319. &motorAccumulator0, &motorAccumulator1, &motorAccumulator2, &motorAccumulator3,
  320. #if MOTOR_COUNT > 4
  321. &motorAccumulator4, &motorAccumulator5, &motorAccumulator6, &motorAccumulator7
  322. #endif
  323. };
  324.  
  325. uint16_t motorMoveSteps0;
  326. uint16_t motorMoveSteps1;
  327. uint16_t motorMoveSteps2;
  328. uint16_t motorMoveSteps3;
  329. #if MOTOR_COUNT > 4
  330. uint16_t motorMoveSteps4;
  331. uint16_t motorMoveSteps5;
  332. uint16_t motorMoveSteps6;
  333. uint16_t motorMoveSteps7;
  334. #endif
  335. uint16_t* motorMoveSteps[MOTOR_COUNT] =
  336. {
  337. &motorMoveSteps0, &motorMoveSteps1, &motorMoveSteps2, &motorMoveSteps3,
  338. #if MOTOR_COUNT > 4
  339. &motorMoveSteps4, &motorMoveSteps5, &motorMoveSteps6, &motorMoveSteps7
  340. #endif
  341. };
  342.  
  343.  
  344. uint16_t motorMoveSpeed0;
  345. uint16_t motorMoveSpeed1;
  346. uint16_t motorMoveSpeed2;
  347. uint16_t motorMoveSpeed3;
  348. #if MOTOR_COUNT > 4
  349. uint16_t motorMoveSpeed4;
  350. uint16_t motorMoveSpeed5;
  351. uint16_t motorMoveSpeed6;
  352. uint16_t motorMoveSpeed7;
  353. #endif
  354. uint16_t * motorMoveSpeed[MOTOR_COUNT] =
  355. {
  356. &motorMoveSpeed0, &motorMoveSpeed1, &motorMoveSpeed2, &motorMoveSpeed3,
  357. #if MOTOR_COUNT > 4
  358. &motorMoveSpeed4, &motorMoveSpeed5, &motorMoveSpeed6, &motorMoveSpeed7
  359. #endif
  360. };
  361.  
  362. volatile boolean nextMoveLoaded;
  363.  
  364.  
  365. unsigned int velocityUpdateCounter;
  366. byte sendPositionCounter;
  367. boolean hardStopRequested;
  368.  
  369. byte sendPosition = 0;
  370. byte motorMoving = 0;
  371. byte toggleStep = 0;
  372.  
  373.  
  374. #define P2P_MOVE_COUNT 7
  375.  
  376. struct Motor
  377. {
  378. byte stepPin;
  379. byte dirPin;
  380.  
  381. // pre-computed move
  382. float moveTime[P2P_MOVE_COUNT];
  383. int32_t movePosition[P2P_MOVE_COUNT];
  384. float moveVelocity[P2P_MOVE_COUNT];
  385. float moveAcceleration[P2P_MOVE_COUNT];
  386.  
  387. float gomoMoveTime[P2P_MOVE_COUNT];
  388. int32_t gomoMovePosition[P2P_MOVE_COUNT];
  389. float gomoMoveVelocity[P2P_MOVE_COUNT];
  390. float gomoMoveAcceleration[P2P_MOVE_COUNT];
  391.  
  392. int currentMove;
  393. float currentMoveTime;
  394.  
  395. volatile boolean dir;
  396.  
  397. int32_t position;
  398. int32_t destination;
  399. float maxVelocity;
  400. float maxAcceleration;
  401.  
  402. uint16_t nextMotorMoveSteps;
  403. float nextMotorMoveSpeed;
  404.  
  405. };
  406.  
  407. boolean goMoReady;
  408. int goMoDelayTime;
  409.  
  410. Motor motors[MOTOR_COUNT];
  411.  
  412. #ifdef KILL_SWITCH_INTERRUPT
  413. void killSwitch()
  414. {
  415. hardStopRequested = true;
  416. }
  417. #endif
  418.  
  419. // add stepper enable pin
  420. const byte enablePin = 8;
  421. /*
  422. setup() gets called once, at the start of the program.
  423. */
  424. void setup()
  425. {
  426. pinMode(enablePin, OUTPUT);
  427. digitalWrite(enablePin, LOW);
  428. goMoReady = false;
  429. lastUserData = 0;
  430. msgState = MSG_STATE_START;
  431. velocityUpdateCounter = 0;
  432. sendPositionCounter = 10;
  433. nextMoveLoaded = false;
  434. hardStopRequested = false;
  435.  
  436. for (int i = 0; i < 32; i++)
  437. txBuf[i] = 0;
  438.  
  439. txBufPtr = txBuf;
  440.  
  441. #ifdef KILL_SWITCH_INTERRUPT
  442. attachInterrupt(KILL_SWITCH_INTERRUPT, killSwitch, CHANGE);
  443. #endif
  444.  
  445. // initialize motor structures
  446. for (int i = 0; i < MOTOR_COUNT; i++)
  447. {
  448. // setup motor pins - you can customize/modify these after loop
  449. // default sets step/dir pairs together, with first four motors at 4/5, 6/7, 8/9, 10/11
  450. // then, for the Mega boards, it jumps to 28/29, 30/31, 32/33, 34/35
  451. #if ( PINOUT_VERSION == 2 )
  452. motors[i].stepPin = (i * 2) + ( (i < 4) ? 4 : 20 );
  453. #elif ( PINOUT_VERSION == 1 )
  454. motors[i].stepPin = (i * 2) + ( (i < 4) ? 4 : 14 );
  455. #endif
  456.  
  457. motors[i].dirPin = motors[i].stepPin + 1;
  458. motors[i].dir = true; // forward
  459. motors[i].position = 0L;
  460. motors[i].destination = 0L;
  461.  
  462. motors[i].nextMotorMoveSteps = 0;
  463. motors[i].nextMotorMoveSpeed = 0;
  464.  
  465. setPulsesPerSecond(i, 5000);
  466. }
  467.  
  468.  
  469. // set output pins
  470. for (int i = 0; i < MOTOR_COUNT; i++)
  471. {
  472. pinMode(motors[i].stepPin, OUTPUT);
  473. pinMode(motors[i].dirPin, OUTPUT);
  474.  
  475. #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  476.  
  477. // disable PWM
  478. switch (motors[i].stepPin)
  479. {
  480. #if defined(TCCR3A) && defined(COM3B1)
  481. case 4:
  482. TCCR3A &= ~COM3B1;
  483. break;
  484. #endif
  485.  
  486. #if defined(TCCR4A) && defined(COM4A1)
  487. case 6:
  488. TCCR4A &= ~COM4A1;
  489. break;
  490. #endif
  491.  
  492. #if defined(TCCR4A) && defined(COM4C1)
  493. case 8:
  494. TCCR4A &= ~COM4C1;
  495. break;
  496. #endif
  497.  
  498. #if defined(TCCR2A) && defined(COM2A1)
  499. case 10:
  500. TCCR2A &= ~COM2A1;
  501. break;
  502. #endif
  503. }
  504.  
  505. #else
  506.  
  507. switch (motors[i].stepPin)
  508. {
  509. #if defined(TCCR1A) && defined(COM1B1)
  510. case 10:
  511. TCCR1A &= ~COM1B1;
  512. break;
  513. #endif
  514.  
  515. }
  516.  
  517. #endif
  518. }
  519.  
  520. // set initial direction
  521. for (int i = 0; i < MOTOR_COUNT; i++)
  522. {
  523. digitalWrite( motors[i].dirPin, motors[i].dir ? HIGH : LOW );
  524. }
  525.  
  526. // setup serial connection
  527. Serial.begin(57600);
  528.  
  529. sendMessage(MSG_HI, 0);
  530.  
  531. // SET UP interrupt timer
  532. #if defined(BOARD_UNO) || defined(BOARD_MEGA)
  533.  
  534. TCCR1A = 0;
  535. TCCR1B = _BV(WGM13);
  536.  
  537. ICR1 = (F_CPU / 4000000) * TIME_CHUNK; // goes twice as often as time chunk, but every other event turns off pins
  538. TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
  539. TIMSK1 = _BV(TOIE1);
  540. TCCR1B |= _BV(CS10);
  541.  
  542. #elif defined(BOARD_101)
  543.  
  544. CurieTimerOne.start(25, &updateStepDirection);
  545.  
  546. #endif
  547. }
  548.  
  549. #if defined(BOARD_101)
  550. void updateStepDirection(void)
  551. {
  552. #else
  553. ISR(TIMER1_OVF_vect)
  554. {
  555. #endif
  556.  
  557. toggleStep = !toggleStep;
  558.  
  559. if (toggleStep)
  560. {
  561. // MOTOR 1
  562. if (motorMoveSteps0)
  563. {
  564. uint16_t a = motorAccumulator0;
  565. motorAccumulator0 += motorMoveSpeed0;
  566. if (motorAccumulator0 < a)
  567. {
  568. motorMoveSteps0--;
  569.  
  570. PIN_ON(MOTOR0_STEP_PORT, MOTOR0_STEP_PIN);
  571. }
  572. }
  573.  
  574. // MOTOR 2
  575. if (motorMoveSteps1)
  576. {
  577. uint16_t a = motorAccumulator1;
  578. motorAccumulator1 += motorMoveSpeed1;
  579. if (motorAccumulator1 < a)
  580. {
  581. motorMoveSteps1--;
  582.  
  583. PIN_ON(MOTOR1_STEP_PORT, MOTOR1_STEP_PIN);
  584. }
  585. }
  586.  
  587. // MOTOR 3
  588. if (motorMoveSteps2)
  589. {
  590. uint16_t a = motorAccumulator2;
  591. motorAccumulator2 += motorMoveSpeed2;
  592. if (motorAccumulator2 < a)
  593. {
  594. motorMoveSteps2--;
  595.  
  596. PIN_ON(MOTOR2_STEP_PORT, MOTOR2_STEP_PIN);
  597. }
  598. }
  599.  
  600. // MOTOR 4
  601. if (motorMoveSteps3)
  602. {
  603. uint16_t a = motorAccumulator3;
  604. motorAccumulator3 += motorMoveSpeed3;
  605. if (motorAccumulator3 < a)
  606. {
  607. motorMoveSteps3--;
  608.  
  609. PIN_ON(MOTOR3_STEP_PORT, MOTOR3_STEP_PIN);
  610. }
  611. }
  612.  
  613. #if MOTOR_COUNT > 4
  614.  
  615. // MOTOR 5
  616. if (motorMoveSteps4)
  617. {
  618. uint16_t a = motorAccumulator4;
  619. motorAccumulator4 += motorMoveSpeed4;
  620. if (motorAccumulator4 < a)
  621. {
  622. motorMoveSteps4--;
  623.  
  624. PIN_ON(MOTOR4_STEP_PORT, MOTOR4_STEP_PIN);
  625. }
  626. }
  627.  
  628. // MOTOR 6
  629. if (motorMoveSteps5)
  630. {
  631. uint16_t a = motorAccumulator5;
  632. motorAccumulator5 += motorMoveSpeed5;
  633. if (motorAccumulator5 < a)
  634. {
  635. motorMoveSteps5--;
  636.  
  637. PIN_ON(MOTOR5_STEP_PORT, MOTOR5_STEP_PIN);
  638. }
  639. }
  640.  
  641. // MOTOR 7
  642. if (motorMoveSteps6)
  643. {
  644. uint16_t a = motorAccumulator6;
  645. motorAccumulator6 += motorMoveSpeed6;
  646. if (motorAccumulator6 < a)
  647. {
  648. motorMoveSteps6--;
  649.  
  650. PIN_ON(MOTOR6_STEP_PORT, MOTOR6_STEP_PIN);
  651. }
  652. }
  653.  
  654. // MOTOR 8
  655. if (motorMoveSteps7)
  656. {
  657. uint16_t a = motorAccumulator7;
  658. motorAccumulator7 += motorMoveSpeed7;
  659. if (motorAccumulator7 < a)
  660. {
  661. motorMoveSteps7--;
  662.  
  663. PIN_ON(MOTOR7_STEP_PORT, MOTOR7_STEP_PIN);
  664. }
  665. }
  666.  
  667. #endif
  668.  
  669. }
  670. else
  671. {
  672. velocityUpdateCounter++;
  673. if (velocityUpdateCounter == VELOCITY_UPDATE_RATE)
  674. {
  675. velocityUpdateCounter = 0;
  676.  
  677. if (sendPositionCounter)
  678. {
  679. sendPositionCounter--;
  680. }
  681.  
  682. for (int i = 0; i < MOTOR_COUNT; i++)
  683. {
  684. if (*motorMoveSpeed[i] && !motors[i].nextMotorMoveSpeed)
  685. {
  686. bitSet(sendPosition, i);
  687. }
  688.  
  689. *motorMoveSteps[i] = motors[i].nextMotorMoveSteps;
  690. *motorMoveSpeed[i] = motors[i].nextMotorMoveSpeed;
  691. digitalWrite(motors[i].dirPin, motors[i].dir);
  692.  
  693. *motorAccumulator[i] = 65535;
  694. }
  695. nextMoveLoaded = false; // ready for new move
  696. }
  697.  
  698. PIN_OFF(MOTOR0_STEP_PORT, MOTOR0_STEP_PIN);
  699. PIN_OFF(MOTOR1_STEP_PORT, MOTOR1_STEP_PIN);
  700. PIN_OFF(MOTOR2_STEP_PORT, MOTOR2_STEP_PIN);
  701. PIN_OFF(MOTOR3_STEP_PORT, MOTOR3_STEP_PIN);
  702.  
  703. #if MOTOR_COUNT > 4
  704. PIN_OFF(MOTOR4_STEP_PORT, MOTOR4_STEP_PIN);
  705. PIN_OFF(MOTOR5_STEP_PORT, MOTOR5_STEP_PIN);
  706. PIN_OFF(MOTOR6_STEP_PORT, MOTOR6_STEP_PIN);
  707. PIN_OFF(MOTOR7_STEP_PORT, MOTOR7_STEP_PIN);
  708. #endif
  709. }
  710. }
  711.  
  712. /*
  713. * For stepper-motor timing, every clock cycle counts.
  714. */
  715. void loop()
  716. {
  717. int32_t *ramValues = (int32_t *)malloc(sizeof(int32_t) * MOTOR_COUNT);
  718. int32_t *ramNotValues = (int32_t *)malloc(sizeof(int32_t) * MOTOR_COUNT);
  719.  
  720. for (int i = 0; i < MOTOR_COUNT; i++)
  721. {
  722. if (ramValues[i] == ~ramNotValues[i])
  723. {
  724. motors[i].position = motors[i].destination = ramValues[i];
  725. }
  726. }
  727.  
  728. while (true)
  729. {
  730. if (!nextMoveLoaded)
  731. updateMotorVelocities();
  732.  
  733. processSerialCommand();
  734.  
  735. // check if we have serial output
  736. #if defined(BOARD_UNO) || defined(BOARD_MEGA)
  737. if (*txBufPtr)
  738. {
  739. if ((TX_UCSRA) & (1 << TX_UDRE))
  740. {
  741. TX_UDR = *txBufPtr++;
  742.  
  743. // we are done with this msg, get the next one
  744. if (!*txBufPtr)
  745. nextMessage();
  746. }
  747. }
  748. #endif
  749.  
  750. if (!sendPositionCounter)
  751. {
  752. sendPositionCounter = 20;
  753.  
  754. byte i;
  755. for (i = 0; i < MOTOR_COUNT; i++)
  756. {
  757. if (bitRead(motorMoving, i) || bitRead(sendPosition, i))
  758. {
  759. sendMessage(MSG_MP, i);
  760. ramValues[i] = motors[i].position;
  761. ramNotValues[i] = ~motors[i].position;
  762. }
  763. }
  764.  
  765. sendPosition = 0;
  766. }
  767. }
  768. }
  769.  
  770. /**
  771. * Update velocities.
  772. */
  773.  
  774. void updateMotorVelocities()
  775. {
  776. // process hard stop interrupt request
  777. if (hardStopRequested)
  778. {
  779. hardStopRequested = 0;
  780. hardStop();
  781. }
  782.  
  783. for (int m = 0; m < MOTOR_COUNT; m++)
  784. {
  785. Motor *motor = &motors[m];
  786. motor->nextMotorMoveSteps = 0;
  787. motor->nextMotorMoveSpeed = 0;
  788.  
  789. if (bitRead(motorMoving, m))
  790. {
  791. int seg = motor->currentMove;
  792.  
  793. if (motor->moveTime[seg] == 0)
  794. {
  795. bitClear(motorMoving, m);
  796. }
  797. else
  798. {
  799. float originalMoveTime = motor->currentMoveTime;
  800. int originalMove = motor->currentMove;
  801.  
  802. motor->currentMoveTime += 0.05f;
  803.  
  804. if (motor->currentMoveTime >= motor->moveTime[seg])
  805. {
  806. motor->currentMoveTime -= motor->moveTime[seg];
  807. motor->currentMove++;
  808. seg++;
  809. }
  810. float t = motor->currentMoveTime;
  811. int32_t xn = (int32_t)(motor->movePosition[seg] + motor->moveVelocity[seg] * t + motor->moveAcceleration[seg] * t * t); // accel was already multiplied * 0.5
  812.  
  813. int32_t dx = abs(xn - motor->position);
  814.  
  815. if (!dx) // don't change direction flag unless we are actually stepping in new direction
  816. continue;
  817.  
  818. boolean forward = xn > motor->position;
  819.  
  820. if (forward != motor->dir) // direction setup time 1/20th second should be plenty
  821. {
  822. // revert everything except for dir flag
  823. motor->currentMoveTime = originalMoveTime;
  824. motor->currentMove = originalMove;
  825. }
  826. else
  827. {
  828. motor->nextMotorMoveSpeed = max(1, min(65535, dx * 65.6f));
  829. motor->nextMotorMoveSteps = dx;
  830. motor->position = xn;
  831. }
  832.  
  833. motor->dir = forward;
  834. }
  835. }
  836. }
  837. nextMoveLoaded = true;
  838. }
  839.  
  840. /*
  841. * Set up the axis for pulses per second (approximate)
  842. */
  843. void setPulsesPerSecond(int motorIndex, uint16_t pulsesPerSecond)
  844. {
  845. if (pulsesPerSecond > 20000)
  846. pulsesPerSecond = 20000;
  847. if (pulsesPerSecond < 100)
  848. pulsesPerSecond = 100;
  849.  
  850. motors[motorIndex].maxVelocity = pulsesPerSecond;
  851. motors[motorIndex].maxAcceleration = pulsesPerSecond * 0.5f;
  852. }
  853.  
  854.  
  855. void setupMotorMove(int motorIndex, int32_t destination)
  856. {
  857. motors[motorIndex].destination = destination;
  858.  
  859. if ( destination != motors[motorIndex].position )
  860. {
  861. calculatePointToPoint(motorIndex, destination);
  862. bitSet(motorMoving, motorIndex);
  863. }
  864.  
  865. }
  866.  
  867.  
  868. void hardStop()
  869. {
  870. // set the destination to the current location, so they won't move any more
  871. for (int i = 0; i < MOTOR_COUNT; i++)
  872. {
  873. stopMotor(i);
  874. }
  875. }
  876.  
  877. void stopMotor(int motorIndex)
  878. {
  879. int32_t delta = (motors[motorIndex].destination - motors[motorIndex].position);
  880. if (!delta)
  881. return;
  882.  
  883. Motor *motor = &motors[motorIndex];
  884. int i;
  885.  
  886. for (i = 0; i < P2P_MOVE_COUNT; i++)
  887. {
  888. motor->moveTime[i] = 0;
  889. motor->moveVelocity[i] = 0;
  890. motor->movePosition[i] = 0;
  891. }
  892.  
  893. float v = VELOCITY_CONVERSION_FACTOR * motors[motorIndex].nextMotorMoveSpeed;
  894. float maxA = motor->maxAcceleration;
  895. float maxV = motor->maxVelocity;
  896.  
  897. if (v > maxV)
  898. v = maxV;
  899.  
  900. if (!motor->dir)
  901. v = -v;
  902.  
  903. float t = fabs(v / maxA);
  904.  
  905. motor->moveTime[0] = t;
  906. motor->movePosition[0] = motor->position;
  907. motor->moveVelocity[0] = v;
  908. motor->moveAcceleration[0] = (v > 0) ? -maxA : maxA;
  909.  
  910. motor->moveTime[1] = 0;
  911. motor->movePosition[1] = (int32_t)(motor->movePosition[0] + motor->moveVelocity[0] * t + 0.5f * motor->moveAcceleration[0] * t * t);
  912. motor->moveVelocity[1] = 0;
  913. motor->moveAcceleration[1] = 0;
  914.  
  915. motor->moveAcceleration[0] *= 0.5f;
  916.  
  917. motor->destination = motor->movePosition[1];
  918.  
  919. motor->currentMoveTime = 0;
  920. motor->currentMove = 0;
  921. }
  922.  
  923. boolean isValidMotor(int motorIndex)
  924. {
  925. return (motorIndex >=0 && motorIndex < MOTOR_COUNT);
  926. }
  927.  
  928.  
  929. void processGoPosition(int motorIndex, int32_t pos)
  930. {
  931. if (motors[motorIndex].position != pos)
  932. {
  933. setupMotorMove(motorIndex, pos);
  934. sendMessage(MSG_MM, motorIndex);
  935. }
  936. else
  937. {
  938. sendMessage(MSG_MP, motorIndex);
  939. }
  940. }
  941.  
  942. /*
  943.  
  944. Command format
  945.  
  946. ASCII
  947. [command two bytes]
  948.  
  949. Version
  950. "hi"
  951. -> "hi 1"
  952.  
  953. zero motor
  954. "zm 1"
  955. -> "z 1"
  956.  
  957. move motor
  958. "mm 1 +1111111111
  959.  
  960. motor position?
  961. mp 1
  962.  
  963. MOTOR STATUS
  964. "ms"
  965. -> "ms [busy motor count]"
  966.  
  967. SET PULSE PER SECOND
  968. pr 1 200
  969.  
  970. STOP MOTOR
  971. sm 1
  972.  
  973. STOP ALL
  974. sa
  975.  
  976. */
  977.  
  978. /*
  979. * int processUserMessage(char data)
  980. *
  981. * Read user data (from virtual com port), processing one byte at a time.
  982. * Implemented with a state machine to reduce memory overhead.
  983. *
  984. * Returns command code for completed command.
  985. */
  986. byte processUserMessage(char data)
  987. {
  988. byte cmd = CMD_NONE;
  989.  
  990. switch (msgState)
  991. {
  992. case MSG_STATE_START:
  993. if (data != '\r' && data != '\n')
  994. {
  995. msgState = MSG_STATE_CMD;
  996. msgNumberSign = 1;
  997. userCmd.command = CMD_NONE;
  998. userCmd.argCount = 0;
  999. userCmd.args[0] = 0;
  1000. }
  1001. break;
  1002.  
  1003. case MSG_STATE_CMD:
  1004. if (lastUserData == 'h' && data == 'i')
  1005. {
  1006. userCmd.command = CMD_HI;
  1007. msgState = MSG_STATE_DONE;
  1008. }
  1009. else if (lastUserData == 'm' && data == 's')
  1010. {
  1011. userCmd.command = CMD_MS;
  1012. msgState = MSG_STATE_DONE;
  1013. }
  1014. else if (lastUserData == 's' && data == 'a')
  1015. {
  1016. userCmd.command = CMD_SA;
  1017. msgState = MSG_STATE_DONE;
  1018. }
  1019. else if (lastUserData == 'm' && data == 'm')
  1020. {
  1021. userCmd.command = CMD_MM;
  1022. msgState = MSG_STATE_DATA;
  1023. }
  1024. else if (lastUserData == 'n' && data == 'p')
  1025. {
  1026. userCmd.command = CMD_NP;
  1027. msgState = MSG_STATE_DATA;
  1028. }
  1029. else if (lastUserData == 'm' && data == 'p')
  1030. {
  1031. userCmd.command = CMD_MP;
  1032. msgState = MSG_STATE_DATA;
  1033. }
  1034. else if (lastUserData == 'z' && data == 'm')
  1035. {
  1036. userCmd.command = CMD_ZM;
  1037. msgState = MSG_STATE_DATA;
  1038. }
  1039. else if (lastUserData == 's' && data == 'm')
  1040. {
  1041. userCmd.command = CMD_SM;
  1042. msgState = MSG_STATE_DATA;
  1043. }
  1044. else if (lastUserData == 'p' && data == 'r')
  1045. {
  1046. userCmd.command = CMD_PR;
  1047. msgState = MSG_STATE_DATA;
  1048. }
  1049. else if (lastUserData == 'b' && data == 'f')
  1050. {
  1051. userCmd.command = CMD_BF;
  1052. msgState = MSG_STATE_DATA;
  1053. }
  1054. else if (lastUserData == 'g' && data == 'o')
  1055. {
  1056. userCmd.command = CMD_GO;
  1057. msgState = MSG_STATE_DONE;
  1058. }
  1059. else if (lastUserData == 'j' && data == 'm') // jm [motor] [destination position]
  1060. {
  1061. userCmd.command = CMD_JM;
  1062. msgState = MSG_STATE_DATA;
  1063. }
  1064. else if (lastUserData == 'i' && data == 'm') // im [motor] [destination position]
  1065. {
  1066. userCmd.command = CMD_IM;
  1067. msgState = MSG_STATE_DATA;
  1068. }
  1069. else
  1070. {
  1071. // error msg? unknown command?
  1072. msgState = MSG_STATE_START;
  1073. }
  1074. break;
  1075.  
  1076. case MSG_STATE_DATA:
  1077. if (((data >= '0' && data <= '9') || data == '-') && lastUserData == ' ')
  1078. {
  1079. userCmd.argCount++;
  1080. if (userCmd.argCount >= USER_CMD_ARGS)
  1081. {
  1082. SERIAL_DEVICE.print("error: too many args\r\n");
  1083. msgState = MSG_STATE_ERR;
  1084. }
  1085. else
  1086. {
  1087. userCmd.args[userCmd.argCount - 1] = 0;
  1088. if (data == '-')
  1089. {
  1090. msgNumberSign = -1;
  1091. }
  1092. else
  1093. {
  1094. msgNumberSign = 1;
  1095. userCmd.args[userCmd.argCount - 1] = (data - '0');
  1096. }
  1097. }
  1098. }
  1099. else if (data >= '0' && data <= '9')
  1100. {
  1101. userCmd.args[userCmd.argCount - 1] = userCmd.args[userCmd.argCount - 1] * 10 + (data - '0');
  1102. }
  1103. else if (data == ' ' || data == '\r')
  1104. {
  1105. if (lastUserData >= '0' && lastUserData <= '9')
  1106. {
  1107. if (userCmd.argCount > 0)
  1108. userCmd.args[userCmd.argCount - 1] *= msgNumberSign;
  1109. }
  1110. if (data == '\r')
  1111. {
  1112. msgState = MSG_STATE_DONE;
  1113. }
  1114. }
  1115. break;
  1116.  
  1117.  
  1118. case MSG_STATE_ERR:
  1119. userCmd.command = CMD_NONE;
  1120. msgState = MSG_STATE_DONE;
  1121. break;
  1122.  
  1123. case MSG_STATE_DONE:
  1124. // wait for newline, then reset
  1125. if (data == '\n' && lastUserData == '\r')
  1126. {
  1127. cmd = userCmd.command;
  1128. msgState = MSG_STATE_START;
  1129. lastUserData = 0;
  1130. }
  1131. break;
  1132.  
  1133. default: // unknown state -> revert to begin
  1134. msgState = MSG_STATE_START;
  1135. lastUserData = 0;
  1136. }
  1137.  
  1138. lastUserData = data;
  1139.  
  1140. return cmd;
  1141. }
  1142.  
  1143. void processSerialCommand()
  1144. {
  1145. byte avail = SERIAL_DEVICE.available();
  1146. byte motor;
  1147. int m;
  1148.  
  1149. for (int i = 0; i < avail; i++)
  1150. {
  1151. int cmd = processUserMessage(SERIAL_DEVICE.read());
  1152.  
  1153. if (cmd != CMD_NONE)
  1154. {
  1155. boolean parseError = false;
  1156.  
  1157. motor = userCmd.args[0] - 1;
  1158.  
  1159. switch (cmd)
  1160. {
  1161. case CMD_HI:
  1162. sendMessage(MSG_HI, 0);
  1163. break;
  1164.  
  1165. case CMD_ZM:
  1166. parseError = (userCmd.argCount != 1 || !isValidMotor(motor));
  1167. if (!parseError)
  1168. {
  1169. motors[motor].position = 0;
  1170. setupMotorMove(motor, 0);
  1171. processGoPosition(motor, 0);
  1172. bitSet(sendPosition, motor);
  1173. }
  1174. break;
  1175.  
  1176. case CMD_MM:
  1177. parseError = (userCmd.argCount != 2 || !isValidMotor(motor));
  1178. if (!parseError)
  1179. {
  1180. processGoPosition(motor, (int32_t)userCmd.args[1]);
  1181. }
  1182. break;
  1183.  
  1184. case CMD_NP:
  1185. parseError = (userCmd.argCount != 2 || !isValidMotor(motor));
  1186. if (!parseError)
  1187. {
  1188. motors[motor].position = userCmd.args[1];
  1189. sendMessage(MSG_MP, motor);
  1190. }
  1191. break;
  1192.  
  1193.  
  1194. case CMD_MP:
  1195. parseError = (userCmd.argCount != 1 || !isValidMotor(motor));
  1196. if (!parseError)
  1197. {
  1198. sendMessage(MSG_MP, motor);
  1199. }
  1200. break;
  1201.  
  1202. case CMD_MS:
  1203. parseError = (userCmd.argCount != 0);
  1204. if (!parseError)
  1205. {
  1206. sendMessage(MSG_MS, 0);
  1207. }
  1208. break;
  1209.  
  1210. case CMD_SM:
  1211. parseError = (userCmd.argCount != 1 || !isValidMotor(motor));
  1212. if (!parseError)
  1213. {
  1214. stopMotor(motor);
  1215. sendMessage(MSG_SM, motor);
  1216. sendMessage(MSG_MP, motor);
  1217. }
  1218. break;
  1219.  
  1220. case CMD_SA:
  1221. parseError = (userCmd.argCount != 0);
  1222. if (!parseError)
  1223. {
  1224. hardStop();
  1225. sendMessage(MSG_SA, 0);
  1226. }
  1227. break;
  1228.  
  1229. case CMD_PR:
  1230. parseError = (userCmd.argCount != 2 || !isValidMotor(motor));
  1231. if (!parseError)
  1232. {
  1233. setPulsesPerSecond(motor, (uint16_t)userCmd.args[1]);
  1234. sendMessage(MSG_PR, motor);
  1235. }
  1236. break;
  1237.  
  1238. case CMD_BF:
  1239. parseError = motorMoving || userCmd.argCount < 5 || ((userCmd.argCount - 2) % 4) != 0;
  1240. if (!parseError)
  1241. {
  1242. goMoDelayTime = 1000;
  1243.  
  1244. int motorCount = (userCmd.argCount - 2) / 4;
  1245.  
  1246. for (m = 0; m < MOTOR_COUNT; m++)
  1247. {
  1248. motors[m].gomoMoveTime[0] = 0.0f;
  1249. }
  1250.  
  1251. for (m = 0; m < motorCount; m++)
  1252. {
  1253. int offset = 2 + m * 4;
  1254. motor = userCmd.args[offset] - 1;
  1255. if (!isValidMotor(motor))
  1256. {
  1257. parseError = true;
  1258. break;
  1259. }
  1260. setupBlur(motor, userCmd.args[0], userCmd.args[1], userCmd.args[offset + 1], userCmd.args[offset + 2], userCmd.args[offset + 3]);
  1261. }
  1262. goMoReady = true;
  1263. sendMessage(MSG_BF, 0);
  1264.  
  1265. }
  1266. break;
  1267.  
  1268. case CMD_GO:
  1269. parseError = motorMoving || (userCmd.argCount > 0) || !goMoReady;
  1270. if (!parseError)
  1271. {
  1272. for (m = 0; m < MOTOR_COUNT; m++)
  1273. {
  1274. if (motors[m].gomoMoveTime[0] != 0)
  1275. {
  1276. int j;
  1277. for (j = 0; j < P2P_MOVE_COUNT; j++)
  1278. {
  1279. motors[m].moveTime[j] = motors[m].gomoMoveTime[j];
  1280. motors[m].movePosition[j] = motors[m].gomoMovePosition[j];
  1281. motors[m].moveVelocity[j] = motors[m].gomoMoveVelocity[j];
  1282. motors[m].moveAcceleration[j] = motors[m].gomoMoveAcceleration[j];
  1283. }
  1284. motors[m].destination = motors[m].gomoMovePosition[4]; // TODO change this!
  1285. motors[m].currentMove = 0;
  1286. bitSet(motorMoving, m);
  1287. }
  1288. }
  1289. updateMotorVelocities();
  1290. noInterrupts();
  1291. velocityUpdateCounter = VELOCITY_UPDATE_RATE - 1;
  1292. interrupts();
  1293. sendMessage(MSG_GO, 0);
  1294. }
  1295. break;
  1296.  
  1297. case CMD_JM:
  1298. parseError = (userCmd.argCount != 2 || !isValidMotor(motor));
  1299. if (!parseError)
  1300. {
  1301. int32_t destination = 0;
  1302. if (jogMotor(motor, userCmd.args[1], &destination))
  1303. {
  1304. if (!bitRead(motorMoving, motor) || destination != motors[motor].destination)
  1305. {
  1306. setupMotorMove(motor, destination);
  1307. }
  1308. }
  1309. sendMessage(MSG_JM, motor);
  1310. }
  1311. break;
  1312.  
  1313. case CMD_IM:
  1314. parseError = (userCmd.argCount != 2 || !isValidMotor(motor));
  1315. if (!parseError)
  1316. {
  1317. inchMotor(motor, userCmd.args[1]);
  1318. sendMessage(MSG_IM, motor);
  1319. }
  1320. break;
  1321.  
  1322. default:
  1323. parseError = true;
  1324. break;
  1325. }
  1326.  
  1327. if (parseError)
  1328. {
  1329. SERIAL_DEVICE.print("parse error\r\n");
  1330. }
  1331. }
  1332. }
  1333. }
  1334.  
  1335.  
  1336. /*
  1337. *
  1338. * Serial transmission.
  1339. *
  1340. */
  1341. void sendMessage(byte msg, byte motorIndex)
  1342. {
  1343. #if defined(BOARD_UNO) || defined(BOARD_MEGA)
  1344.  
  1345. int i = (unsigned int)(txMsgBuffer.head + 1) % TX_MSG_BUF_SIZE;
  1346.  
  1347. if (i != txMsgBuffer.tail)
  1348. {
  1349. txMsgBuffer.buffer[txMsgBuffer.head].msg = msg;
  1350. txMsgBuffer.buffer[txMsgBuffer.head].motor = motorIndex;
  1351. txMsgBuffer.head = i;
  1352.  
  1353. if (!*txBufPtr)
  1354. nextMessage();
  1355. }
  1356.  
  1357. #else
  1358. int i;
  1359.  
  1360. switch (msg)
  1361. {
  1362. case MSG_HI:
  1363. SERIAL_DEVICE.print("hi ");
  1364. SERIAL_DEVICE.print(DFMOCO_VERSION);
  1365. SERIAL_DEVICE.print(" ");
  1366. SERIAL_DEVICE.print(MOTOR_COUNT);
  1367. SERIAL_DEVICE.print(" ");
  1368. SERIAL_DEVICE.print(DFMOCO_VERSION_STRING);
  1369. SERIAL_DEVICE.print("\r\n");
  1370. break;
  1371. case MSG_MM:
  1372. SERIAL_DEVICE.print("mm ");
  1373. SERIAL_DEVICE.print(motorIndex + 1);
  1374. SERIAL_DEVICE.print(" ");
  1375. SERIAL_DEVICE.print(motors[motorIndex].destination);
  1376. SERIAL_DEVICE.print("\r\n");
  1377. break;
  1378. case MSG_MP:
  1379. SERIAL_DEVICE.print("mp ");
  1380. SERIAL_DEVICE.print(motorIndex + 1);
  1381. SERIAL_DEVICE.print(" ");
  1382. SERIAL_DEVICE.print(motors[motorIndex].position);
  1383. SERIAL_DEVICE.print("\r\n");
  1384. break;
  1385. case MSG_MS:
  1386. SERIAL_DEVICE.print("ms ");
  1387. for (i = 0; i < MOTOR_COUNT; i++)
  1388. SERIAL_DEVICE.print(bitRead(motorMoving, i) ? '1' : '0');
  1389. SERIAL_DEVICE.print("\r\n");
  1390. break;
  1391. case MSG_PR:
  1392. SERIAL_DEVICE.print("pr ");
  1393. SERIAL_DEVICE.print(motorIndex + 1);
  1394. SERIAL_DEVICE.print(" ");
  1395. SERIAL_DEVICE.print((uint16_t)motors[motorIndex].maxVelocity);
  1396. SERIAL_DEVICE.print("\r\n");
  1397. break;
  1398. case MSG_SM:
  1399. SERIAL_DEVICE.print("sm ");
  1400. SERIAL_DEVICE.print(motorIndex + 1);
  1401. SERIAL_DEVICE.print("\r\n");
  1402. break;
  1403. case MSG_SA:
  1404. SERIAL_DEVICE.print("sa\r\n");
  1405. break;
  1406. case MSG_BF:
  1407. SERIAL_DEVICE.print("bf ");
  1408. SERIAL_DEVICE.print(goMoDelayTime);
  1409. SERIAL_DEVICE.print("\r\n");
  1410. case MSG_GO:
  1411. SERIAL_DEVICE.print("go\r\n");
  1412. break;
  1413. case MSG_JM:
  1414. SERIAL_DEVICE.print("jm ");
  1415. SERIAL_DEVICE.print(motorIndex + 1);
  1416. SERIAL_DEVICE.print("\r\n");
  1417. break;
  1418. case MSG_IM:
  1419. SERIAL_DEVICE.print("im ");
  1420. SERIAL_DEVICE.print(motorIndex + 1);
  1421. SERIAL_DEVICE.print("\r\n");
  1422. break;
  1423. }
  1424. #endif
  1425. }
  1426.  
  1427. #if defined(BOARD_UNO) || defined(BOARD_MEGA)
  1428. void nextMessage()
  1429. {
  1430. char *bufPtr;
  1431. int i;
  1432.  
  1433. if ((TX_MSG_BUF_SIZE + txMsgBuffer.head - txMsgBuffer.tail) % TX_MSG_BUF_SIZE)
  1434. {
  1435. byte msg = txMsgBuffer.buffer[txMsgBuffer.tail].msg;
  1436. byte motorIndex = txMsgBuffer.buffer[txMsgBuffer.tail].motor;
  1437. txMsgBuffer.tail = (unsigned int)(txMsgBuffer.tail + 1) % TX_MSG_BUF_SIZE;
  1438.  
  1439. switch (msg)
  1440. {
  1441. case MSG_HI:
  1442. sprintf(txBuf, "hi %d %d %s\r\n", DFMOCO_VERSION, MOTOR_COUNT, DFMOCO_VERSION_STRING);
  1443. break;
  1444. case MSG_MM:
  1445. sprintf(txBuf, "mm %d %ld\r\n", motorIndex + 1, motors[motorIndex].destination);
  1446. break;
  1447. case MSG_MP:
  1448. sprintf(txBuf, "mp %d %ld\r\n", motorIndex + 1, motors[motorIndex].position);
  1449. break;
  1450. case MSG_MS:
  1451. sprintf(txBuf, "ms ");
  1452. bufPtr = txBuf + 3;
  1453. for (i = 0; i < MOTOR_COUNT; i++)
  1454. *bufPtr++ = bitRead(motorMoving, i) ? '1' : '0';
  1455. *bufPtr++ = '\r';
  1456. *bufPtr++ = '\n';
  1457. *bufPtr = 0;
  1458. break;
  1459. case MSG_PR:
  1460. sprintf(txBuf, "pr %d %u\r\n", motorIndex + 1, (uint16_t)motors[motorIndex].maxVelocity);
  1461. break;
  1462. case MSG_SM:
  1463. sprintf(txBuf, "sm %d\r\n", motorIndex + 1);
  1464. break;
  1465. case MSG_SA:
  1466. sprintf(txBuf, "sa\r\n");
  1467. break;
  1468. case MSG_BF:
  1469. sprintf(txBuf, "bf %d\r\n", goMoDelayTime);
  1470. break;
  1471. case MSG_GO:
  1472. sprintf(txBuf, "go\r\n");
  1473. break;
  1474. case MSG_JM:
  1475. sprintf(txBuf, "jm %d\r\n", motorIndex + 1);
  1476. break;
  1477. case MSG_IM:
  1478. sprintf(txBuf, "im %d\r\n", motorIndex + 1);
  1479. break;
  1480. }
  1481.  
  1482. txBufPtr = txBuf;
  1483. }
  1484. }
  1485. #endif
  1486.  
  1487. boolean jogMotor(int motorIndex, int32_t target, int32_t * destination)
  1488. {
  1489. Motor *motor = &motors[motorIndex];
  1490. // ideally send motor to distance where decel happens after 2 seconds
  1491. float vi = (motor->dir ? 1 : -1) * VELOCITY_CONVERSION_FACTOR * motor->nextMotorMoveSpeed;
  1492.  
  1493. int dir = (target > motor->position) ? 1 : -1;
  1494. // if switching direction, just stop
  1495. if (motor->nextMotorMoveSpeed && motor->dir * dir < 0)
  1496. {
  1497. stopMotor(motorIndex);
  1498. return false;
  1499. }
  1500. if (target == motor->position)
  1501. {
  1502. return false;
  1503. }
  1504.  
  1505. float maxVelocity = motor->maxVelocity;
  1506. float maxAcceleration = motor->maxAcceleration;
  1507.  
  1508.  
  1509. // given current velocity vi
  1510. // compute distance so that decel starts after 0.5 seconds
  1511. // time to accel
  1512. // time at maxvelocity
  1513. // time to decel
  1514. float accelTime = 0, atMaxVelocityTime = 0;
  1515. if (fabs(vi) < maxVelocity)
  1516. {
  1517. accelTime = (maxVelocity - fabs(vi)) / maxAcceleration;
  1518. if (accelTime < 0.5f)
  1519. {
  1520. atMaxVelocityTime = 0.5f - accelTime;
  1521. }
  1522. else
  1523. {
  1524. accelTime = 0.5f;
  1525. }
  1526. }
  1527. else
  1528. {
  1529. atMaxVelocityTime = 0.5f;
  1530. }
  1531. float maxVelocityReached = fabs(vi) + maxAcceleration * accelTime;
  1532.  
  1533. int32_t delta = fabs(vi) * accelTime + (0.5f * maxAcceleration * accelTime * accelTime);
  1534. delta += atMaxVelocityTime * maxVelocityReached;
  1535. delta += 0.5f * (maxVelocityReached * maxVelocityReached) / maxAcceleration; // = 0.5 * a * t^2 -> t = (v/a)
  1536.  
  1537. int32_t dest = motor->position + dir * delta;
  1538.  
  1539. // now clamp to target
  1540. if ( (dir == 1 && dest > target) || (dir == -1 && dest < target) )
  1541. {
  1542. dest = target;
  1543. }
  1544. *destination = dest;
  1545. return true;
  1546. }
  1547.  
  1548. void inchMotor(int motorIndex, int32_t target)
  1549. {
  1550. Motor *motor = &motors[motorIndex];
  1551. // ideally send motor to distance where decel happens after 2 seconds
  1552.  
  1553. // if switching direction, just stop
  1554. int dir = (target > motor->destination) ? 1 : -1;
  1555.  
  1556. if (motor->nextMotorMoveSpeed)// && motor->dir * dir < 0)
  1557. {
  1558. stopMotor(motorIndex);
  1559. return;
  1560. }
  1561.  
  1562. int32_t dest = motor->destination + dir * 2;
  1563.  
  1564. // now clamp to target
  1565. if ( (dir == 1 && dest > target) || (dir == -1 && dest < target) )
  1566. {
  1567. dest = target;
  1568. }
  1569. //setupMotorMove(motorIndex, dest);
  1570.  
  1571. int i, moveCount;
  1572. moveCount = 0;
  1573.  
  1574. for (i = 0; i < P2P_MOVE_COUNT; i++)
  1575. {
  1576. motor->moveTime[i] = 0;
  1577. motor->moveVelocity[i] = 0;
  1578. motor->moveAcceleration[i] = 0;
  1579. }
  1580. motor->currentMoveTime = 0;
  1581. motor->moveTime[0] = 0.01f;
  1582. motor->movePosition[0] = motor->position;
  1583. motor->movePosition[1] = motor->position + dir * 2;
  1584. motor->currentMove = 0;
  1585.  
  1586. motor->destination = dest;
  1587.  
  1588. if ( dest != motor->position )
  1589. {
  1590. bitSet(motorMoving, motorIndex);
  1591. }
  1592. }
  1593.  
  1594. void calculatePointToPoint(int motorIndex, int32_t destination)
  1595. {
  1596. Motor *motor = &motors[motorIndex];
  1597.  
  1598. int i, moveCount;
  1599. moveCount = 0;
  1600.  
  1601. for (i = 0; i < P2P_MOVE_COUNT; i++)
  1602. {
  1603. motor->moveTime[i] = 0;
  1604. motor->moveVelocity[i] = 0;
  1605. motor->moveAcceleration[i] = 0;
  1606. }
  1607. motor->currentMoveTime = 0;
  1608. motor->movePosition[0] = motor->position;
  1609.  
  1610. float tmax = motor->maxVelocity / motor->maxAcceleration;
  1611. float dmax = motor->maxVelocity * tmax;
  1612.  
  1613. float dist = abs(destination - motor->position);
  1614. int dir = destination > motor->position ? 1 : -1;
  1615.  
  1616. if (motor->nextMotorMoveSpeed > 5) // we need to account for existing velocity
  1617. {
  1618. float vi = (motor->dir ? 1 : -1) * VELOCITY_CONVERSION_FACTOR * motor->nextMotorMoveSpeed;
  1619. float ti = fabs(vi / motor->maxAcceleration);
  1620. float di = 0.5f * motor->maxAcceleration * ti * ti;
  1621.  
  1622. if (vi * dir < 0) // switching directions
  1623. {
  1624. motor->moveTime[moveCount] = ti;
  1625. motor->moveAcceleration[moveCount] = dir * motor->maxAcceleration;
  1626. motor->moveVelocity[moveCount] = vi;
  1627. moveCount++;
  1628.  
  1629. dist += di;
  1630. }
  1631. else if (dist < di) // must decelerate and switch directions
  1632. {
  1633. motor->moveTime[moveCount] = ti;
  1634. motor->moveAcceleration[moveCount] = -dir * motor->maxAcceleration;
  1635. motor->moveVelocity[moveCount] = vi;
  1636. moveCount++;
  1637.  
  1638. dist = (di - dist);
  1639. dir = -dir;
  1640. }
  1641. else // further on in same direction
  1642. {
  1643. dist += di;
  1644. motor->movePosition[0] -= dir * di;
  1645.  
  1646. motor->currentMoveTime = ti;
  1647. }
  1648. }
  1649.  
  1650. float t = tmax;
  1651. if (dist <= dmax)
  1652. {
  1653. t = sqrt(dist / motor->maxAcceleration);
  1654. }
  1655.  
  1656. motor->moveTime[moveCount] = t;
  1657. motor->moveAcceleration[moveCount] = dir * motor->maxAcceleration;
  1658.  
  1659. if (dist > dmax)
  1660. {
  1661. moveCount++;
  1662. dist -= dmax;
  1663. float tconst = dist / motor->maxVelocity;
  1664. motor->moveTime[moveCount] = tconst;
  1665. motor->moveAcceleration[moveCount] = 0;
  1666. }
  1667.  
  1668. moveCount++;
  1669. motor->moveTime[moveCount] = t;
  1670. motor->moveAcceleration[moveCount] = dir * -motor->maxAcceleration;
  1671.  
  1672.  
  1673. for (i = 1; i <= moveCount; i++)
  1674. {
  1675. float t = motor->moveTime[i - 1];
  1676. motor->movePosition[i] = (int32_t)(motor->movePosition[i - 1] + motor->moveVelocity[i - 1] * t + 0.5f * motor->moveAcceleration[i - 1] * t * t);
  1677. motor->moveVelocity[i] = motor->moveVelocity[i - 1] + motor->moveAcceleration[i - 1] * t;
  1678. }
  1679. motor->movePosition[moveCount + 1] = destination;
  1680. for (i = 0; i <= moveCount; i++)
  1681. {
  1682. motor->moveAcceleration[i] *= 0.5f; // pre-multiply here for later position calculation
  1683. }
  1684. motor->currentMove = 0;
  1685.  
  1686. return;
  1687.  
  1688. }
  1689.  
  1690. void setupBlur(int motorIndex, int exposure, int blur, int32_t p0, int32_t p1, int32_t p2)
  1691. {
  1692. Motor *motor = &motors[motorIndex];
  1693. int i;
  1694.  
  1695. float b = blur * 0.001f;
  1696. float expTime = exposure * 0.001f;
  1697.  
  1698. p0 = p1 + b * (p0 - p1);
  1699. p2 = p1 + b * (p2 - p1);
  1700.  
  1701. for (i = 0; i < P2P_MOVE_COUNT; i++)
  1702. {
  1703. motor->gomoMoveTime[i] = 0;
  1704. motor->gomoMoveVelocity[i] = 0;
  1705. motor->gomoMoveAcceleration[i] = 0;
  1706. }
  1707.  
  1708. motor->gomoMovePosition[1] = p0;
  1709. motor->gomoMoveTime[1] = expTime * 0.5f;
  1710. motor->gomoMoveVelocity[1] = (float)(p1 - p0) / (expTime * 0.5f);
  1711.  
  1712. motor->gomoMovePosition[2] = p1;
  1713. motor->gomoMoveTime[2] = expTime * 0.5f;
  1714. motor->gomoMoveVelocity[2] = (float)(p2 - p1) / (expTime * 0.5f);
  1715.  
  1716. // v = a*t -> a = v / t
  1717. float accelTime = 1.0f;
  1718. float a = motor->gomoMoveVelocity[1] / accelTime;
  1719. float dp = 0.5f * a * accelTime * accelTime;
  1720. float sp = p0 - dp; // starting position
  1721.  
  1722. motor->gomoMovePosition[0] = sp;
  1723. motor->gomoMoveTime[0] = accelTime;
  1724. motor->gomoMoveAcceleration[0] = 0.5f * a; // pre-multiplied
  1725.  
  1726. a = motor->gomoMoveVelocity[2] / accelTime;
  1727. dp = 0.5f * a * accelTime * accelTime;
  1728. float fp = p2 + dp;
  1729.  
  1730. motor->gomoMovePosition[3] = p2;
  1731. motor->gomoMoveTime[3] = accelTime;
  1732. motor->gomoMoveVelocity[3] = motor->gomoMoveVelocity[2];
  1733. motor->gomoMoveAcceleration[3] = -0.5f * a; // pre-multiplied
  1734.  
  1735. motor->gomoMovePosition[4] = fp;
  1736.  
  1737. setupMotorMove(motorIndex, sp);
  1738. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!