API tools faq
paste
v12345vtm

dfmoco cnc shield

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