From 515cdbab0e82c83077255d63afe4d42c27279d10 Mon Sep 17 00:00:00 2001 From: Vincent M Date: Wed, 2 Mar 2016 13:07:51 +0100 Subject: [PATCH] Modification du code servostrap --- .../ServoStrapPortManipulationLD293D_1_0.ino | 201 +++++++++++++++++ ..._Port_manipulation_LD293D-1.1AVEC_ENA.ino} | 0 ...ap_Port_manipulation_LD293D-1.1SS_ENA.ino} | 0 ...p_Port_manipulation_LD293D-1.1SS_ENA2.ino} | 12 +- ...Strap_Port_manipulation_LD293D_1_0_pde.ino | 204 ++++++++++++++++++ .../digitalWriteFast.h | 165 ++++++++++++++ 6 files changed, 577 insertions(+), 5 deletions(-) create mode 100644 Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_0/ServoStrapPortManipulationLD293D_1_0.ino rename Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/{ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.h => ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino} (100%) rename Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/{ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.h => ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.ino} (100%) rename Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/{ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.h => ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino} (93%) create mode 100644 Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino create mode 100644 Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h diff --git a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_0/ServoStrapPortManipulationLD293D_1_0.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_0/ServoStrapPortManipulationLD293D_1_0.ino new file mode 100644 index 0000000..9d5e360 --- /dev/null +++ b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_0/ServoStrapPortManipulationLD293D_1_0.ino @@ -0,0 +1,201 @@ +#include +//this is to use DWF library, it will increase the speed of digitalRead/Write command +//used in the interrupt function doEncoderMotor0, but may be used everywhere. +/* +https://github.com/danithebest91/ServoStrap + i have made this code for the LMD18245 motor controller, + i have merged the pid code of Josh Kopel + whith the code of makerbot servo-controller board, + you can use this code on the some board changing some values. + Daniele Poddighe + external ardware require a quadrature encoder, timing slit strip and a dc motor, + all you can find inside an old printer, i have took it from canon and hp printers(psc1510) + + for motor controll you can choose different type of H-bridge, i have used LMD18245, + you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing + the schematic and PCB layout on eagle. + + + read a rotary encoder with interrupts + Encoder hooked up with common to GROUND, + encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below) + it doesn't matter which encoder pin you use for A or B + + is possible to change PID costants by sending on serial interfaces the values separated by ',' in this order: KP,KD,KI + example: 5.2,3.1,0 so we have KP=5.2 KD=3.1 KI=0 is only for testing purposes, but i will leave this function with eeprom storage + + This code use Port manipulation : + Port registers allow for lower-level and faster manipulation of the i/o pins of the microcontroller on an Arduino board. The chips used on the Arduino board + (the ATmega8 and ATmega168) have three ports: + B (digital pin 8 to 13) + C (analog input pins) + D (digital pins 0 to 7) + https://www.arduino.cc/en/Reference/PortManipulation + + https://www.arduino.cc/en/Hacking/PinMapping + https://www.arduino.cc/en/Hacking/PinMapping168 +*/ + +#define encoder0PinA 2 // PD2 (INT0) You can't modify port mapping here because this code use Port manipulation (I2) +#define encoder0PinB 8 // PB0; (I8) + +#define MotorIN1 5 //(I5) IN1 +#define MotorIN2 6 //(I6) IN2 + +//from ramps 1.4 stepper driver +#define STEP_PIN 3 //PD3 (INT1) (I3) +#define DIR_PIN 7 //PC0; (A0) +//#define ENABLE_PIN 13 //PB5; for now is USELESS (I13) + +//to use current motor as speed control, the LMD18245 has 4 bit cuttent output +//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any) +//#define M1 10 // PB1; PB2; PB3; PB4 +//#define M2 11 +//#define M3 12 + + +volatile long encoder0Pos = 0; + +long target = 0; +long target1 = 0; +int amp=212; +//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) +//PID controller constants +float KP = 6.0 ; //position multiplier (gain) 2.25 +float KI = 0.1; // Intergral multiplier (gain) .25 +float KD = 1.3; // derivative multiplier (gain) 1.0 + +int lastError = 0; +int sumError = 0; + +//Integral term min/max (random value and not yet tested/verified) +int iMax = 100; +int iMin = 0; + +long previousTarget = 0; +long previousMillis = 0; // will store last time LED was updated +long interval = 5; // interval at which to blink (milliseconds) + +//for motor control ramps 1.4 +bool newStep = false; +bool oldStep = false; +bool dir = false; + +void setup() { + pinModeFast(2, INPUT); + pinModeFast(encoder0PinA, INPUT); + pinModeFast(encoder0PinB, INPUT); + + pinModeFast(MotorIN1, OUTPUT); + pinModeFast(MotorIN2, OUTPUT); + //pinModeFast(DirectionPin, OUTPUT); + //pinMode(SpeedPin, OUTPUT); + + //ramps 1.4 motor control + pinModeFast(STEP_PIN, INPUT); + pinModeFast(DIR_PIN, INPUT); + //pinModeFast(M0,OUTPUT); + //pinModeFast(M1,OUTPUT); + //pinModeFast(M2,OUTPUT); + //pinModeFast(M3,OUTPUT); + + attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2 + attachInterrupt(1, countStep, RISING); //on pin 3 + + Serial.begin (115200); + Serial.println("start"); // a personal quirk + +} + +void loop(){ + + while (Serial.available() > 0) { + KP = Serial.parseFloat(); + KD = Serial.parseFloat(); + KI = Serial.parseFloat(); + + + Serial.println(KP); + Serial.println(KD); + Serial.println(KI); +} + + if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time + Serial.print(encoder0Pos); + Serial.print(','); + Serial.println(target1); + previousTarget=millis(); + } + + target = target1; + docalc(); +} + +void docalc() { + + if (millis() - previousMillis > interval) + { + previousMillis = millis(); // remember the last time we blinked the LED + + long error = encoder0Pos - target ; // find the error term of current position - target + + //generalized PID formula + //correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) + long ms = KP * error + KD * (error - lastError) +KI * (sumError); + + lastError = error; + sumError += error; + + //scale the sum for the integral term + if(sumError > iMax) { + sumError = iMax; + } else if(sumError < iMin){ + sumError = iMin; + } + int motorspeed = map(ms,0,amp,0,255); + if( motorspeed >= 255) motorspeed=255; + + if(ms > 0){ + digitalWrite ( MotorIN1 , LOW ); + analogWrite ( MotorIN2, motorspeed ); + } + if(ms < 0){ + digitalWrite ( MotorIN1 , HIGH ); + analogWrite ( MotorIN2, 255 - motorspeed ); + ms = -1 * ms; + } + } +} + +void doEncoderMotor0(){ + if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2) + + if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 (I8) + // encoder is turning + encoder0Pos-- ; // CCW + } + else { + encoder0Pos++ ; // CW + } + } + else // found a high-to-low on channel A + { + if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 (I8) + // encoder is turning + encoder0Pos++ ; // CW + } + else { + encoder0Pos-- ; // CCW + } + + } + +} + +void countStep(){ + dir=digitalRead(DIR_PIN)==LOW; + //dir = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital; + //here will be (PINB&B0000001) to not use shift in the stable version + if (dir) target1++; + else target1--; +} diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.h b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.h rename to Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.h b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.ino similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.h rename to Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.ino diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.h b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino similarity index 93% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.h rename to Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino index fba4f22..9666c7e 100644 --- a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.h +++ b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino @@ -86,7 +86,9 @@ void setup() { pinModeFast(encoder0PinA, INPUT); pinModeFast(encoder0PinB, INPUT); - pinModeFast(DirectionPin, OUTPUT); + pinModeFast(MotorIN1, OUTPUT); + pinModeFast(MotorIN2, OUTPUT); + //pinModeFast(DirectionPin, OUTPUT); //pinMode(SpeedPin, OUTPUT); //ramps 1.4 motor control @@ -154,12 +156,12 @@ void docalc() { if( motorspeed >= 255) motorspeed=255; if(ms > 0){ - digitalWriteFast ( MotorIN1 ,LOW ); + digitalWrite ( MotorIN1 , LOW ); analogWrite ( MotorIN2, motorspeed ); } if(ms < 0){ - digitalWriteFast ( MotorIN1 ,HIGH ); - analogWrite ( MotorIN2, 255-motorspeed ); + digitalWrite ( MotorIN1 , HIGH ); + analogWrite ( MotorIN2, 255 - motorspeed ); ms = -1 * ms; } } @@ -195,4 +197,4 @@ void countStep(){ //here will be (PINB&B0000001) to not use shift in the stable version if (dir) target1++; else target1--; -} \ No newline at end of file +} diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino new file mode 100644 index 0000000..11215ed --- /dev/null +++ b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino @@ -0,0 +1,204 @@ +#include +//this is to use DWF library, it will increase the speed of digitalRead/Write command +//used in the interrupt function doEncoderMotor0, but may be used everywhere. +/* +https://github.com/danithebest91/ServoStrap + i have made this code for the LMD18245 motor controller, + i have merged the pid code of Josh Kopel + whith the code of makerbot servo-controller board, + you can use this code on the some board changing some values. + Daniele Poddighe + external ardware require a quadrature encoder, timing slit strip and a dc motor, + all you can find inside an old printer, i have took it from canon and hp printers(psc1510) + + for motor controll you can choose different type of H-bridge, i have used LMD18245, + you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing + the schematic and PCB layout on eagle. + + + read a rotary encoder with interrupts + Encoder hooked up with common to GROUND, + encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below) + it doesn't matter which encoder pin you use for A or B + + is possible to change PID costants by sending on serial interfaces the values separated by ',' in this order: KP,KD,KI + example: 5.2,3.1,0 so we have KP=5.2 KD=3.1 KI=0 is only for testing purposes, but i will leave this function with eeprom storage + + This code use Port manipulation : + Port registers allow for lower-level and faster manipulation of the i/o pins of the microcontroller on an Arduino board. The chips used on the Arduino board + (the ATmega8 and ATmega168) have three ports: + B (digital pin 8 to 13) + C (analog input pins) + D (digital pins 0 to 7) + https://www.arduino.cc/en/Reference/PortManipulation + + https://www.arduino.cc/en/Hacking/PinMapping + https://www.arduino.cc/en/Hacking/PinMapping168 +*/ + +#define encoder0PinA 2 // PD2 (INT0) You can't modify port mapping here because this code use Port manipulation (I2) +#define encoder0PinB 8 // PB0; (I8) + +#define SpeedPin 6 //(I6) +#define DirectionPin 15 //PC1; (A1) + +//from ramps 1.4 stepper driver + +#define STEP_PIN 3 //PD3 (INT1) (I3) +#define DIR_PIN 14 //PC0; (A0) + +//#define ENABLE_PIN 13 //PB5; for now is USELESS (I13) +//to use current motor as speed control, the LMD18245 has 4 bit cuttent output +//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any) +//#define M1 10 // PB1; PB2; PB3; PB4 +//#define M2 11 +//#define M3 12 + + +volatile long encoder0Pos = 0; + +long target = 0; +long target1 = 0; +int amp=212; +//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) +//PID controller constants +float KP = 6.0 ; //position multiplier (gain) 2.25 +float KI = 0.1; // Intergral multiplier (gain) .25 +float KD = 1.3; // derivative multiplier (gain) 1.0 + +int lastError = 0; +int sumError = 0; + +//Integral term min/max (random value and not yet tested/verified) +int iMax = 100; +int iMin = 0; + +long previousTarget = 0; +long previousMillis = 0; // will store last time LED was updated +long interval = 5; // interval at which to blink (milliseconds) + +//for motor control ramps 1.4 +bool newStep = false; +bool oldStep = false; +bool dir = false; + +void setup() { + pinModeFast(2, INPUT); + pinModeFast(encoder0PinA, INPUT); + pinModeFast(encoder0PinB, INPUT); + + pinModeFast(DirectionPin, OUTPUT); + //pinMode(SpeedPin, OUTPUT); + + //ramps 1.4 motor control + pinModeFast(STEP_PIN, INPUT); + pinModeFast(DIR_PIN, INPUT); + //pinModeFast(M0,OUTPUT); + //pinModeFast(M1,OUTPUT); + //pinModeFast(M2,OUTPUT); + //pinModeFast(M3,OUTPUT); + + attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2 + attachInterrupt(1, countStep, RISING); //on pin 3 + + Serial.begin (115200); + Serial.println("start"); // a personal quirk + +} + +void loop(){ + + while (Serial.available() > 0) { + KP = Serial.parseFloat(); + KD = Serial.parseFloat(); + KI = Serial.parseFloat(); + + + Serial.println(KP); + Serial.println(KD); + Serial.println(KI); +} + + if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time + Serial.print(encoder0Pos); + Serial.print(','); + Serial.println(target1); + previousTarget=millis(); + } + + target = target1; + docalc(); +} + +void docalc() { + + if (millis() - previousMillis > interval) + { + previousMillis = millis(); // remember the last time we blinked the LED + + long error = encoder0Pos - target ; // find the error term of current position - target + + //generalized PID formula + //correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) + long ms = KP * error + KD * (error - lastError) +KI * (sumError); + + lastError = error; + sumError += error; + + //scale the sum for the integral term + if(sumError > iMax) { + sumError = iMax; + } else if(sumError < iMin){ + sumError = iMin; + } + + if(ms > 0){ + PORTC |=B00000010; //digitalWriteFast2 ( DirectionPin ,HIGH ); write PC1 HIGH (A1) + } + if(ms < 0){ + PORTC &=(B11111101); //digitalWriteFast2 ( DirectionPin , LOW ); write PC1 LOW (A1) + ms = -1 * ms; + } + + int motorspeed = map(ms,0,amp,0,255); + if( motorspeed >= 255) motorspeed=255; + //PORTB |=(motorspeed<<1); // is a sort of: digitalwrite(M0 M1 M2 M3, 0 0 0 0 to 1 1 1 1); it set directly PORTB to B**M3M2M1M0*; + //analogWrite ( SpeedPin, (255 - motorSpeed) ); + analogWrite ( SpeedPin, motorspeed ); + //Serial.print ( ms ); + //Serial.print ( ',' ); + //Serial.println ( motorspeed ); + } +} + +void doEncoderMotor0(){ + if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2) + + if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 (I8) + // encoder is turning + encoder0Pos-- ; // CCW + } + else { + encoder0Pos++ ; // CW + } + } + else // found a high-to-low on channel A + { + if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 (I8) + // encoder is turning + encoder0Pos++ ; // CW + } + else { + encoder0Pos-- ; // CCW + } + + } + +} + +void countStep(){ + dir = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital; + //here will be (PINB&B0000001) to not use shift in the stable version + if (dir) target1++; + else target1--; +} diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h new file mode 100644 index 0000000..b8c2618 --- /dev/null +++ b/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h @@ -0,0 +1,165 @@ +#if !defined(digitalPinToPortReg) +#if !defined(__AVR_ATmega1280__) + +// Standard Arduino Pins +#define digitalPinToPortReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PORTD : (((P) >= 8 && (P) <= 13) ? &PORTB : &PORTC)) +#define digitalPinToDDRReg(P) \ +(((P) >= 0 && (P) <= 7) ? &DDRD : (((P) >= 8 && (P) <= 13) ? &DDRB : &DDRC)) +#define digitalPinToPINReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PIND : (((P) >= 8 && (P) <= 13) ? &PINB : &PINC)) +#define digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) + +#if defined(__AVR_ATmega8__) + +// 3 PWM +#define digitalPinToTimer(P) \ +(((P) == 9 || (P) == 10) ? &TCCR1A : (((P) == 11) ? &TCCR2 : 0)) +#define digitalPinToTimerBit(P) \ +(((P) == 9) ? COM1A1 : (((P) == 10) ? COM1B1 : COM21)) +#else + +// 6 PWM +#define digitalPinToTimer(P) \ +(((P) == 6 || (P) == 5) ? &TCCR0A : \ + (((P) == 9 || (P) == 10) ? &TCCR1A : \ + (((P) == 11 || (P) == 3) ? &TCCR2A : 0))) +#define digitalPinToTimerBit(P) \ +(((P) == 6) ? COM0A1 : (((P) == 5) ? COM0B1 : \ + (((P) == 9) ? COM1A1 : (((P) == 10) ? COM1B1 : \ + (((P) == 11) ? COM2A1 : COM2B1))))) +#endif + +#else +// Arduino Mega Pins +#define digitalPinToPortReg(P) \ +(((P) >= 22 && (P) <= 29) ? &PORTA : \ + ((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PORTB : \ + (((P) >= 30 && (P) <= 37) ? &PORTC : \ + ((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PORTD : \ + ((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PORTE : \ + (((P) >= 54 && (P) <= 61) ? &PORTF : \ + ((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PORTG : \ + ((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PORTH : \ + (((P) == 14 || (P) == 15) ? &PORTJ : \ + (((P) >= 62 && (P) <= 69) ? &PORTK : &PORTL)))))))))) + +#define digitalPinToDDRReg(P) \ +(((P) >= 22 && (P) <= 29) ? &DDRA : \ + ((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &DDRB : \ + (((P) >= 30 && (P) <= 37) ? &DDRC : \ + ((((P) >= 18 && (P) <= 21) || (P) == 38) ? &DDRD : \ + ((((P) >= 0 && (P) <= 3) || (P) == 5) ? &DDRE : \ + (((P) >= 54 && (P) <= 61) ? &DDRF : \ + ((((P) >= 39 && (P) <= 41) || (P) == 4) ? &DDRG : \ + ((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &DDRH : \ + (((P) == 14 || (P) == 15) ? &DDRJ : \ + (((P) >= 62 && (P) <= 69) ? &DDRK : &DDRL)))))))))) + +#define digitalPinToPINReg(P) \ +(((P) >= 22 && (P) <= 29) ? &PINA : \ + ((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PINB : \ + (((P) >= 30 && (P) <= 37) ? &PINC : \ + ((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PIND : \ + ((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PINE : \ + (((P) >= 54 && (P) <= 61) ? &PINF : \ + ((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PING : \ + ((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PINH : \ + (((P) == 14 || (P) == 15) ? &PINJ : \ + (((P) >= 62 && (P) <= 69) ? &PINK : &PINL)))))))))) + +#define digitalPinToBit(P) \ +(((P) >= 7 && (P) <= 9) ? (P) - 3 : \ + (((P) >= 10 && (P) <= 13) ? (P) - 6 : \ + (((P) >= 22 && (P) <= 29) ? (P) - 22 : \ + (((P) >= 30 && (P) <= 37) ? 37 - (P) : \ + (((P) >= 39 && (P) <= 41) ? 41 - (P) : \ + (((P) >= 42 && (P) <= 49) ? 49 - (P) : \ + (((P) >= 50 && (P) <= 53) ? 53 - (P) : \ + (((P) >= 54 && (P) <= 61) ? (P) - 54 : \ + (((P) >= 62 && (P) <= 69) ? (P) - 62 : \ + (((P) == 0 || (P) == 15 || (P) == 17 || (P) == 21) ? 0 : \ + (((P) == 1 || (P) == 14 || (P) == 16 || (P) == 20) ? 1 : \ + (((P) == 19) ? 2 : \ + (((P) == 5 || (P) == 6 || (P) == 18) ? 3 : \ + (((P) == 2) ? 4 : \ + (((P) == 3 || (P) == 4) ? 5 : 7))))))))))))))) + +// 15 PWM +#define digitalPinToTimer(P) \ +(((P) == 13 || (P) == 4) ? &TCCR0A : \ + (((P) == 11 || (P) == 12) ? &TCCR1A : \ + (((P) == 10 || (P) == 9) ? &TCCR2A : \ + (((P) == 5 || (P) == 2 || (P) == 3) ? &TCCR3A : \ + (((P) == 6 || (P) == 7 || (P) == 8) ? &TCCR4A : \ + (((P) == 46 || (P) == 45 || (P) == 44) ? &TCCR5A : 0)))))) +#define digitalPinToTimerBit(P) \ +(((P) == 13) ? COM0A1 : (((P) == 4) ? COM0B1 : \ + (((P) == 11) ? COM1A1 : (((P) == 12) ? COM1B1 : \ + (((P) == 10) ? COM2A1 : (((P) == 9) ? COM2B1 : \ + (((P) == 5) ? COM3A1 : (((P) == 2) ? COM3B1 : (((P) == 3) ? COM3C1 : \ + (((P) == 6) ? COM4A1 : (((P) == 7) ? COM4B1 : (((P) == 8) ? COM4C1 : \ + (((P) == 46) ? COM5A1 : (((P) == 45) ? COM5B1 : COM5C1)))))))))))))) + +#endif +#endif + +#if !defined(digitalWriteFast) +#define digitalWriteFast(P, V) \ +if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ + if (digitalPinToTimer(P)) \ + bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)); \ + bitWrite(*digitalPinToPortReg(P), digitalPinToBit(P), (V)); \ + } else { \ + digitalWrite((P), (V)); \ + } +#endif + +#if !defined(pinModeFast) +#define pinModeFast(P, V) \ +if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ + bitWrite(*digitalPinToDDRReg(P), digitalPinToBit(P), (V)); \ + } else { \ + pinMode((P), (V)); \ + } +#endif + +#if !defined(digitalReadFast) +#define digitalReadFast(P) ( (int) __digitalReadFast__((P)) ) +#define __digitalReadFast__(P ) \ +(__builtin_constant_p(P) ) ? ( \ + digitalPinToTimer(P) ? ( \ + bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)) , \ + bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) : \ + bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) : \ + digitalRead((P)) +#endif + +#if !defined(digitalWriteFast2) +#define digitalWriteFast2(P, V) \ +if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ + bitWrite(*digitalPinToPortReg(P), digitalPinToBit(P), (V)); \ + } else { \ + digitalWrite((P), (V)); \ + } +#endif + +#if !defined(pinModeFast2) +#define pinModeFast2(P, V) \ +if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ + if (digitalPinToTimer(P)) \ + bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)); \ + bitWrite(*digitalPinToDDRReg(P), digitalPinToBit(P), (V)); \ + } else { \ + pinMode((P), (V)); \ + } +#endif + +#if !defined(digitalReadFast2) +#define digitalReadFast2(P) ( (int) __digitalReadFast2__((P)) ) +#define __digitalReadFast2__(P ) \ +(__builtin_constant_p(P) ) ? ( \ + ( bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) ) : \ + digitalRead((P)) +#endif