├── .gitignore ├── AdvancedFirmata.ino ├── FirmataStepper.cpp ├── FirmataStepper.h └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store -------------------------------------------------------------------------------- /AdvancedFirmata.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * Firmata is a generic protocol for communicating with microcontrollers 3 | * from software on a host computer. It is intended to work with 4 | * any host computer software package. 5 | * 6 | * To download a host software package, please clink on the following link 7 | * to open the download page in your default browser. 8 | * 9 | * http://firmata.org/wiki/Download 10 | */ 11 | 12 | /* 13 | Copyright (C) 2006-2008 Hans-Christoph Steiner. All rights reserved. 14 | Copyright (C) 2010-2011 Paul Stoffregen. All rights reserved. 15 | Copyright (C) 2009 Shigeru Kobayashi. All rights reserved. 16 | Copyright (C) 2009-2014 Jeff Hoefs. All rights reserved. 17 | 18 | This library is free software; you can redistribute it and/or 19 | modify it under the terms of the GNU Lesser General Public 20 | License as published by the Free Software Foundation; either 21 | version 2.1 of the License, or (at your option) any later version. 22 | 23 | See file LICENSE.txt for further informations on licensing terms. 24 | 25 | formatted using the GNU C formatting and indenting 26 | */ 27 | 28 | #include 29 | #include "FirmataStepper.h" 30 | #include 31 | #include 32 | 33 | // move the following defines to Firmata.h? 34 | #define I2C_WRITE B00000000 35 | #define I2C_READ B00001000 36 | #define I2C_READ_CONTINUOUSLY B00010000 37 | #define I2C_STOP_READING B00011000 38 | #define I2C_READ_WRITE_MODE_MASK B00011000 39 | #define I2C_10BIT_ADDRESS_MODE_MASK B00100000 40 | 41 | #define MAX_QUERIES 8 // max number of i2c devices in read continuous mode 42 | #define MINIMUM_SAMPLING_INTERVAL 10 43 | 44 | #define REGISTER_NOT_SPECIFIED -1 45 | 46 | #define MAX_STEPPERS 6 // arbitrary value... may need to adjust 47 | #define STEPPER_DATA 0x72 // move this to Firmata.h 48 | #define STEPPER_CONFIG 0 49 | #define STEPPER_STEP 1 50 | #define STEPPER 0x08 // pin configured for stepper motor 51 | 52 | /*============================================================================== 53 | * GLOBAL VARIABLES 54 | *============================================================================*/ 55 | 56 | /* analog inputs */ 57 | int analogInputsToReport = 0; // bitwise array to store pin reporting 58 | 59 | /* digital input ports */ 60 | byte reportPINs[TOTAL_PORTS]; // 1 = report this port, 0 = silence 61 | byte previousPINs[TOTAL_PORTS]; // previous 8 bits sent 62 | 63 | /* pins configuration */ 64 | byte pinConfig[TOTAL_PINS]; // configuration of every pin 65 | byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else 66 | int pinState[TOTAL_PINS]; // any value that has been written 67 | 68 | /* timer variables */ 69 | unsigned long currentMillis; // store the current value from millis() 70 | unsigned long previousMillis; // for comparison with currentMillis 71 | unsigned int samplingInterval = 19; // how often to run the main loop (in ms) 72 | 73 | /* i2c data */ 74 | struct i2c_device_info 75 | { 76 | byte addr; 77 | int reg; 78 | byte bytes; 79 | }; 80 | 81 | /* for i2c read continuous more */ 82 | i2c_device_info query[MAX_QUERIES]; 83 | 84 | byte i2cRxData[32]; 85 | boolean isI2CEnabled = false; 86 | signed char queryIndex = -1; 87 | unsigned int i2cReadDelayTime = 0; // default delay time between i2c read request and Wire.requestFrom() 88 | 89 | FirmataStepper *stepper[MAX_STEPPERS]; 90 | byte numSteppers = 0; 91 | 92 | Servo servos[MAX_SERVOS]; 93 | byte servoPinMap[TOTAL_PINS]; 94 | byte detachedServos[MAX_SERVOS]; 95 | byte detachedServoCount = 0; 96 | byte servoCount = 0; 97 | 98 | /*============================================================================== 99 | * FUNCTIONS 100 | *============================================================================*/ 101 | 102 | void attachServo(byte pin, int minPulse, int maxPulse) 103 | { 104 | if (servoCount < MAX_SERVOS) 105 | { 106 | // reuse indexes of detached servos until all have been reallocated 107 | if (detachedServoCount > 0) 108 | { 109 | servoPinMap[pin] = detachedServos[detachedServoCount - 1]; 110 | if (detachedServoCount > 0) detachedServoCount--; 111 | } 112 | else 113 | { 114 | servoPinMap[pin] = servoCount; 115 | servoCount++; 116 | } 117 | if (minPulse > 0 && maxPulse > 0) 118 | { 119 | servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse); 120 | } 121 | else 122 | { 123 | servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin)); 124 | } 125 | } 126 | else 127 | { 128 | Firmata.sendString("Max servos attached"); 129 | } 130 | } 131 | 132 | void detachServo(byte pin) 133 | { 134 | servos[servoPinMap[pin]].detach(); 135 | // if we're detaching the last servo, decrement the count 136 | // otherwise store the index of the detached servo 137 | if (servoPinMap[pin] == servoCount && servoCount > 0) 138 | { 139 | servoCount--; 140 | } 141 | else if (servoCount > 0) 142 | { 143 | // keep track of detached servos because we want to reuse their indexes 144 | // before incrementing the count of attached servos 145 | detachedServoCount++; 146 | detachedServos[detachedServoCount - 1] = servoPinMap[pin]; 147 | } 148 | 149 | servoPinMap[pin] = 255; 150 | } 151 | 152 | void readAndReportData(byte address, int theRegister, byte numBytes) 153 | { 154 | // allow I2C requests that don't require a register read 155 | // for example, some devices using an interrupt pin to signify new data available 156 | // do not always require the register read so upon interrupt you call Wire.requestFrom() 157 | if (theRegister != REGISTER_NOT_SPECIFIED) 158 | { 159 | Wire.beginTransmission(address); 160 | #if ARDUINO >= 100 161 | Wire.write((byte)theRegister); 162 | #else 163 | Wire.send((byte)theRegister); 164 | #endif 165 | Wire.endTransmission(); 166 | // do not set a value of 0 167 | if (i2cReadDelayTime > 0) 168 | { 169 | // delay is necessary for some devices such as WiiNunchuck 170 | delayMicroseconds(i2cReadDelayTime); 171 | } 172 | } 173 | else 174 | { 175 | theRegister = 0; // fill the register with a dummy value 176 | } 177 | 178 | Wire.requestFrom(address, numBytes); // all bytes are returned in requestFrom 179 | 180 | // check to be sure correct number of bytes were returned by slave 181 | if (numBytes < Wire.available()) 182 | { 183 | Firmata.sendString("I2C Read Error: Too many bytes received"); 184 | } 185 | else if (numBytes > Wire.available()) 186 | { 187 | Firmata.sendString("I2C Read Error: Too few bytes received"); 188 | } 189 | 190 | i2cRxData[0] = address; 191 | i2cRxData[1] = theRegister; 192 | 193 | for (int i = 0; i < numBytes && Wire.available(); i++) 194 | { 195 | #if ARDUINO >= 100 196 | i2cRxData[2 + i] = Wire.read(); 197 | #else 198 | i2cRxData[2 + i] = Wire.receive(); 199 | #endif 200 | } 201 | 202 | // send slave address, register and received bytes 203 | Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData); 204 | } 205 | 206 | void outputPort(byte portNumber, byte portValue, byte forceSend) 207 | { 208 | // pins not configured as INPUT are cleared to zeros 209 | portValue = portValue & portConfigInputs[portNumber]; 210 | // only send if the value is different than previously sent 211 | if (forceSend || previousPINs[portNumber] != portValue) 212 | { 213 | Firmata.sendDigitalPort(portNumber, portValue); 214 | previousPINs[portNumber] = portValue; 215 | } 216 | } 217 | 218 | /* ----------------------------------------------------------------------------- 219 | * check all the active digital inputs for change of state, then add any events 220 | * to the Serial output queue using Serial.print() */ 221 | void checkDigitalInputs(void) 222 | { 223 | /* Using non-looping code allows constants to be given to readPort(). 224 | * The compiler will apply substantial optimizations if the inputs 225 | * to readPort() are compile-time constants. */ 226 | if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false); 227 | if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false); 228 | if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false); 229 | if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false); 230 | if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false); 231 | if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false); 232 | if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false); 233 | if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false); 234 | if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false); 235 | if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false); 236 | if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false); 237 | if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false); 238 | if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false); 239 | if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false); 240 | if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false); 241 | if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false); 242 | } 243 | 244 | // ----------------------------------------------------------------------------- 245 | /* sets the pin mode to the correct state and sets the relevant bits in the 246 | * two bit-arrays that track Digital I/O and PWM status 247 | */ 248 | void setPinModeCallback(byte pin, int mode) 249 | { 250 | if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) 251 | { 252 | // disable i2c so pins can be used for other functions 253 | // the following if statements should reconfigure the pins properly 254 | disableI2CPins(); 255 | } 256 | if (IS_PIN_DIGITAL(pin) && mode != SERVO) 257 | { 258 | if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) 259 | { 260 | detachServo(pin); 261 | } 262 | } 263 | if (IS_PIN_ANALOG(pin)) 264 | { 265 | reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting 266 | } 267 | if (IS_PIN_DIGITAL(pin)) 268 | { 269 | if (mode == INPUT) 270 | { 271 | portConfigInputs[pin / 8] |= (1 << (pin & 7)); 272 | } 273 | else 274 | { 275 | portConfigInputs[pin / 8] &= ~(1 << (pin & 7)); 276 | } 277 | } 278 | pinState[pin] = 0; 279 | switch (mode) 280 | { 281 | case ANALOG: 282 | if (IS_PIN_ANALOG(pin)) 283 | { 284 | if (IS_PIN_DIGITAL(pin)) 285 | { 286 | pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver 287 | digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups 288 | } 289 | pinConfig[pin] = ANALOG; 290 | } 291 | break; 292 | case INPUT: 293 | if (IS_PIN_DIGITAL(pin)) 294 | { 295 | pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver 296 | digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups 297 | pinConfig[pin] = INPUT; 298 | } 299 | break; 300 | case OUTPUT: 301 | if (IS_PIN_DIGITAL(pin)) 302 | { 303 | digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM 304 | pinMode(PIN_TO_DIGITAL(pin), OUTPUT); 305 | pinConfig[pin] = OUTPUT; 306 | } 307 | break; 308 | case PWM: 309 | if (IS_PIN_PWM(pin)) 310 | { 311 | pinMode(PIN_TO_PWM(pin), OUTPUT); 312 | analogWrite(PIN_TO_PWM(pin), 0); 313 | pinConfig[pin] = PWM; 314 | } 315 | break; 316 | case SERVO: 317 | if (IS_PIN_DIGITAL(pin)) 318 | { 319 | pinConfig[pin] = SERVO; 320 | if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) 321 | { 322 | // pass -1 for min and max pulse values to use default values set 323 | // by Servo library 324 | attachServo(pin, -1, -1); 325 | } 326 | } 327 | break; 328 | case I2C: 329 | if (IS_PIN_I2C(pin)) 330 | { 331 | // mark the pin as i2c 332 | // the user must call I2C_CONFIG to enable I2C for a device 333 | pinConfig[pin] = I2C; 334 | } 335 | break; 336 | default: 337 | Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM 338 | } 339 | // TODO: save status to EEPROM here, if changed 340 | } 341 | 342 | void analogWriteCallback(byte pin, int value) 343 | { 344 | if (pin < TOTAL_PINS) 345 | { 346 | switch (pinConfig[pin]) 347 | { 348 | case SERVO: 349 | if (IS_PIN_DIGITAL(pin)) 350 | servos[servoPinMap[pin]].write(value); 351 | pinState[pin] = value; 352 | break; 353 | case PWM: 354 | if (IS_PIN_PWM(pin)) 355 | analogWrite(PIN_TO_PWM(pin), value); 356 | pinState[pin] = value; 357 | break; 358 | } 359 | } 360 | } 361 | 362 | void digitalWriteCallback(byte port, int value) 363 | { 364 | byte pin, lastPin, mask = 1, pinWriteMask = 0; 365 | 366 | if (port < TOTAL_PORTS) 367 | { 368 | // create a mask of the pins on this port that are writable. 369 | lastPin = port * 8 + 8; 370 | if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS; 371 | for (pin = port * 8; pin < lastPin; pin++) 372 | { 373 | // do not disturb non-digital pins (eg, Rx & Tx) 374 | if (IS_PIN_DIGITAL(pin)) 375 | { 376 | // only write to OUTPUT and INPUT (enables pullup) 377 | // do not touch pins in PWM, ANALOG, SERVO or other modes 378 | if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) 379 | { 380 | pinWriteMask |= mask; 381 | pinState[pin] = ((byte)value & mask) ? 1 : 0; 382 | } 383 | } 384 | mask = mask << 1; 385 | } 386 | writePort(port, (byte)value, pinWriteMask); 387 | } 388 | } 389 | 390 | 391 | // ----------------------------------------------------------------------------- 392 | /* sets bits in a bit array (int) to toggle the reporting of the analogIns 393 | */ 394 | //void FirmataClass::setAnalogPinReporting(byte pin, byte state) { 395 | //} 396 | void reportAnalogCallback(byte analogPin, int value) 397 | { 398 | if (analogPin < TOTAL_ANALOG_PINS) 399 | { 400 | if (value == 0) 401 | { 402 | analogInputsToReport = analogInputsToReport & ~ (1 << analogPin); 403 | } 404 | else 405 | { 406 | analogInputsToReport = analogInputsToReport | (1 << analogPin); 407 | // Send pin value immediately. This is helpful when connected via 408 | // ethernet, wi-fi or bluetooth so pin states can be known upon 409 | // reconnecting. 410 | Firmata.sendAnalog(analogPin, analogRead(analogPin)); 411 | } 412 | } 413 | // TODO: save status to EEPROM here, if changed 414 | } 415 | 416 | void reportDigitalCallback(byte port, int value) 417 | { 418 | if (port < TOTAL_PORTS) 419 | { 420 | reportPINs[port] = (byte)value; 421 | // Send port value immediately. This is helpful when connected via 422 | // ethernet, wi-fi or bluetooth so pin states can be known upon 423 | // reconnecting. 424 | if (value) outputPort(port, readPort(port, portConfigInputs[port]), true); 425 | } 426 | // do not disable analog reporting on these 8 pins, to allow some 427 | // pins used for digital, others analog. Instead, allow both types 428 | // of reporting to be enabled, but check if the pin is configured 429 | // as analog when sampling the analog inputs. Likewise, while 430 | // scanning digital pins, portConfigInputs will mask off values from any 431 | // pins configured as analog 432 | } 433 | 434 | /*============================================================================== 435 | * SYSEX-BASED commands 436 | *============================================================================*/ 437 | 438 | void sysexCallback(byte command, byte argc, byte *argv) 439 | { 440 | byte mode; 441 | byte slaveAddress; 442 | byte data; 443 | int slaveRegister; 444 | unsigned int delayTime; 445 | 446 | switch (command) 447 | { 448 | case I2C_REQUEST: 449 | mode = argv[1] & I2C_READ_WRITE_MODE_MASK; 450 | if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) 451 | { 452 | Firmata.sendString("10-bit addressing not supported"); 453 | return; 454 | } 455 | else 456 | { 457 | slaveAddress = argv[0]; 458 | } 459 | 460 | switch (mode) 461 | { 462 | case I2C_WRITE: 463 | Wire.beginTransmission(slaveAddress); 464 | for (byte i = 2; i < argc; i += 2) 465 | { 466 | data = argv[i] + (argv[i + 1] << 7); 467 | #if ARDUINO >= 100 468 | Wire.write(data); 469 | #else 470 | Wire.send(data); 471 | #endif 472 | } 473 | Wire.endTransmission(); 474 | delayMicroseconds(70); 475 | break; 476 | case I2C_READ: 477 | if (argc == 6) 478 | { 479 | // a slave register is specified 480 | slaveRegister = argv[2] + (argv[3] << 7); 481 | data = argv[4] + (argv[5] << 7); // bytes to read 482 | } 483 | else 484 | { 485 | // a slave register is NOT specified 486 | slaveRegister = REGISTER_NOT_SPECIFIED; 487 | data = argv[2] + (argv[3] << 7); // bytes to read 488 | } 489 | readAndReportData(slaveAddress, (int)slaveRegister, data); 490 | break; 491 | case I2C_READ_CONTINUOUSLY: 492 | if ((queryIndex + 1) >= MAX_QUERIES) 493 | { 494 | // too many queries, just ignore 495 | Firmata.sendString("too many queries"); 496 | break; 497 | } 498 | if (argc == 6) 499 | { 500 | // a slave register is specified 501 | slaveRegister = argv[2] + (argv[3] << 7); 502 | data = argv[4] + (argv[5] << 7); // bytes to read 503 | } 504 | else 505 | { 506 | // a slave register is NOT specified 507 | slaveRegister = (int)REGISTER_NOT_SPECIFIED; 508 | data = argv[2] + (argv[3] << 7); // bytes to read 509 | } 510 | queryIndex++; 511 | query[queryIndex].addr = slaveAddress; 512 | query[queryIndex].reg = slaveRegister; 513 | query[queryIndex].bytes = data; 514 | break; 515 | case I2C_STOP_READING: 516 | byte queryIndexToSkip; 517 | // if read continuous mode is enabled for only 1 i2c device, disable 518 | // read continuous reporting for that device 519 | if (queryIndex <= 0) 520 | { 521 | queryIndex = -1; 522 | } 523 | else 524 | { 525 | // if read continuous mode is enabled for multiple devices, 526 | // determine which device to stop reading and remove it's data from 527 | // the array, shifiting other array data to fill the space 528 | for (byte i = 0; i < queryIndex + 1; i++) 529 | { 530 | if (query[i].addr == slaveAddress) 531 | { 532 | queryIndexToSkip = i; 533 | break; 534 | } 535 | } 536 | 537 | for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) 538 | { 539 | if (i < MAX_QUERIES) 540 | { 541 | query[i].addr = query[i + 1].addr; 542 | query[i].reg = query[i + 1].reg; 543 | query[i].bytes = query[i + 1].bytes; 544 | } 545 | } 546 | queryIndex--; 547 | } 548 | break; 549 | default: 550 | break; 551 | } 552 | break; 553 | case I2C_CONFIG: 554 | delayTime = (argv[0] + (argv[1] << 7)); 555 | 556 | if (delayTime > 0) 557 | { 558 | i2cReadDelayTime = delayTime; 559 | } 560 | 561 | if (!isI2CEnabled) 562 | { 563 | enableI2CPins(); 564 | } 565 | 566 | break; 567 | case SERVO_CONFIG: 568 | if (argc > 4) 569 | { 570 | // these vars are here for clarity, they'll optimized away by the compiler 571 | byte pin = argv[0]; 572 | int minPulse = argv[1] + (argv[2] << 7); 573 | int maxPulse = argv[3] + (argv[4] << 7); 574 | 575 | if (IS_PIN_DIGITAL(pin)) 576 | { 577 | if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) 578 | { 579 | detachServo(pin); 580 | } 581 | attachServo(pin, minPulse, maxPulse); 582 | setPinModeCallback(pin, SERVO); 583 | } 584 | } 585 | break; 586 | 587 | case STEPPER_DATA: 588 | byte stepCommand, deviceNum, directionPin, stepPin, stepDirection; 589 | byte interface, interfaceType; 590 | byte motorPin3, motorPin4; 591 | unsigned int stepsPerRev; 592 | long numSteps; 593 | int stepSpeed; 594 | int accel; 595 | int decel; 596 | 597 | stepCommand = argv[0]; 598 | deviceNum = argv[1]; 599 | 600 | if (deviceNum < MAX_STEPPERS) 601 | { 602 | if (stepCommand == STEPPER_CONFIG) 603 | { 604 | interface = argv[2]; // upper 4 bits are the stepDelay, lower 4 bits are the interface type 605 | interfaceType = interface & 0x0F; // the interface type is specified by the lower 4 bits 606 | stepsPerRev = (argv[3] + (argv[4] << 7)); 607 | 608 | directionPin = argv[5]; // or motorPin1 for TWO_WIRE or FOUR_WIRE interface 609 | stepPin = argv[6]; // // or motorPin2 for TWO_WIRE or FOUR_WIRE interface 610 | setPinModeCallback(directionPin, STEPPER); 611 | setPinModeCallback(stepPin, STEPPER); 612 | 613 | if (!stepper[deviceNum]) 614 | { 615 | numSteppers++; // assumes steppers are added in order 0 -> 5 616 | } 617 | 618 | if (interfaceType == FirmataStepper::DRIVER || interfaceType == FirmataStepper::TWO_WIRE) 619 | { 620 | stepper[deviceNum] = new FirmataStepper(interface, stepsPerRev, directionPin, stepPin); 621 | } 622 | else if (interfaceType == FirmataStepper::FOUR_WIRE) 623 | { 624 | motorPin3 = argv[7]; 625 | motorPin4 = argv[8]; 626 | setPinModeCallback(motorPin3, STEPPER); 627 | setPinModeCallback(motorPin4, STEPPER); 628 | stepper[deviceNum] = new FirmataStepper(interface, stepsPerRev, directionPin, stepPin, motorPin3, motorPin4); 629 | } 630 | } 631 | else if (stepCommand == STEPPER_STEP) 632 | { 633 | stepDirection = argv[2]; 634 | numSteps = (long)argv[3] | ((long)argv[4] << 7) | ((long)argv[5] << 14); 635 | stepSpeed = (argv[6] + (argv[7] << 7)); 636 | 637 | if (stepDirection == 0) 638 | { 639 | numSteps *= -1; 640 | } 641 | if (stepper[deviceNum]) 642 | { 643 | if (argc >= 8 && argc < 12) 644 | { 645 | // num steps, speed (0.01*rad/sec) 646 | stepper[deviceNum]->setStepsToMove(numSteps, stepSpeed); 647 | } 648 | else if (argc == 12) 649 | { 650 | accel = (argv[8] + (argv[9] << 7)); 651 | decel = (argv[10] + (argv[11] << 7)); 652 | // num steps, speed (0.01*rad/sec), accel (0.01*rad/sec^2), decel (0.01*rad/sec^2) 653 | stepper[deviceNum]->setStepsToMove(numSteps, stepSpeed, accel, decel); 654 | } 655 | } 656 | } 657 | } 658 | 659 | break; 660 | 661 | case SAMPLING_INTERVAL: 662 | if (argc > 1) 663 | { 664 | samplingInterval = argv[0] + (argv[1] << 7); 665 | if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) 666 | { 667 | samplingInterval = MINIMUM_SAMPLING_INTERVAL; 668 | } 669 | } 670 | else 671 | { 672 | //Firmata.sendString("Not enough data"); 673 | } 674 | break; 675 | case EXTENDED_ANALOG: 676 | if (argc > 1) 677 | { 678 | int val = argv[1]; 679 | if (argc > 2) val |= (argv[2] << 7); 680 | if (argc > 3) val |= (argv[3] << 14); 681 | analogWriteCallback(argv[0], val); 682 | } 683 | break; 684 | case CAPABILITY_QUERY: 685 | Serial.write(START_SYSEX); 686 | Serial.write(CAPABILITY_RESPONSE); 687 | for (byte pin = 0; pin < TOTAL_PINS; pin++) 688 | { 689 | if (IS_PIN_DIGITAL(pin)) 690 | { 691 | Serial.write((byte)INPUT); 692 | Serial.write(1); 693 | Serial.write((byte)OUTPUT); 694 | Serial.write(1); 695 | } 696 | if (IS_PIN_ANALOG(pin)) 697 | { 698 | Serial.write(ANALOG); 699 | Serial.write(10); 700 | } 701 | if (IS_PIN_PWM(pin)) 702 | { 703 | Serial.write(PWM); 704 | Serial.write(8); 705 | } 706 | if (IS_PIN_DIGITAL(pin)) 707 | { 708 | Serial.write(SERVO); 709 | Serial.write(14); 710 | } 711 | if (IS_PIN_I2C(pin)) 712 | { 713 | Serial.write(I2C); 714 | Serial.write(1); // to do: determine appropriate value 715 | } 716 | if (IS_PIN_DIGITAL(pin)) 717 | { 718 | Serial.write(STEPPER); 719 | Serial.write(21); //21 bits used for number of steps 720 | } 721 | Serial.write(127); 722 | } 723 | Serial.write(END_SYSEX); 724 | break; 725 | case PIN_STATE_QUERY: 726 | if (argc > 0) 727 | { 728 | byte pin = argv[0]; 729 | Serial.write(START_SYSEX); 730 | Serial.write(PIN_STATE_RESPONSE); 731 | Serial.write(pin); 732 | if (pin < TOTAL_PINS) 733 | { 734 | Serial.write((byte)pinConfig[pin]); 735 | Serial.write((byte)pinState[pin] & 0x7F); 736 | if (pinState[pin] & 0xFF80) Serial.write((byte)(pinState[pin] >> 7) & 0x7F); 737 | if (pinState[pin] & 0xC000) Serial.write((byte)(pinState[pin] >> 14) & 0x7F); 738 | } 739 | Serial.write(END_SYSEX); 740 | } 741 | break; 742 | case ANALOG_MAPPING_QUERY: 743 | Serial.write(START_SYSEX); 744 | Serial.write(ANALOG_MAPPING_RESPONSE); 745 | for (byte pin = 0; pin < TOTAL_PINS; pin++) 746 | { 747 | Serial.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127); 748 | } 749 | Serial.write(END_SYSEX); 750 | break; 751 | } 752 | } 753 | 754 | void enableI2CPins() 755 | { 756 | byte i; 757 | // is there a faster way to do this? would probaby require importing 758 | // Arduino.h to get SCL and SDA pins 759 | for (i = 0; i < TOTAL_PINS; i++) 760 | { 761 | if (IS_PIN_I2C(i)) 762 | { 763 | // mark pins as i2c so they are ignore in non i2c data requests 764 | setPinModeCallback(i, I2C); 765 | } 766 | } 767 | 768 | isI2CEnabled = true; 769 | 770 | // is there enough time before the first I2C request to call this here? 771 | Wire.begin(); 772 | } 773 | 774 | /* disable the i2c pins so they can be used for other functions */ 775 | void disableI2CPins() 776 | { 777 | isI2CEnabled = false; 778 | // disable read continuous mode for all devices 779 | queryIndex = -1; 780 | // uncomment the following if or when the end() method is added to Wire library 781 | // Wire.end(); 782 | } 783 | 784 | /*============================================================================== 785 | * SETUP() 786 | *============================================================================*/ 787 | 788 | void systemResetCallback() 789 | { 790 | // initialize a defalt state 791 | // TODO: option to load config from EEPROM instead of default 792 | if (isI2CEnabled) 793 | { 794 | disableI2CPins(); 795 | } 796 | for (byte i = 0; i < TOTAL_PORTS; i++) 797 | { 798 | reportPINs[i] = false; // by default, reporting off 799 | portConfigInputs[i] = 0; // until activated 800 | previousPINs[i] = 0; 801 | } 802 | // pins with analog capability default to analog input 803 | // otherwise, pins default to digital output 804 | for (byte i = 0; i < TOTAL_PINS; i++) 805 | { 806 | if (IS_PIN_ANALOG(i)) 807 | { 808 | // turns off pullup, configures everything 809 | setPinModeCallback(i, ANALOG); 810 | } 811 | else 812 | { 813 | // sets the output to 0, configures portConfigInputs 814 | setPinModeCallback(i, OUTPUT); 815 | } 816 | } 817 | // by default, do not report any analog inputs 818 | analogInputsToReport = 0; 819 | 820 | for (byte i = 0; i < MAX_STEPPERS; i++) 821 | { 822 | if (stepper[i]) 823 | { 824 | free(stepper[i]); 825 | stepper[i] = 0; 826 | } 827 | } 828 | numSteppers = 0; 829 | detachedServoCount = 0; 830 | servoCount = 0; 831 | 832 | /* send digital inputs to set the initial state on the host computer, 833 | * since once in the loop(), this firmware will only send on change */ 834 | /* 835 | TODO: this can never execute, since no pins default to digital input 836 | but it will be needed when/if we support EEPROM stored config 837 | for (byte i=0; i < TOTAL_PORTS; i++) { 838 | outputPort(i, readPort(i, portConfigInputs[i]), true); 839 | } 840 | */ 841 | } 842 | 843 | void setup() 844 | { 845 | Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION); 846 | 847 | Firmata.attach(ANALOG_MESSAGE, analogWriteCallback); 848 | Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback); 849 | Firmata.attach(REPORT_ANALOG, reportAnalogCallback); 850 | Firmata.attach(REPORT_DIGITAL, reportDigitalCallback); 851 | Firmata.attach(SET_PIN_MODE, setPinModeCallback); 852 | Firmata.attach(START_SYSEX, sysexCallback); 853 | Firmata.attach(SYSTEM_RESET, systemResetCallback); 854 | 855 | Firmata.begin(57600); 856 | systemResetCallback(); // reset to default config 857 | } 858 | 859 | /*============================================================================== 860 | * LOOP() 861 | *============================================================================*/ 862 | void loop() 863 | { 864 | byte pin, analogPin; 865 | 866 | /* DIGITALREAD - as fast as possible, check for changes and output them to the 867 | * FTDI buffer using Serial.print() */ 868 | checkDigitalInputs(); 869 | 870 | /* SERIALREAD - processing incoming messagse as soon as possible, while still 871 | * checking digital inputs. */ 872 | while (Firmata.available()) 873 | Firmata.processInput(); 874 | 875 | /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over 876 | * 60 bytes. use a timer to sending an event character every 4 ms to 877 | * trigger the buffer to dump. */ 878 | 879 | // if one or more stepper motors are used, update their position 880 | if (numSteppers > 0) 881 | { 882 | for (int i = 0; i < MAX_STEPPERS; i++) 883 | { 884 | if (stepper[i]) 885 | { 886 | bool done = stepper[i]->update(); 887 | // send command to client application when stepping is complete 888 | if (done) 889 | { 890 | Serial.write(START_SYSEX); 891 | Serial.write(STEPPER_DATA); 892 | Serial.write(i); 893 | Serial.write(END_SYSEX); 894 | } 895 | } 896 | } 897 | } 898 | 899 | currentMillis = millis(); 900 | if (currentMillis - previousMillis > samplingInterval) 901 | { 902 | previousMillis += samplingInterval; 903 | /* ANALOGREAD - do all analogReads() at the configured sampling interval */ 904 | for (pin = 0; pin < TOTAL_PINS; pin++) 905 | { 906 | if (IS_PIN_ANALOG(pin) && pinConfig[pin] == ANALOG) 907 | { 908 | analogPin = PIN_TO_ANALOG(pin); 909 | if (analogInputsToReport & (1 << analogPin)) 910 | { 911 | Firmata.sendAnalog(analogPin, analogRead(analogPin)); 912 | } 913 | } 914 | } 915 | // report i2c data for all device with read continuous mode enabled 916 | if (queryIndex > -1) 917 | { 918 | for (byte i = 0; i < queryIndex + 1; i++) 919 | { 920 | readAndReportData(query[i].addr, query[i].reg, query[i].bytes); 921 | } 922 | } 923 | } 924 | } 925 | 926 | -------------------------------------------------------------------------------- /FirmataStepper.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | FirmataStepper is a simple non-blocking stepper motor library 3 | for 2 and 4 wire bipolar and unipolar stepper motor drive circuits 4 | as well as EasyDriver (http://schmalzhaus.com/EasyDriver/) and 5 | other step + direction drive circuits. 6 | 7 | FirmataStepper (0.2) by Jeff Hoefs 8 | 9 | EasyDriver support based on modifications by Chris Coleman 10 | 11 | Acceleration / Deceleration algorithms and code based on: 12 | app note: http://www.atmel.com/dyn/resources/prod_documents/doc8017.pdf 13 | source code: http://www.atmel.com/dyn/resources/prod_documents/AVR446.zip 14 | 15 | stepMotor function based on Stepper.cpp Stepper library for 16 | Wiring/Arduino created by Tom Igoe, Sebastian Gassner 17 | David Mellis and Noah Shibley. 18 | 19 | Relevant notes from Stepper.cpp: 20 | 21 | When wiring multiple stepper motors to a microcontroller, 22 | you quickly run out of output pins, with each motor requiring 4 connections. 23 | 24 | By making use of the fact that at any time two of the four motor 25 | coils are the inverse of the other two, the number of 26 | control connections can be reduced from 4 to 2. 27 | 28 | A slightly modified circuit around a Darlington transistor array or an L293 H-bridge 29 | connects to only 2 microcontroler pins, inverts the signals received, 30 | and delivers the 4 (2 plus 2 inverted ones) output signals required 31 | for driving a stepper motor. 32 | 33 | The sequence of control signals for 4 control wires is as follows: 34 | 35 | Step C0 C1 C2 C3 36 | 1 1 0 1 0 37 | 2 0 1 1 0 38 | 3 0 1 0 1 39 | 4 1 0 0 1 40 | 41 | The sequence of controls signals for 2 control wires is as follows 42 | (columns C1 and C2 from above): 43 | 44 | Step C0 C1 45 | 1 0 1 46 | 2 1 1 47 | 3 1 0 48 | 4 0 0 49 | 50 | The circuits can be found at 51 | http://www.arduino.cc/en/Tutorial/Stepper 52 | */ 53 | 54 | #include "FirmataStepper.h" 55 | 56 | /** 57 | * Constructor. 58 | * 59 | * Configure a stepper for an EasyDriver or other step + direction interface or 60 | * configure a bipolar or unipolar stepper motor for 2 wire drive mode. 61 | * Configure a bipolar or unipolar stepper for 4 wire drive mode. 62 | * @param interface Lower 3 bits: 63 | * The interface type: FirmataStepper::DRIVER, 64 | * FirmataStepper::TWO_WIRE or FirmataStepper::FOUR_WIRE 65 | * Upper 4 bits: Any bits set = use 2 microsecond delay 66 | * @param steps_per_rev The number of steps to make 1 revolution. 67 | * @param first_pin The direction pin (EasyDriver) or the pin attached to the 68 | * 1st motor coil (2 wire drive mode) 69 | * @param second_pin The step pin (EasyDriver) or the pin attached to the 2nd 70 | * motor coil (2 wire drive mode) 71 | * @param motor_pin_3 The pin attached to the 3rd motor coil 72 | * @param motor_pin_4 The pin attached to the 4th motor coil 73 | */ 74 | FirmataStepper::FirmataStepper(byte interface, 75 | int steps_per_rev, 76 | byte pin1, 77 | byte pin2, 78 | byte pin3, 79 | byte pin4) 80 | { 81 | this->step_number = 0; // which step the motor is on 82 | this->direction = 0; // motor direction 83 | this->last_step_time = 0; // time stamp in ms of the last step taken 84 | this->steps_per_rev = steps_per_rev; // total number of steps for this motor 85 | this->running = false; 86 | this->interface = interface & 0x0F; // default to Easy Stepper (or other step + direction driver) 87 | 88 | // could update this in future to support additional delays if necessary 89 | if (((interface & 0xF0) >> 4) > 0) 90 | { 91 | // high current driver 92 | this->stepDelay = 2; // microseconds 93 | } 94 | else 95 | { 96 | this->stepDelay = 1; // microseconds 97 | } 98 | 99 | this->motor_pin_1 = pin1; 100 | this->motor_pin_2 = pin2; 101 | this->dir_pin = pin1; 102 | this->step_pin = pin2; 103 | 104 | // setup the pins on the microcontroller: 105 | pinMode(this->motor_pin_1, OUTPUT); 106 | pinMode(this->motor_pin_2, OUTPUT); 107 | 108 | if (this->interface == FirmataStepper::FOUR_WIRE) 109 | { 110 | this->motor_pin_3 = pin3; 111 | this->motor_pin_4 = pin4; 112 | pinMode(this->motor_pin_3, OUTPUT); 113 | pinMode(this->motor_pin_4, OUTPUT); 114 | } 115 | 116 | 117 | this->alpha = PI_2 / this->steps_per_rev; 118 | this->at_x100 = (long)(this->alpha * T1_FREQ * 100); 119 | this->ax20000 = (long)(this->alpha * 20000); 120 | this->alpha_x2 = this->alpha * 2; 121 | 122 | } 123 | 124 | /** 125 | * Move the stepper a given number of steps at the specified 126 | * speed (rad/sec), acceleration (rad/sec^2) and deceleration (rad/sec^2). 127 | * 128 | * @param steps_to_move The number ofsteps to move the motor 129 | * @param speed Max speed in 0.01*rad/sec 130 | * @param accel [optional] Acceleration in 0.01*rad/sec^2 131 | * @param decel [optional] Deceleration in 0.01*rad/sec^2 132 | */ 133 | void FirmataStepper::setStepsToMove(long steps_to_move, int speed, int accel, int decel) 134 | { 135 | unsigned long maxStepLimit; 136 | unsigned long accelerationLimit; 137 | 138 | this->step_number = 0; 139 | this->lastAccelDelay = 0; 140 | this->stepCount = 0; 141 | this->rest = 0; 142 | 143 | if (steps_to_move < 0) 144 | { 145 | this->direction = FirmataStepper::CCW; 146 | steps_to_move = -steps_to_move; 147 | } 148 | else 149 | { 150 | this->direction = FirmataStepper::CW; 151 | } 152 | 153 | this->steps_to_move = steps_to_move; 154 | 155 | // set max speed limit, by calc min_delay 156 | // min_delay = (alpha / tt)/w 157 | this->min_delay = this->at_x100 / speed; 158 | 159 | // if acceleration or deceleration are not defined 160 | // start in RUN state and do no decelerate 161 | if (accel == 0 || decel == 0) 162 | { 163 | this->step_delay = this->min_delay; 164 | 165 | this->decel_start = steps_to_move; 166 | this->run_state = FirmataStepper::RUN; 167 | this->accel_count = 0; 168 | this->running = true; 169 | 170 | return; 171 | } 172 | 173 | // if only moving 1 step 174 | if (steps_to_move == 1) 175 | { 176 | 177 | // move one step 178 | this->accel_count = -1; 179 | this->run_state = FirmataStepper::DECEL; 180 | 181 | this->step_delay = this->min_delay; 182 | this->running = true; 183 | } 184 | else if (steps_to_move != 0) 185 | { 186 | // set initial step delay 187 | // step_delay = 1/tt * sqrt(2*alpha/accel) 188 | // step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000 189 | this->step_delay = (long)((T1_FREQ_148 * sqrt(alpha_x2 / accel)) * 1000); 190 | 191 | // find out after how many steps does the speed hit the max speed limit. 192 | // maxSpeedLimit = speed^2 / (2*alpha*accel) 193 | maxStepLimit = (long)speed * speed / (long)(((long)this->ax20000 * accel) / 100); 194 | 195 | // if we hit max spped limit before 0.5 step it will round to 0. 196 | // but in practice we need to move at least 1 step to get any speed at all. 197 | if (maxStepLimit == 0) 198 | { 199 | maxStepLimit = 1; 200 | } 201 | 202 | // find out after how many steps we must start deceleration. 203 | // n1 = (n1+n2)decel / (accel + decel) 204 | accelerationLimit = (long)((steps_to_move * decel) / (accel + decel)); 205 | 206 | // we must accelerate at least 1 step before we can start deceleration 207 | if (accelerationLimit == 0) 208 | { 209 | accelerationLimit = 1; 210 | } 211 | 212 | // use the limit we hit first to calc decel 213 | if (accelerationLimit <= maxStepLimit) 214 | { 215 | this->decel_val = accelerationLimit - steps_to_move; 216 | } 217 | else 218 | { 219 | this->decel_val = -(long)(maxStepLimit * accel) / decel; 220 | } 221 | 222 | // we must decelerate at least 1 step to stop 223 | if (this->decel_val == 0) 224 | { 225 | this->decel_val = -1; 226 | } 227 | 228 | // find step to start deceleration 229 | this->decel_start = steps_to_move + this->decel_val; 230 | 231 | // if the max spped is so low that we don't need to go via acceleration state. 232 | if (this->step_delay <= this->min_delay) 233 | { 234 | this->step_delay = this->min_delay; 235 | this->run_state = FirmataStepper::RUN; 236 | } 237 | else 238 | { 239 | this->run_state = FirmataStepper::ACCEL; 240 | } 241 | 242 | // reset counter 243 | this->accel_count = 0; 244 | this->running = true; 245 | } 246 | } 247 | 248 | 249 | bool FirmataStepper::update() 250 | { 251 | bool done = false; 252 | long newStepDelay; 253 | 254 | unsigned long curTimeVal = micros(); 255 | long timeDiff = curTimeVal - this->last_step_time; 256 | 257 | if (this->running == true && timeDiff >= this->step_delay) 258 | { 259 | 260 | this->last_step_time = curTimeVal; 261 | 262 | switch (this->run_state) 263 | { 264 | case FirmataStepper::STOP: 265 | this->stepCount = 0; 266 | this->rest = 0; 267 | if (this->running) 268 | { 269 | done = true; 270 | } 271 | this->running = false; 272 | break; 273 | 274 | case FirmataStepper::ACCEL: 275 | updateStepPosition(); 276 | this->stepCount++; 277 | this->accel_count++; 278 | newStepDelay = this->step_delay - (((2 * (long)this->step_delay) + this->rest) / (4 * this->accel_count + 1)); 279 | this->rest = ((2 * (long)this->step_delay) + this->rest) % (4 * this->accel_count + 1); 280 | 281 | // check if we should start deceleration 282 | if (this->stepCount >= this->decel_start) 283 | { 284 | this->accel_count = this->decel_val; 285 | this->run_state = FirmataStepper::DECEL; 286 | this->rest = 0; 287 | } 288 | // check if we hit max speed 289 | else if (newStepDelay <= this->min_delay) 290 | { 291 | this->lastAccelDelay = newStepDelay; 292 | newStepDelay = this->min_delay; 293 | this->rest = 0; 294 | this->run_state = FirmataStepper::RUN; 295 | } 296 | break; 297 | 298 | case FirmataStepper::RUN: 299 | updateStepPosition(); 300 | this->stepCount++; 301 | newStepDelay = this->min_delay; 302 | 303 | // if no accel or decel was specified, go directly to STOP state 304 | if (stepCount >= this->steps_to_move) 305 | { 306 | this->run_state = FirmataStepper::STOP; 307 | } 308 | // check if we should start deceleration 309 | else if (this->stepCount >= this->decel_start) 310 | { 311 | this->accel_count = this->decel_val; 312 | // start deceleration with same delay that accel ended with 313 | newStepDelay = this->lastAccelDelay; 314 | this->run_state = FirmataStepper::DECEL; 315 | } 316 | break; 317 | 318 | case FirmataStepper::DECEL: 319 | updateStepPosition(); 320 | this->stepCount++; 321 | this->accel_count++; 322 | 323 | newStepDelay = this->step_delay - (((2 * (long)this->step_delay) + this->rest) / (4 * this->accel_count + 1)); 324 | this->rest = ((2 * (long)this->step_delay) + this->rest) % (4 * this->accel_count + 1); 325 | 326 | if (newStepDelay < 0) newStepDelay = -newStepDelay; 327 | // check if we ar at the last step 328 | if (this->accel_count >= 0) 329 | { 330 | this->run_state = FirmataStepper::STOP; 331 | } 332 | 333 | break; 334 | } 335 | 336 | this->step_delay = newStepDelay; 337 | 338 | } 339 | 340 | return done; 341 | 342 | } 343 | 344 | /** 345 | * Update the step position. 346 | * @private 347 | */ 348 | void FirmataStepper::updateStepPosition() 349 | { 350 | // increment or decrement the step number, 351 | // depending on direction: 352 | if (this->direction == FirmataStepper::CW) 353 | { 354 | this->step_number++; 355 | if (this->step_number >= this->steps_per_rev) 356 | { 357 | this->step_number = 0; 358 | } 359 | } 360 | else 361 | { 362 | if (this->step_number <= 0) 363 | { 364 | this->step_number = this->steps_per_rev; 365 | } 366 | this->step_number--; 367 | } 368 | 369 | // step the motor to step number 0, 1, 2, or 3: 370 | stepMotor(this->step_number % 4, this->direction); 371 | } 372 | 373 | /** 374 | * Moves the motor forward or backwards. 375 | * @param step_num For 2 or 4 wire configurations, this is the current step in 376 | * the 2 or 4 step sequence. 377 | * @param direction The direction of rotation 378 | */ 379 | void FirmataStepper::stepMotor(byte step_num, byte direction) 380 | { 381 | if (this->interface == FirmataStepper::DRIVER) 382 | { 383 | digitalWrite(dir_pin, direction); 384 | delayMicroseconds(this->stepDelay); 385 | digitalWrite(step_pin, LOW); 386 | delayMicroseconds(this->stepDelay); 387 | digitalWrite(step_pin, HIGH); 388 | } 389 | else if (this->interface == FirmataStepper::TWO_WIRE) 390 | { 391 | switch (step_num) 392 | { 393 | case 0: /* 01 */ 394 | digitalWrite(motor_pin_1, LOW); 395 | digitalWrite(motor_pin_2, HIGH); 396 | break; 397 | case 1: /* 11 */ 398 | digitalWrite(motor_pin_1, HIGH); 399 | digitalWrite(motor_pin_2, HIGH); 400 | break; 401 | case 2: /* 10 */ 402 | digitalWrite(motor_pin_1, HIGH); 403 | digitalWrite(motor_pin_2, LOW); 404 | break; 405 | case 3: /* 00 */ 406 | digitalWrite(motor_pin_1, LOW); 407 | digitalWrite(motor_pin_2, LOW); 408 | break; 409 | } 410 | } 411 | else if (this->interface == FirmataStepper::FOUR_WIRE) 412 | { 413 | switch (step_num) 414 | { 415 | case 0: // 1010 416 | digitalWrite(motor_pin_1, HIGH); 417 | digitalWrite(motor_pin_2, LOW); 418 | digitalWrite(motor_pin_3, HIGH); 419 | digitalWrite(motor_pin_4, LOW); 420 | break; 421 | case 1: // 0110 422 | digitalWrite(motor_pin_1, LOW); 423 | digitalWrite(motor_pin_2, HIGH); 424 | digitalWrite(motor_pin_3, HIGH); 425 | digitalWrite(motor_pin_4, LOW); 426 | break; 427 | case 2: //0101 428 | digitalWrite(motor_pin_1, LOW); 429 | digitalWrite(motor_pin_2, HIGH); 430 | digitalWrite(motor_pin_3, LOW); 431 | digitalWrite(motor_pin_4, HIGH); 432 | break; 433 | case 3: //1001 434 | digitalWrite(motor_pin_1, HIGH); 435 | digitalWrite(motor_pin_2, LOW); 436 | digitalWrite(motor_pin_3, LOW); 437 | digitalWrite(motor_pin_4, HIGH); 438 | break; 439 | } 440 | } 441 | } 442 | 443 | /** 444 | * @return The version number of this library. 445 | */ 446 | byte FirmataStepper::version(void) 447 | { 448 | return 2; 449 | } 450 | -------------------------------------------------------------------------------- /FirmataStepper.h: -------------------------------------------------------------------------------- 1 | /* 2 | FirmataStepper is a simple non-blocking stepper motor library 3 | for 2 and 4 wire bipolar and unipolar stepper motor drive circuits 4 | as well as EasyDriver (http://schmalzhaus.com/EasyDriver/) and 5 | other step + direction drive circuits. 6 | 7 | FirmataStepper (0.1) by Jeff Hoefs 8 | 9 | EasyDriver support based on modifications by Chris Coleman 10 | 11 | Acceleration / Deceleration algorithms and code based on: 12 | app note: http://www.atmel.com/dyn/resources/prod_documents/doc8017.pdf 13 | source code: http://www.atmel.com/dyn/resources/prod_documents/AVR446.zip 14 | 15 | stepMotor function based on Stepper.cpp Stepper library for 16 | Wiring/Arduino created by Tom Igoe, Sebastian Gassner 17 | David Mellis and Noah Shibley. 18 | 19 | Relevant notes from Stepper.cpp: 20 | 21 | When wiring multiple stepper motors to a microcontroller, 22 | you quickly run out of output pins, with each motor requiring 4 connections. 23 | 24 | By making use of the fact that at any time two of the four motor 25 | coils are the inverse of the other two, the number of 26 | control connections can be reduced from 4 to 2. 27 | 28 | A slightly modified circuit around a Darlington transistor array or an L293 H-bridge 29 | connects to only 2 microcontroler pins, inverts the signals received, 30 | and delivers the 4 (2 plus 2 inverted ones) output signals required 31 | for driving a stepper motor. 32 | 33 | The sequence of control signals for 4 control wires is as follows: 34 | 35 | Step C0 C1 C2 C3 36 | 1 1 0 1 0 37 | 2 0 1 1 0 38 | 3 0 1 0 1 39 | 4 1 0 0 1 40 | 41 | The sequence of controls signals for 2 control wires is as follows 42 | (columns C1 and C2 from above): 43 | 44 | Step C0 C1 45 | 1 0 1 46 | 2 1 1 47 | 3 1 0 48 | 4 0 0 49 | 50 | The circuits can be found at 51 | http://www.arduino.cc/en/Tutorial/Stepper 52 | */ 53 | 54 | // ensure this library description is only included once 55 | #ifndef FirmataStepper_h 56 | #define FirmataStepper_h 57 | 58 | #if defined(ARDUINO) && ARDUINO >= 100 59 | #include "Arduino.h" 60 | #else 61 | #include "WProgram.h" 62 | #endif 63 | 64 | #define PI_2 2*3.14159 65 | #define T1_FREQ 1000000L // provides the most accurate step delay values 66 | #define T1_FREQ_148 ((long)((T1_FREQ*0.676)/100)) // divided by 100 and scaled by 0.676 67 | 68 | // library interface description 69 | class FirmataStepper 70 | { 71 | public: 72 | FirmataStepper(byte interface = FirmataStepper::DRIVER, 73 | int steps_per_rev = 200, 74 | byte pin1 = 2, 75 | byte pin2 = 3, 76 | byte pin3 = 4, 77 | byte pin4 = 5); 78 | 79 | enum Interface 80 | { 81 | DRIVER = 1, 82 | TWO_WIRE = 2, 83 | FOUR_WIRE = 4 84 | }; 85 | 86 | enum RunState 87 | { 88 | STOP = 0, 89 | ACCEL = 1, 90 | DECEL = 2, 91 | RUN = 3 92 | }; 93 | 94 | enum Direction 95 | { 96 | CCW = 0, 97 | CW = 1 98 | }; 99 | 100 | void setStepsToMove(long steps_to_move, int speed, int accel = 0, int decel = 0); 101 | 102 | // update the stepper position 103 | bool update(); 104 | 105 | byte version(void); 106 | 107 | private: 108 | void stepMotor(byte step_num, byte direction); 109 | void updateStepPosition(); 110 | bool running; 111 | byte interface; // Type of interface: DRIVER, TWO_WIRE or FOUR_WIRE 112 | byte direction; // Direction of rotation 113 | unsigned long step_delay; // delay between steps, in microseconds 114 | int steps_per_rev; // number of steps to make one revolution 115 | long step_number; // which step the motor is on 116 | long steps_to_move; // total number of teps to move 117 | byte stepDelay; // delay between steps (default = 1, increase for high current drivers) 118 | 119 | byte run_state; 120 | int accel_count; 121 | long min_delay; 122 | long decel_start; 123 | int decel_val; 124 | 125 | long lastAccelDelay; 126 | unsigned long stepCount; 127 | unsigned int rest; 128 | 129 | float alpha; // PI * 2 / steps_per_rev 130 | long at_x100; // alpha * T1_FREQ * 100 131 | long ax20000; // alph a* 20000 132 | float alpha_x2; // alpha * 2 133 | 134 | // motor pin numbers: 135 | byte dir_pin; 136 | byte step_pin; 137 | byte motor_pin_1; 138 | byte motor_pin_2; 139 | byte motor_pin_3; 140 | byte motor_pin_4; 141 | 142 | unsigned long last_step_time; // time stamp in microseconds of when the last step was taken 143 | }; 144 | 145 | #endif 146 | 147 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | AdvancedFirmata 2 | =============== 3 | 4 | **Please note:** This repo is no longer actively maintained. Please use [firmatabuilder.com](http://firmatabuilder.com/) and [ConfigurableFirmata](https://github.com/firmata/ConfigurableFirmata) to enable the same (and even additional) features that AdvancedFirmata provided. To enable the same features, from the firmatabuilder website, select the following features: 5 | - DigitalInputFirmata 6 | - DigitalOutputFirmata 7 | - AnalogInputFirmata 8 | - AnalogOutputFirmata 9 | - ServoFirmata 10 | - I2CFirmata 11 | - StepperFirmata 12 | 13 | You may select additional features as well if the Firmata client library you are using supports them. Just be careful not to exceed the memory limits of your Arduino board. 14 | 15 | AdvancedFirmata builds on [StandardFirmata](https://github.com/firmata/arduino/tree/master/examples/StandardFirmata) to add additional functionality such 16 | as Stepper motor support. The idea behind AdvancedFirmata is to add features 17 | that are not yet available in the StandardFirmata (that is included in 18 | Firmata/examples/ in the Arduino IDE) and hopefully add them to the official 19 | Firmata library at a later time. 20 | 21 | Like StandardFirmata (which is just an Arduino Sketch), AdvancedFirmata relies 22 | on the Firmata library included with the Arduino IDE. Therefore Firmata.cpp, 23 | Firmata.h or Boards.h should not be changed in order to add a feature to 24 | AdvancedFirmata. 25 | 26 | In time, some of the featues in AdvancedFirmata may be added to StandardFirmata 27 | and the official Firmata library. 28 | 29 | 30 | To Use 31 | --- 32 | 33 | Clone or download and copy AdvancedFirmata into your Arduino projects/sketch 34 | directory (or anywhere on your hard drive). Open in the Arduino IDE, verify and 35 | upload to your board. 36 | 37 | If you download rather than clone this repository, rename the folder to 38 | "AdvancedFirmata" after unzipping and before copying into your Arduino projects 39 | directory. 40 | 41 | 42 | Features (not included in StandardFirmata) 43 | --- 44 | 45 | - Stepper motor support. AdvancedFirmata can be used to drive bipolar and 46 | unipolar stepper motors. 47 | 48 | 49 | Requested Features 50 | --- 51 | 52 | - Shift in/out support 53 | - Pulse in/out support 54 | - Ethernet support (since Firmata implements the Stream interface) 55 | 56 | 57 | Restrictions 58 | --- 59 | 60 | Due to the large file size, AdvancedFirmata will only run on newer Arduino 61 | boards that have > 16KB of flash memory. This is most Arduino and 62 | arduino-compatible boards released over the past few years. 63 | 64 | 65 | Contributing 66 | --- 67 | 68 | Please create a branch with the name of the feature you are adding (shiftout, 69 | rotaryencoder, etc) before submitting a pull request. --------------------------------------------------------------------------------