diff --git a/libraries/Firmata/Firmata.cpp b/libraries/Firmata/Firmata.cpp index 9a18615c4..31aad5f69 100644 --- a/libraries/Firmata/Firmata.cpp +++ b/libraries/Firmata/Firmata.cpp @@ -60,21 +60,18 @@ FirmataClass::FirmataClass(void) /* begin method for overriding default serial bitrate */ void FirmataClass::begin(void) { - Serial.begin(57600); - blinkVersion(); - delay(300); - printVersion(); + begin(57600); } /* begin method for overriding default serial bitrate */ void FirmataClass::begin(long speed) { - blinkVersion(); #if defined(__AVR_ATmega128__) // Wiring Serial.begin((uint32_t)speed); #else Serial.begin(speed); #endif + blinkVersion(); delay(300); printVersion(); printFirmwareVersion(); diff --git a/libraries/Firmata/Firmata.h b/libraries/Firmata/Firmata.h index 2732fd67b..f3ac161a2 100644 --- a/libraries/Firmata/Firmata.h +++ b/libraries/Firmata/Firmata.h @@ -158,6 +158,7 @@ extern FirmataClass Firmata; #define ANALOG_PORT 2 // port# of analog used as digital #define FIRST_ANALOG_PIN 14 // pin# corresponding to analog 0 #define VERSION_BLINK_PIN 13 // digital pin to blink version on +#define FIRST_SERVO_PIN 2 // pin# of the first servo pin #elif defined(__AVR_ATmega8__) // old Arduinos #define TOTAL_ANALOG_PINS 6 #define TOTAL_DIGITAL_PINS 20 // 14 digital + 6 analog @@ -165,6 +166,7 @@ extern FirmataClass Firmata; #define ANALOG_PORT 2 // port# of analog used as digital #define FIRST_ANALOG_PIN 14 // pin# corresponding to analog 0 #define VERSION_BLINK_PIN 13 // digital pin to blink version on +#define FIRST_SERVO_PIN 2 // pin# of the first servo pin #elif defined(__AVR_ATmega1280__)// Arduino Mega #define TOTAL_ANALOG_PINS 16 #define TOTAL_DIGITAL_PINS 70 // 54 digital + 16 analog @@ -172,13 +174,15 @@ extern FirmataClass Firmata; #define ANALOG_PORT 8 // port# of analog used as digital #define FIRST_ANALOG_PIN 54 // pin# corresponding to analog 0 #define VERSION_BLINK_PIN 13 // digital pin to blink version on +#define FIRST_SERVO_PIN 2 // pin# of the first servo pin #elif defined(__AVR_ATmega128__)// Wiring #define TOTAL_ANALOG_PINS 8 #define TOTAL_DIGITAL_PINS 51 #define TOTAL_PORTS 7 // total number of ports for the board #define ANALOG_PORT 5 // port# of analog used as digital #define FIRST_ANALOG_PIN 40 // pin# corresponding to analog 0 -#define VERSION_BLINK_PIN 13 // digital pin to blink version on +#define VERSION_BLINK_PIN 48 // digital pin to blink version on +#define FIRST_SERVO_PIN 8 // pin# of the first servo pin #elif defined(__AVR_AT90USB162__) // Teensy #define TOTAL_ANALOG_PINS 0 #define TOTAL_DIGITAL_PINS 21 // 21 digital + no analog diff --git a/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde b/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde index 16c8b878a..db61904a8 100644 --- a/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde +++ b/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde @@ -11,9 +11,7 @@ formatted using the GNU C formatting and indenting */ - /* - * TODO: add Servo support using setPinModeCallback(pin, SERVO); * TODO: use Program Control to load stored profiles from EEPROM */ @@ -39,7 +37,7 @@ unsigned long currentMillis; // store the current value from millis() unsigned long nextExecuteMillis; // for comparison with currentMillis int samplingInterval = 19; // how often to run the main loop (in ms) -Servo servos[2]; // the servo library can control servos on pins 9 and 10 only +Servo servos[MAX_SERVOS]; /*============================================================================== * FUNCTIONS @@ -99,6 +97,9 @@ void setPinModeCallback(byte pin, int mode) { } if(pin > 1) { // ignore RxTx (pins 0 and 1) + if (isServoSupportedPin(pin) && mode != SERVO) + if (servos[pin - FIRST_SERVO_PIN].attached()) + servos[pin - FIRST_SERVO_PIN].detach(); if(pin > 13) reportAnalogCallback(pin - 14, mode == ANALOG ? 1 : 0); // turn on/off reporting switch(mode) { @@ -117,10 +118,13 @@ void setPinModeCallback(byte pin, int mode) { portStatus[port] = portStatus[port] | (1 << (pin - offset)); break; case SERVO: - if((pin == 9 || pin == 10)) + // TODO: Support Arduino Mega + if (isServoSupportedPin(pin)) { pinStatus[pin] = mode; - else - Firmata.sendString("Servo only on pins 9 and 10"); + if (!servos[pin - FIRST_SERVO_PIN].attached()) + servos[pin - FIRST_SERVO_PIN].attach(pin); + } else + Firmata.sendString("Servo only on pins from 2 to 13"); break; case I2C: pinStatus[pin] = mode; @@ -137,8 +141,8 @@ void analogWriteCallback(byte pin, int value) { switch(pinStatus[pin]) { case SERVO: - if(pin == 9) servos[0].write(value); - if(pin == 10) servos[1].write(value); + if (isServoSupportedPin(pin)) + servos[pin - FIRST_SERVO_PIN].write(value); break; case PWM: analogWrite(pin, value); @@ -179,7 +183,6 @@ void reportAnalogCallback(byte pin, int value) } else { // everything but 0 enables reporting of that pin analogInputsToReport = analogInputsToReport | (1 << pin); - setPinModeCallback(pin, ANALOG); } // TODO: save status to EEPROM here, if changed } @@ -201,12 +204,17 @@ void sysexCallback(byte command, byte argc, byte *argv) case SERVO_CONFIG: if(argc > 4) { // these vars are here for clarity, they'll optimized away by the compiler - byte pin = argv[0] - 9; // servos are pins 9 and 10, so offset for array + byte pin = argv[0]; int minPulse = argv[1] + (argv[2] << 7); int maxPulse = argv[3] + (argv[4] << 7); - servos[pin].attach(argv[0], minPulse, maxPulse); - // TODO does the Servo have to be detach()ed before reconfiguring? - setPinModeCallback(pin, SERVO); + + if (isServoSupportedPin(pin)) { + // servos are pins from 2 to 13, so offset for array + if (servos[pin - FIRST_SERVO_PIN].attached()) + servos[pin - FIRST_SERVO_PIN].detach(); + servos[pin - FIRST_SERVO_PIN].attach(pin, minPulse, maxPulse); + setPinModeCallback(pin, SERVO); + } } break; case SAMPLING_INTERVAL: @@ -218,6 +226,10 @@ void sysexCallback(byte command, byte argc, byte *argv) } } +boolean isServoSupportedPin(byte pin) +{ + return ((FIRST_SERVO_PIN <= pin) && (pin <= (FIRST_SERVO_PIN + MAX_SERVOS))); +} /*============================================================================== * SETUP() @@ -239,7 +251,7 @@ void setup() portStatus[1] = B11000000; // ignore 14/15 pins portStatus[2] = B00000000; - for(i=0; i