GSaha567/seq_level_training_data
052
1text,length,is_long_context,metric_val,label_metric2"#define DFMOCO_VERSION 13#define DFMOCO_VERSION_STRING ""1.3.0""4 5/*6 DFMoco version 1.3.07 8 Multi-axis motion control.9 For use with the Arc motion control system in Dragonframe 4.10 Generates step and direction signals, which can be sent to stepper motor drivers.11 12 Control up to four axes with an Uno, Duemilanove or 101 board.13 Control up to eight axes with a Mega or Mega 2560.14 15 Version History16 17 Version 1.3.0 Arduino 101 support. Remove non-Arduino support (chipKit, Maple).18 Version 1.2.7 Direction setup time.19 Version 1.2.6 Add PINOUT_VERSION option to use older pinout.20 Version 1.2.5 Fix jogging with low pulse rate.21 Version 1.2.4 Fix pin assignments22 Version 1.2.3 New Position command23 Version 1.2.2 Jog and Inch commands24 Version 1.2.1 Moved step/direction pins for motions 5-8.25 Detects board type automatically.26 Version 1.2.0 Basic go-motion capabilities27 Version 1.1.2 Smooth transitions when changing direction28 Version 1.1.1 Save/restore motor position29 Version 1.1.0 Major rework 30 Version 1.0.2 Moved pulses into interrupt handler31 Version 1.0.1 Added delay for pulse widths 32 Version 1.0.0 Initial public release.33 34 Getting Started:35 36 1. Install IDE (Integrated Development Environment):37 Go to https://www.arduino.cc/en/Main/Software and download the Arduino Software 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 148 PIN 4 step49 PIN 5 direction50 channel 251 PIN 6 step52 PIN 7 direction53 channel 354 PIN 8 step55 PIN 9 direction56 channel 457 PIN 10 step58 PIN 11 direction59 60 channel 561 PIN 28 step62 PIN 29 direction63 channel 664 PIN 30 step65 PIN 31 direction66 channel 767 PIN 32 step68 PIN 33 direction69 channel 870 PIN 34 step71 PIN 35 direction72 */73 74// change this to 1 if you want original pinout for channels 5-875#define PINOUT_VERSION 276 77/*78 This is PINOUT_VERSION 179 80 channel 581 PIN 22 step82 PIN 23 direction83 channel 684 PIN 24 step85 PIN 25 direction86 channel 787 PIN 26 step88 PIN 27 direction89 channel 890 PIN 28 step91 PIN 29 direction92*/93 94// detect board type95#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)96 #define BOARD_MEGA 197#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega168__)98 #define BOARD_UNO 199#elif defined(ARDUINO_ARCH_ARC32) // Intel Curie/101100 #define BOARD_101 1101 #include ""CurieTimerOne.h""102#else103 #error Cannot identify board104#endif105 106// USER: if you want a kill switch, uncomment out the next line by removing the // characters107//#define KILL_SWITCH_INTERRUPT 0108 109#define SERIAL_DEVICE Serial110 111#if defined(BOARD_101)112 #define PIN_ON(port, pin) { digitalWrite(pin, 1); }113 #define PIN_OFF(port, pin) { digitalWrite(pin, 0); }114#else115 #define PIN_ON(port, pin) { port |= pin; }116 #define PIN_OFF(port, pin) { port &= ~pin; }117#endif118 119// Arduino Uno/Duemilanove -> 4 MOTORS MAX120// Arduino Mega 2560 / Mega -> 8 MOTORS MAX121#if defined(BOARD_UNO) || defined(BOARD_101)122#define MOTOR_COUNT 4123#else124#define MOTOR_COUNT 8125#endif126 127#define TIME_CHUNK 50128#define SEND_POSITION_COUNT 20000129 130// update velocities 20 x second131#define VELOCITY_UPDATE_RATE (50000 / TIME_CHUNK)132#define VELOCITY_INC(maxrate) (max(1.0f, maxrate / 70.0f))133#define VELOCITY_CONVERSION_FACTOR 0.30517578125f /* 20 / 65.536f */134 135// setup step and direction pins136#if defined(BOARD_101)137 138 #define MOTOR0_STEP_PORT 0139 #define MOTOR0_STEP_PIN 4140 141 #define MOTOR1_STEP_PORT 0142 #define MOTOR1_STEP_PIN 6143 144 #define MOTOR2_STEP_PORT 0145 #define MOTOR2_STEP_PIN 8146 147 #define MOTOR3_STEP_PORT 0148 #define MOTOR3_STEP_PIN 10149 150#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)151 152 #define MOTOR0_STEP_PORT PORTG153 #define MOTOR0_STEP_PIN B00100000154 155 #define MOTOR1_STEP_PORT PORTH156 #define MOTOR1_STEP_PIN B00001000157 158 #define MOTOR2_STEP_PORT PORTH159 #define MOTOR2_STEP_PIN B00100000160 161 #define MOTOR3_STEP_PORT PORTB162 #define MOTOR3_STEP_PIN B00010000163 164 #if ( PINOUT_VERSION == 2 )165 166 #define MOTOR4_STEP_PORT PORTA167 #define MOTOR4_STEP_PIN B01000000168 169 #define MOTOR5_STEP_PORT PORTC170 #define MOTOR5_STEP_PIN B10000000171 172 #define MOTOR6_STEP_PORT PORTC173 #define MOTOR6_STEP_PIN B00100000174 175 #define MOTOR7_STEP_PORT PORTC176 #define MOTOR7_STEP_PIN B00001000177 178 #elif ( PINOUT_VERSION == 1 )179 180 #define MOTOR4_STEP_PORT PORTA181 #define MOTOR4_STEP_PIN B00000001182 183 #define MOTOR5_STEP_PORT PORTA184 #define MOTOR5_STEP_PIN B00000100185 186 #define MOTOR6_STEP_PORT PORTA187 #define MOTOR6_STEP_PIN B00010000188 189 #define MOTOR7_STEP_PORT PORTA190 #define MOTOR7_STEP_PIN B01000000191 192 #endif193 194#elif defined(BOARD_UNO)195 196 #define MOTOR0_STEP_PORT PORTD197 #define MOTOR0_STEP_PIN B00000100198 199 #define MOTOR1_STEP_PORT PORTD200 #define MOTOR1_STEP_PIN B00001000201 202 #define MOTOR2_STEP_PORT PORTD203 #define MOTOR2_STEP_PIN B00010000204 205 #define MOTOR3_STEP_PORT PORTB206 #define MOTOR3_STEP_PIN B00010000207 208#endif209 210 211 212/**213 * Serial output specialization214 */215#if defined(UBRRH)216#define TX_UCSRA UCSRA217#define TX_UDRE UDRE218#define TX_UDR UDR219#else220#define TX_UCSRA UCSR0A221#define TX_UDRE UDRE0222#define TX_UDR UDR0223#endif224 225char txBuf[32];226char *txBufPtr;227 228#define TX_MSG_BUF_SIZE 16229 230#define MSG_STATE_START 0231#define MSG_STATE_CMD 1232#define MSG_STATE_DATA 2233#define MSG_STATE_ERR 3234 235#define MSG_STATE_DONE 100236 237/*238 * Command codes from user239 */240#define USER_CMD_ARGS 40241 242#define CMD_NONE 0243#define CMD_HI 10244#define CMD_MS 30245#define CMD_NP 31246#define CMD_MM 40 // move motor247#define CMD_PR 41 // pulse rate248#define CMD_SM 42 // stop motor249#define CMD_MP 43 // motor position250#define CMD_ZM 44 // zero motor251#define CMD_SA 50 // stop all (hard)252#define CMD_BF 60 // blur frame253#define CMD_GO 61 // go!254 255#define CMD_JM 70 // jog motor256#define CMD_IM 71 // inch motor257 258 259#define MSG_HI 01260#define MSG_MM 02261#define MSG_MP 03262#define MSG_MS 04263#define MSG_PR 05264#define MSG_SM 06265#define MSG_SA 07266#define MSG_BF 10267#define MSG_GO 11268#define MSG_JM 12269#define MSG_IM 13270 271 272struct UserCmd273{274 byte command;275 byte argCount;276 int32_t args[USER_CMD_ARGS];277} ;278 279/*280 * Message state machine variables.281 */282byte lastUserData;283int msgState;284int msgNumberSign;285UserCmd userCmd;286 287 288struct txMsg289{290 byte msg;291 byte motor;292};293 294struct TxMsgBuffer295{296 txMsg buffer[TX_MSG_BUF_SIZE];297 byte head;298 byte tail;299};300 301TxMsgBuffer txMsgBuffer;302 303 304/*305 Motor data.306 */307 308uint16_t motorAccumulator0;309uint16_t motorAccumulator1;310uint16_t motorAccumulator2;311uint16_t motorAccumulator3;312#if MOTOR_COUNT > 4313uint16_t motorAccumulator4;314uint16_t motorAccumulator5;315uint16_t motorAccumulator6;316uint16_t motorAccumulator7;317#endif318uint16_t* motorAccumulator[MOTOR_COUNT] =319{320 &motorAccumulator0, &motorAccumulator1, &motorAccumulator2, &motorAccumulator3, 321#if MOTOR_COUNT > 4322 &motorAccumulator4, &motorAccumulator5, &motorAccumulator6, &motorAccumulator7 323#endif324};325 326uint16_t motorMoveSteps0;327uint16_t motorMoveSteps1;328uint16_t motorMoveSteps2;329uint16_t motorMoveSteps3;330#if MOTOR_COUNT > 4331uint16_t motorMoveSteps4;332uint16_t motorMoveSteps5;333uint16_t motorMoveSteps6;334uint16_t motorMoveSteps7;335#endif336uint16_t* motorMoveSteps[MOTOR_COUNT] =337{338 &motorMoveSteps0, &motorMoveSteps1, &motorMoveSteps2, &motorMoveSteps3,339#if MOTOR_COUNT > 4340 &motorMoveSteps4, &motorMoveSteps5, &motorMoveSteps6, &motorMoveSteps7341#endif342};343 344 345uint16_t motorMoveSpeed0;346uint16_t motorMoveSpeed1;347uint16_t motorMoveSpeed2;348uint16_t motorMoveSpeed3;349#if MOTOR_COUNT > 4350uint16_t motorMoveSpeed4;351uint16_t motorMoveSpeed5;352uint16_t motorMoveSpeed6;353uint16_t motorMoveSpeed7;354#endif355uint16_t * motorMoveSpeed[MOTOR_COUNT] =356{357 &motorMoveSpeed0, &motorMoveSpeed1, &motorMoveSpeed2, &motorMoveSpeed3,358#if MOTOR_COUNT > 4359 &motorMoveSpeed4, &motorMoveSpeed5, &motorMoveSpeed6, &motorMoveSpeed7360#endif361};362 363volatile boolean nextMoveLoaded;364 365 366unsigned int velocityUpdateCounter;367byte sendPositionCounter;368boolean hardStopRequested;369 370byte sendPosition = 0;371byte motorMoving = 0;372byte toggleStep = 0;373 374 375#define P2P_MOVE_COUNT 7376 377struct Motor378{379 byte stepPin;380 byte dirPin;381 382 // pre-computed move383 float moveTime[P2P_MOVE_COUNT];384 int32_t movePosition[P2P_MOVE_COUNT];385 float moveVelocity[P2P_MOVE_COUNT];386 float moveAcceleration[P2P_MOVE_COUNT];387 388 float gomoMoveTime[P2P_MOVE_COUNT];389 int32_t gomoMovePosition[P2P_MOVE_COUNT];390 float gomoMoveVelocity[P2P_MOVE_COUNT];391 float gomoMoveAcceleration[P2P_MOVE_COUNT];392 393 int currentMove;394 float currentMoveTime;395 396 volatile boolean dir;397 398 int32_t position;399 int32_t destination;400 float maxVelocity;401 float maxAcceleration;402 403 uint16_t nextMotorMoveSteps;404 float nextMotorMoveSpeed;405 406};407 408boolean goMoReady;409int goMoDelayTime;410 411Motor motors[MOTOR_COUNT];412 413#ifdef KILL_SWITCH_INTERRUPT414void killSwitch()415{416 hardStopRequested = true;417}418#endif419 420/*421 * setup() gets called once, at the start of the program.422 */423void setup()424{425 goMoReady = false;426 lastUserData = 0;427 msgState = MSG_STATE_START;428 velocityUpdateCounter = 0;429 sendPositionCounter = 10;430 nextMoveLoaded = false;431 hardStopRequested = false;432 433 for (int i = 0; i < 32; i++)434 txBuf[i] = 0;435 436 txBufPtr = txBuf;437 438 #ifdef KILL_SWITCH_INTERRUPT439 attachInterrupt(KILL_SWITCH_INTERRUPT, killSwitch, CHANGE);440 #endif441 442 // initialize motor structures443 for (int i = 0; i < MOTOR_COUNT; i++)444 {445 // setup motor pins - you can customize/modify these after loop446 // default sets step/dir pairs together, with first four motors at 4/5, 6/7, 8/9, 10/11447 // then, for the Mega boards, it jumps to 28/29, 30/31, 32/33, 34/35448 #if ( PINOUT_VERSION == 2 )449 motors[i].stepPin = (i * 2) + ( (i < 4) ? 4 : 20 );450 #elif ( PINOUT_VERSION == 1 )451 motors[i].stepPin = (i * 2) + ( (i < 4) ? 4 : 14 );452 #endif453 454 motors[i].dirPin = motors[i].stepPin + 1;455 motors[i].dir = true; // forward456 motors[i].position = 0L;457 motors[i].destination = 0L;458 459 motors[i].nextMotorMoveSteps = 0;460 motors[i].nextMotorMoveSpeed = 0;461 462 setPulsesPerSecond(i, 5000);463 }464 465motors[0].stepPin = 2;466motors[0].dirPin = 5;467motors[1].stepPin = 3;468motors[1].dirPin = 6;469motors[2].stepPin = 4;470motors[2].dirPin = 7;471motors[3].stepPin = 12;472motors[3].dirPin = 13; 473 474 // set output pins475 for (int i = 0; i < MOTOR_COUNT; i++)476 {477 pinMode(motors[i].stepPin, OUTPUT);478 pinMode(motors[i].dirPin, OUTPUT);479 480#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)481 482 // disable PWM483 switch (motors[i].stepPin)484 {485 #if defined(TCCR3A) && defined(COM3B1)486 case 4:487 TCCR3A &= ~COM3B1;488 break;489 #endif490 491 #if defined(TCCR4A) && defined(COM4A1)492 case 6:493 TCCR4A &= ~COM4A1;494 break;495 #endif496 497 #if defined(TCCR4A) && defined(COM4C1)498 case 8:499 TCCR4A &= ~COM4C1;500 break;501 #endif502 503 #if defined(TCCR2A) && defined(COM2A1)504 case 10:505 TCCR2A &= ~COM2A1;506 break;507 #endif508 }509 510#else511 512 switch (motors[i].stepPin)513 {514 #if defined(TCCR1A) && defined(COM1B1)515 case 10:516 TCCR1A &= ~COM1B1;517 break;518 #endif519 520 }521 522#endif523 }524 525 // set initial direction526 for (int i = 0; i < MOTOR_COUNT; i++)527 {528 digitalWrite( motors[i].dirPin, motors[i].dir ? HIGH : LOW );529 }530 531 // setup serial connection532 Serial.begin(57600);533 534 sendMessage(MSG_HI, 0);535 536 // SET UP interrupt timer 537 #if defined(BOARD_UNO) || defined(BOARD_MEGA)538 539 TCCR1A = 0;540 TCCR1B = _BV(WGM13);541 542 ICR1 = (F_CPU / 4000000) * TIME_CHUNK; // goes twice as often as time chunk, but every other event turns off pins543 TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));544 TIMSK1 = _BV(TOIE1);545 TCCR1B |= _BV(CS10);546 547 #elif defined(BOARD_101)548 549 CurieTimerOne.start(25, &updateStepDirection);550 551 #endif552}553 554#if defined(BOARD_101)555void updateStepDirection(void)556{557#else558ISR(TIMER1_OVF_vect)559{560#endif561 562 toggleStep = !toggleStep;563 564 if (toggleStep)565 {566 // MOTOR 1567 if (motorMoveSteps0)568 {569 uint16_t a = motorAccumulator0;570 motorAccumulator0 += motorMoveSpeed0;571 if (motorAccumulator0 < a)572 {573 motorMoveSteps0--;574 575 PIN_ON(MOTOR0_STEP_PORT, MOTOR0_STEP_PIN);576 }577 }578 579 // MOTOR 2580 if (motorMoveSteps1)581 {582 uint16_t a = motorAccumulator1;583 motorAccumulator1 += motorMoveSpeed1;584 if (motorAccumulator1 < a)585 {586 motorMoveSteps1--;587 588 PIN_ON(MOTOR1_STEP_PORT, MOTOR1_STEP_PIN);589 }590 }591 592 // MOTOR 3593 if (motorMoveSteps2)594 {595 uint16_t a = motorAccumulator2;596 motorAccumulator2 += motorMoveSpeed2;597 if (motorAccumulator2 < a)598 {599 motorMoveSteps2--;600 601 PIN_ON(MOTOR2_STEP_PORT, MOTOR2_STEP_PIN);602 }603 }604 605 // MOTOR 4606 if (motorMoveSteps3)607 {608 uint16_t a = motorAccumulator3;609 motorAccumulator3 += motorMoveSpeed3;610 if (motorAccumulator3 < a)611 {612 motorMoveSteps3--;613 614 PIN_ON(MOTOR3_STEP_PORT, MOTOR3_STEP_PIN);615 }616 }617 618#if MOTOR_COUNT > 4619 620 // MOTOR 5621 if (motorMoveSteps4)622 {623 uint16_t a = motorAccumulator4;624 motorAccumulator4 += motorMoveSpeed4;625 if (motorAccumulator4 < a)626 {627 motorMoveSteps4--;628 629 PIN_ON(MOTOR4_STEP_PORT, MOTOR4_STEP_PIN);630 }631 }632 633 // MOTOR 6634 if (motorMoveSteps5)635 {636 uint16_t a = motorAccumulator5;637 motorAccumulator5 += motorMoveSpeed5;638 if (motorAccumulator5 < a)639 {640 motorMoveSteps5--;641 642 PIN_ON(MOTOR5_STEP_PORT, MOTOR5_STEP_PIN);643 }644 }645 646 // MOTOR 7647 if (motorMoveSteps6)648 {649 uint16_t a = motorAccumulator6;650 motorAccumulator6 += motorMoveSpeed6;651 if (motorAccumulator6 < a)652 {653 motorMoveSteps6--;654 655 PIN_ON(MOTOR6_STEP_PORT, MOTOR6_STEP_PIN);656 }657 }658 659 // MOTOR 8660 if (motorMoveSteps7)661 {662 uint16_t a = motorAccumulator7;663 motorAccumulator7 += motorMoveSpeed7;664 if (motorAccumulator7 < a)665 {666 motorMoveSteps7--;667 668 PIN_ON(MOTOR7_STEP_PORT, MOTOR7_STEP_PIN);669 }670 }671 672#endif673 674 }675 else676 {677 velocityUpdateCounter++;678 if (velocityUpdateCounter == VELOCITY_UPDATE_RATE)679 {680 velocityUpdateCounter = 0;681 682 if (sendPositionCounter)683 {684 sendPositionCounter--;685 }686 687 for (int i = 0; i < MOTOR_COUNT; i++)688 {689 if (*motorMoveSpeed[i] && !motors[i].nextMotorMoveSpeed)690 {691 bitSet(sendPosition, i);692 }693 694 *motorMoveSteps[i] = motors[i].nextMotorMoveSteps;695 *motorMoveSpeed[i] = motors[i].nextMotorMoveSpeed;696 digitalWrite(motors[i].dirPin, motors[i].dir);697 698 *motorAccumulator[i] = 65535;699 }700 nextMoveLoaded = false; // ready for new move701 }702 703 PIN_OFF(MOTOR0_STEP_PORT, MOTOR0_STEP_PIN);704 PIN_OFF(MOTOR1_STEP_PORT, MOTOR1_STEP_PIN);705 PIN_OFF(MOTOR2_STEP_PORT, MOTOR2_STEP_PIN);706 PIN_OFF(MOTOR3_STEP_PORT, MOTOR3_STEP_PIN);707 708 #if MOTOR_COUNT > 4709 PIN_OFF(MOTOR4_STEP_PORT, MOTOR4_STEP_PIN);710 PIN_OFF(MOTOR5_STEP_PORT, MOTOR5_STEP_PIN);711 PIN_OFF(MOTOR6_STEP_PORT, MOTOR6_STEP_PIN);712 PIN_OFF(MOTOR7_STEP_PORT, MOTOR7_STEP_PIN);713 #endif714 }715}716 717/*718 * For stepper-motor timing, every clock cycle counts.719 */720void loop()721{722 int32_t *ramValues = (int32_t *)malloc(sizeof(int32_t) * MOTOR_COUNT);723 int32_t *ramNotValues = (int32_t *)malloc(sizeof(int32_t) * MOTOR_COUNT);724 725 for (int i = 0; i < MOTOR_COUNT; i++)726 { 727 if (ramValues[i] == ~ramNotValues[i])728 {729 motors[i].position = motors[i].destination = ramValues[i];730 }731 } 732 733 while (true)734 {735 if (!nextMoveLoaded)736 updateMotorVelocities();737 738 processSerialCommand();739 740 // check if we have serial output741 #if defined(BOARD_UNO) || defined(BOARD_MEGA)742 if (*txBufPtr)743 {744 if ((TX_UCSRA) & (1 << TX_UDRE))745 {746 TX_UDR = *txBufPtr++;747 748 // we are done with this msg, get the next one749 if (!*txBufPtr)750 nextMessage();751 }752 }753 #endif754 755 if (!sendPositionCounter)756 {757 sendPositionCounter = 20;758 759 byte i;760 for (i = 0; i < MOTOR_COUNT; i++)761 {762 if (bitRead(motorMoving, i) || bitRead(sendPosition, i))763 {764 sendMessage(MSG_MP, i);765 ramValues[i] = motors[i].position;766 ramNotValues[i] = ~motors[i].position;767 }768 }769 770 sendPosition = 0;771 }772 }773}774 775/**776 * Update velocities.777 */778 779void updateMotorVelocities()780{781 // process hard stop interrupt request782 if (hardStopRequested)783 {784 hardStopRequested = 0;785 hardStop();786 }787 788 for (int m = 0; m < MOTOR_COUNT; m++)789 {790 Motor *motor = &motors[m];791 motor->nextMotorMoveSteps = 0;792 motor->nextMotorMoveSpeed = 0;793 794 if (bitRead(motorMoving, m))795 {796 int seg = motor->currentMove;797 798 if (motor->moveTime[seg] == 0)799 {800 bitClear(motorMoving, m);801 }802 else803 {804 float originalMoveTime = motor->currentMoveTime;805 int originalMove = motor->currentMove;806 807 motor->currentMoveTime += 0.05f;808 809 if (motor->currentMoveTime >= motor->moveTime[seg])810 {811 motor->currentMoveTime -= motor->moveTime[seg];812 motor->currentMove++;813 seg++;814 }815 float t = motor->currentMoveTime;816 int32_t xn = (int32_t)(motor->movePosition[seg] + motor->moveVelocity[seg] * t + motor->moveAcceleration[seg] * t * t); // accel was already multiplied * 0.5817 818 int32_t dx = abs(xn - motor->position);819 820 if (!dx) // don't change direction flag unless we are actually stepping in new direction821 continue;822 823 boolean forward = xn > motor->position;824 825 if (forward != motor->dir) // direction setup time 1/20th second should be plenty826 {827 // revert everything except for dir flag828 motor->currentMoveTime = originalMoveTime;829 motor->currentMove = originalMove;830 }831 else832 {833 motor->nextMotorMoveSpeed = max(1, min(65535, dx * 65.6f));834 motor->nextMotorMoveSteps = dx;835 motor->position = xn;836 }837 838 motor->dir = forward;839 } 840 }841 }842 nextMoveLoaded = true;843}844 845/*846 * Set up the axis for pulses per second (approximate)847 */848void setPulsesPerSecond(int motorIndex, uint16_t pulsesPerSecond)849{850 if (pulsesPerSecond > 20000)851 pulsesPerSecond = 20000;852 if (pulsesPerSecond < 100)853 pulsesPerSecond = 100;854 855 motors[motorIndex].maxVelocity = pulsesPerSecond;856 motors[motorIndex].maxAcceleration = pulsesPerSecond * 0.5f; 857}858 859 860void setupMotorMove(int motorIndex, int32_t destination)861{862 motors[motorIndex].destination = destination;863 864 if ( destination != motors[motorIndex].position )865 {866 calculatePointToPoint(motorIndex, destination);867 bitSet(motorMoving, motorIndex);868 }869 870}871 872 873void hardStop()874{875 // set the destination to the current location, so they won't move any more876 for (int i = 0; i < MOTOR_COUNT; i++)877 {878 stopMotor(i);879 }880}881 882void stopMotor(int motorIndex)883{884 int32_t delta = (motors[motorIndex].destination - motors[motorIndex].position);885 if (!delta)886 return;887 888 Motor *motor = &motors[motorIndex];889 int i;890 891 for (i = 0; i < P2P_MOVE_COUNT; i++)892 {893 motor->moveTime[i] = 0;894 motor->moveVelocity[i] = 0;895 motor->movePosition[i] = 0;896 }897 898 float v = VELOCITY_CONVERSION_FACTOR * motors[motorIndex].nextMotorMoveSpeed;899 float maxA = motor->maxAcceleration;900 float maxV = motor->maxVelocity;901 902 if (v > maxV)903 v = maxV;904 905 if (!motor->dir)906 v = -v;907 908 float t = fabs(v / maxA);909 910 motor->moveTime[0] = t;911 motor->movePosition[0] = motor->position;912 motor->moveVelocity[0] = v;913 motor->moveAcceleration[0] = (v > 0) ? -maxA : maxA;914 915 motor->moveTime[1] = 0;916 motor->movePosition[1] = (int32_t)(motor->movePosition[0] + motor->moveVelocity[0] * t + 0.5f * motor->moveAcceleration[0] * t * t);917 motor->moveVelocity[1] = 0;918 motor->moveAcceleration[1] = 0;919 920 motor->moveAcceleration[0] *= 0.5f;921 922 motor->destination = motor->movePosition[1];923 924 motor->currentMoveTime = 0;925 motor->currentMove = 0; 926}927 928boolean isValidMotor(int motorIndex)929{930 return (motorIndex >=0 && motorIndex < MOTOR_COUNT);931}932 933 934void processGoPosition(int motorIndex, int32_t pos)935{936 if (motors[motorIndex].position != pos)937 {938 setupMotorMove(motorIndex, pos);939 sendMessage(MSG_MM, motorIndex);940 }941 else942 {943 sendMessage(MSG_MP, motorIndex);944 }945}946 947/*948 949Command format950 951ASCII952[command two bytes]953 954Version955""hi""956-> ""hi 1""957 958zero motor959""zm 1""960-> ""z 1""961 962move motor963""mm 1 +1111111111964 965motor position?966mp 1967 968MOTOR STATUS969""ms""970-> ""ms [busy motor count]""971 972SET PULSE PER SECOND973pr 1 200974 975STOP MOTOR976sm 1977 978STOP ALL979sa980 981*/982 983/*984 * int processUserMessage(char data)985 *986 * Read user data (from virtual com port), processing one byte at a time.987 * Implemented with a state machine to reduce memory overhead.988 *989 * Returns command code for completed command.990 */991byte processUserMessage(char data)992{993 byte cmd = CMD_NONE;994 995 switch (msgState)996 {997 case MSG_STATE_START:998 if (data != '\\r' && data != '\\n')999 {1000 msgState = MSG_STATE_CMD;1001 msgNumberSign = 1;1002 userCmd.command = CMD_NONE;1003 userCmd.argCount = 0;1004 userCmd.args[0] = 0;1005 }1006 break;1007 1008 case MSG_STATE_CMD:1009 if (lastUserData == 'h' && data == 'i')1010 {1011 userCmd.command = CMD_HI;1012 msgState = MSG_STATE_DONE;1013 }1014 else if (lastUserData == 'm' && data == 's')1015 {1016 userCmd.command = CMD_MS;1017 msgState = MSG_STATE_DONE;1018 }1019 else if (lastUserData == 's' && data == 'a')1020 {1021 userCmd.command = CMD_SA;1022 msgState = MSG_STATE_DONE;1023 }1024 else if (lastUserData == 'm' && data == 'm')1025 {1026 userCmd.command = CMD_MM;1027 msgState = MSG_STATE_DATA;1028 }1029 else if (lastUserData == 'n' && data == 'p')1030 {1031 userCmd.command = CMD_NP;1032 msgState = MSG_STATE_DATA;1033 }1034 else if (lastUserData == 'm' && data == 'p')1035 {1036 userCmd.command = CMD_MP;1037 msgState = MSG_STATE_DATA;1038 }1039 else if (lastUserData == 'z' && data == 'm')1040 {1041 userCmd.command = CMD_ZM;1042 msgState = MSG_STATE_DATA;1043 }1044 else if (lastUserData == 's' && data == 'm')1045 {1046 userCmd.command = CMD_SM;1047 msgState = MSG_STATE_DATA;1048 }1049 else if (lastUserData == 'p' && data == 'r')1050 {1051 userCmd.command = CMD_PR;1052 msgState = MSG_STATE_DATA;1053 }1054 else if (lastUserData == 'b' && data == 'f')1055 {1056 userCmd.command = CMD_BF;1057 msgState = MSG_STATE_DATA;1058 }1059 else if (lastUserData == 'g' && data == 'o')1060 {1061 userCmd.command = CMD_GO;1062 msgState = MSG_STATE_DONE;1063 }1064 else if (lastUserData == 'j' && data == 'm') // jm [motor] [destination position]1065 {1066 userCmd.command = CMD_JM;1067 msgState = MSG_STATE_DATA;1068 }1069 else if (lastUserData == 'i' && data == 'm') // im [motor] [destination position]1070 {1071 userCmd.command = CMD_IM;1072 msgState = MSG_STATE_DATA;1073 }1074 else1075 {1076 // error msg? unknown command?1077 msgState = MSG_STATE_START;1078 }1079 break;1080 1081 case MSG_STATE_DATA:1082 if (((data >= '0' && data <= '9') || data == '-') && lastUserData == ' ')1083 {1084 userCmd.argCount++;1085 if (userCmd.argCount >= USER_CMD_ARGS)1086 {1087 SERIAL_DEVICE.print(""error: too many args\\r\\n"");1088 msgState = MSG_STATE_ERR;1089 }1090 else1091 {1092 userCmd.args[userCmd.argCount - 1] = 0;1093 if (data == '-')1094 {1095 msgNumberSign = -1;1096 }1097 else1098 {1099 msgNumberSign = 1;1100 userCmd.args[userCmd.argCount - 1] = (data - '0');1101 }1102 }1103 }1104 else if (data >= '0' && data <= '9')1105 {1106 userCmd.args[userCmd.argCount - 1] = userCmd.args[userCmd.argCount - 1] * 10 + (data - '0');1107 }1108 else if (data == ' ' || data == '\\r')1109 {1110 if (lastUserData >= '0' && lastUserData <= '9')1111 {1112 if (userCmd.argCount > 0)1113 userCmd.args[userCmd.argCount - 1] *= msgNumberSign;1114 }1115 if (data == '\\r')1116 {1117 msgState = MSG_STATE_DONE;1118 }1119 }1120 break;1121 1122 1123 case MSG_STATE_ERR:1124 userCmd.command = CMD_NONE;1125 msgState = MSG_STATE_DONE;1126 break;1127 1128 case MSG_STATE_DONE:1129 // wait for newline, then reset1130 if (data == '\\n' && lastUserData == '\\r')1131 {1132 cmd = userCmd.command;1133 msgState = MSG_STATE_START;1134 lastUserData = 0;1135 }1136 break;1137 1138 default: // unknown state -> revert to begin1139 msgState = MSG_STATE_START;1140 lastUserData = 0;1141 }1142 1143 lastUserData = data;1144 1145 return cmd;1146}1147 1148void processSerialCommand()1149{1150 byte avail = SERIAL_DEVICE.available();1151 byte motor;1152 int m;1153 1154 for (int i = 0; i < avail; i++)1155 {1156 int cmd = processUserMessage(SERIAL_DEVICE.read());1157 1158 if (cmd != CMD_NONE)1159 {1160 boolean parseError = false;1161 1162 motor = userCmd.args[0] - 1;1163 1164 switch (cmd)1165 {1166 case CMD_HI:1167 sendMessage(MSG_HI, 0);1168 break;1169 1170 case CMD_ZM:1171 parseError = (userCmd.argCount != 1 || !isValidMotor(motor));1172 if (!parseError)1173 {1174 motors[motor].position = 0;1175 setupMotorMove(motor, 0);1176 processGoPosition(motor, 0);1177 bitSet(sendPosition, motor);1178 }1179 break;1180 1181 case CMD_MM:1182 parseError = (userCmd.argCount != 2 || !isValidMotor(motor));1183 if (!parseError)1184 {1185 processGoPosition(motor, (int32_t)userCmd.args[1]);1186 }1187 break;1188 1189 case CMD_NP:1190 parseError = (userCmd.argCount != 2 || !isValidMotor(motor));1191 if (!parseError)1192 {1193 motors[motor].position = userCmd.args[1];1194 sendMessage(MSG_MP, motor);1195 }1196 break;1197 1198 1199 case CMD_MP:1200 parseError = (userCmd.argCount != 1 || !isValidMotor(motor));