From 515cdbab0e82c83077255d63afe4d42c27279d10 Mon Sep 17 00:00:00 2001 From: Vincent M Date: Wed, 2 Mar 2016 13:07:51 +0100 Subject: [PATCH 01/14] 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 From 7119af219fe6ffae2c7468bcb889c31a07c55990 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Sabl=C3=A9?= Date: Wed, 2 Mar 2016 19:27:15 +0100 Subject: [PATCH 02/14] mon message de commit qui roxxx MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ça roxx toujours --- Coucou.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Coucou.txt b/Coucou.txt index a802506..83778cb 100644 --- a/Coucou.txt +++ b/Coucou.txt @@ -1 +1,2 @@ -Bonjour je suis cool ! \ No newline at end of file +Bonjour je suis cool ! +Non ! \ No newline at end of file From 261a35470cf5adef0b89362a2f5efc4481661d8c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Sabl=C3=A9?= Date: Wed, 2 Mar 2016 19:28:32 +0100 Subject: [PATCH 03/14] modif test test --- Coucou.txt | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 Coucou.txt diff --git a/Coucou.txt b/Coucou.txt deleted file mode 100644 index 83778cb..0000000 --- a/Coucou.txt +++ /dev/null @@ -1,2 +0,0 @@ -Bonjour je suis cool ! -Non ! \ No newline at end of file From dd6382a758e1fed674caaf39243681f8c022ed63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Sabl=C3=A9?= Date: Wed, 2 Mar 2016 19:35:40 +0100 Subject: [PATCH 04/14] =?UTF-8?q?Cr=C3=A9ation=20de=20la=20branche=20Modif?= =?UTF-8?q?ication=20du=20corps?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Modification_du_corps/README | 1 + 1 file changed, 1 insertion(+) create mode 100644 Modification_du_corps/README diff --git a/Modification_du_corps/README b/Modification_du_corps/README new file mode 100644 index 0000000..2da751e --- /dev/null +++ b/Modification_du_corps/README @@ -0,0 +1 @@ +Comming soon ... \ No newline at end of file From 8b60845b49d30fa9f909d1ebaeed0bdade902ac1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Sabl=C3=A9?= Date: Wed, 2 Mar 2016 19:37:43 +0100 Subject: [PATCH 05/14] =?UTF-8?q?tentative=20de=20cr=C3=A9ation=20d'une=20?= =?UTF-8?q?branche=20...=20--'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit désésspoir --- Modification_du_corps/README | 1 - 1 file changed, 1 deletion(-) delete mode 100644 Modification_du_corps/README diff --git a/Modification_du_corps/README b/Modification_du_corps/README deleted file mode 100644 index 2da751e..0000000 --- a/Modification_du_corps/README +++ /dev/null @@ -1 +0,0 @@ -Comming soon ... \ No newline at end of file From 63a6cd03b53491c0643026b8cfecfda3724343ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Sabl=C3=A9?= Date: Wed, 2 Mar 2016 19:38:33 +0100 Subject: [PATCH 06/14] =?UTF-8?q?deuxi=C3=A8me=20tentative...?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Modification_du_corps/README | 1 + 1 file changed, 1 insertion(+) create mode 100644 Modification_du_corps/README diff --git a/Modification_du_corps/README b/Modification_du_corps/README new file mode 100644 index 0000000..2da751e --- /dev/null +++ b/Modification_du_corps/README @@ -0,0 +1 @@ +Comming soon ... \ No newline at end of file From 8dcd4f08311012d5af73260a56bfb658ad2812e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Sabl=C3=A9?= Date: Wed, 2 Mar 2016 19:39:26 +0100 Subject: [PATCH 07/14] --- --- Modification_du_corps/README | 1 - 1 file changed, 1 deletion(-) delete mode 100644 Modification_du_corps/README diff --git a/Modification_du_corps/README b/Modification_du_corps/README deleted file mode 100644 index 2da751e..0000000 --- a/Modification_du_corps/README +++ /dev/null @@ -1 +0,0 @@ -Comming soon ... \ No newline at end of file From c9ba9f7ffb8e4c3233283b6998029364ad74457a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Sabl=C3=A9?= Date: Wed, 2 Mar 2016 19:41:28 +0100 Subject: [PATCH 08/14] =?UTF-8?q?Cr=C3=A9ation=20de=20branche=20"Modificat?= =?UTF-8?q?ion=20du=20corps"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Modification_du_corps/README | 1 + 1 file changed, 1 insertion(+) create mode 100644 Modification_du_corps/README diff --git a/Modification_du_corps/README b/Modification_du_corps/README new file mode 100644 index 0000000..2da751e --- /dev/null +++ b/Modification_du_corps/README @@ -0,0 +1 @@ +Comming soon ... \ No newline at end of file From 66ae35ea13ce42441f70a5107bb449268afaf3d3 Mon Sep 17 00:00:00 2001 From: Vincent M Date: Thu, 3 Mar 2016 02:06:03 +0100 Subject: [PATCH 09/14] =?UTF-8?q?Driver=5FMoteur=5FDC=20premi=C3=A8re=20im?= =?UTF-8?q?pression=20viable?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit YATA :) --- Hello | 0 Modification_du_corps/README | 0 .../PID_AutoTune_v0/AutoTune_Example.pde | 0 .../AutoTune_Example/AutoTune_Example.pde | 0 .../AutoTune_Example/PID_AutoTune_v0.cpp | 0 .../AutoTune_Example/PID_AutoTune_v0.h | 0 .../AutoTune_Example/PID_v1.cpp | 0 .../PID_AutoTune_v0/AutoTune_Example/PID_v1.h | 0 .../AutoTune_Example/AutoTune_Example.pde | 0 .../PID_AutoTune_v0/PID_AutoTune_v0.cpp | 0 .../PID_AutoTune_v0/PID_AutoTune_v0.h | 0 .../Arduino-PID-AutoTune-Library/README.txt | 0 Moteur DC/DC_servo/DC_1servo.7z | Bin Moteur DC/DC_servo/DC_1servo/DC_1servo.ino | 0 Moteur DC/DC_servo/DC_1servo/PID_v1.cpp | 0 Moteur DC/DC_servo/DC_1servo/PID_v1.h | 0 .../Arduino_stepper_motor_emulator_v1.0.0.pde | 0 Moteur DC/ServoStrap/LICENSE | 0 ...ulator_v1.0.1_direct_port_manipulation.pde | 0 .../STM32_olimexino_stepper_motor_emulator | 0 .../keywords.txt | 0 .../Driver_printer/Driver_printer.ino | 0 .../libraries/digitalWriteFast.h | 0 ...ervoStrap_Port_manipulation_LD293D-1.0.pde | 0 ...p_Port_manipulation_LD293D-1.1AVEC_ENA.ino | 0 ...rap_Port_manipulation_LD293D-1.1SS_ENA.ino | 0 ...ap_Port_manipulation_LD293D-1.1SS_ENA2.ino | 0 ...Strap_Port_manipulation_LD293D_1_0_pde.ino | 0 .../digitalWriteFast.h | 0 Moteur DC/ServoStrap/README.md | 0 .../STM32_olimexino_stepper_motor_emulator | 0 .../ServoStrapPortManipulationLD293D_1_0.ino | 0 .../ServoStrapPortManipulationLD293D_1_1.ino | 229 ++++++++++++++++++ .../ServoStrapPortManipulationLD293D_1_2.ino | 229 ++++++++++++++++++ Moteur DC/ServoStrap/digitalWriteFast.h | 0 Moteur DC/ServoStrap/keywords.txt | 0 .../Test/Test_Moteur_DC/Test_Moteur_DC.ino | 0 .../Test_Roue_Codeuse/Test_Roue_Codeuse.ino | 0 README | 0 .../Imprimante_3d_fablab.SLDASM | Bin .../pièces/Boulon M6 hexa 30mm.SLDPRT | Bin .../pièces/HeatingBed_214x214mm.SLDPRT | Bin .../pièces/NORDEX-ABX-C3-8.SLDPRT | Bin .../pièces/Rexroth-Default.SLDPRT | Bin .../pièces/Tasseau M6.SLDPRT | Bin .../User Library-fan - pabst 512f.SLDPRT | Bin ...30R-SC030R - 5 mm x 8 mm_SC030R_A-1.sldprt | Bin .../pièces/axe charnière.SLDPRT | Bin .../pièces/batant charnière.SLDPRT | Bin .../eclisse_40x40x222_bisote_x1.SLDPRT | Bin .../eclisse_40x40x404_bisote_x2.SLDPRT | Bin .../eclisse_40x40x670_bisote_x1.SLDPRT | Bin .../eclisse_40x40x704_bisote_x2.SLDPRT | Bin .../pièces/plaque_acrylique_380x210.SLDPRT | Bin .../pièces/plaque_acrylique_380x670.SLDPRT | Bin .../pièces/plaque_acrylique_380x680.SLDPRT | Bin .../pièces/plaque_acrylique_680x210.SLDPRT | Bin .../pièces/plaque_acrylique_680x670.SLDPRT | Bin .../plaque_acrylique_680x670_porte.SLDPRT | Bin ...ue_repartition_aire_chaud_300x544mm.SLDPRT | Bin .../pièces/plaque_verre_428x210mm.SLDPRT | Bin imprimante_3d_fablab/pièces/porte.SLDPRT | Bin .../pièces/prof_40x40x300mm.SLDPRT | Bin .../pièces/prof_40x40x600mm.SLDPRT | Bin .../pièces/support_moteur_chassis.SLDPRT | Bin .../pièces/support_plateau.SLDPRT | Bin .../pièces/volume_d_impression.SLDPRT | Bin .../sous_assemblages/NEMA 17/Base.SLDPRT | Bin .../sous_assemblages/NEMA 17/Cuerpo.SLDPRT | Bin .../sous_assemblages/NEMA 17/Eje.SLDPRT | Bin .../NEMA 17/NEMA 17 with T5 Pulley.SLDASM | Bin .../NEMA 17/NEMA 17 with T5 Pulley.STEP | 0 .../NEMA 17/NEMA 17 with T5 Pulley.STP | 0 .../NEMA 17/NEMA 17+support.SLDASM | Bin .../sous_assemblages/NEMA 17/NEMA 17.SLDASM | Bin .../sous_assemblages/NEMA 17/NEMA 17.STL | Bin .../NEMA 17 with T5 Pulley Render.avi | Bin .../NEMA 17 with T5 Pulley Render.gif | Bin .../Renderings/NEMA 17 with T5 Pulley.avi | Bin .../NEMA 17/Renderings/NEMA17 2.JPG | Bin .../NEMA 17/Renderings/NEMA17 3.JPG | Bin .../Renderings/NEMA17 with T5 Pulley.JPG | Bin .../sous_assemblages/NEMA 17/T5 Pulley.SLDPRT | Bin .../sous_assemblages/NEMA 17/Tapa.SLDPRT | Bin .../NEMA 17/radial ball bearing_68_am.sldprt | Bin .../sous_assemblages/cadre_600x300mm.SLDASM | Bin .../sous_assemblages/carcasse.SLDASM | Bin .../sous_assemblages/charniere.SLDASM | Bin .../sous_assemblages/couvercle.SLDASM | Bin .../sous_assemblages/plateau_complet.SLDASM | Bin .../vue_carcasse+plateau_16.11.2015.png | Bin ..._carcasse+volume_impression_13.11.2015.png | Bin .../vue_carcasse_13.11.2015.png | Bin .../vue_carcasse_prof_only_16.11.2015.png | Bin ...ue_dessu_cotes_pour_core_XY_16.11.2015.png | Bin .../vue_plateau_15.11.2015.png | Bin 96 files changed, 458 insertions(+) mode change 100644 => 100755 Hello mode change 100644 => 100755 Modification_du_corps/README mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example.pde mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/AutoTune_Example.pde mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.cpp mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.h mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.cpp mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.h mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/Examples/AutoTune_Example/AutoTune_Example.pde mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.cpp mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.h mode change 100644 => 100755 Moteur DC/Arduino-PID-AutoTune-Library/README.txt mode change 100644 => 100755 Moteur DC/DC_servo/DC_1servo.7z mode change 100644 => 100755 Moteur DC/DC_servo/DC_1servo/DC_1servo.ino mode change 100644 => 100755 Moteur DC/DC_servo/DC_1servo/PID_v1.cpp mode change 100644 => 100755 Moteur DC/DC_servo/DC_1servo/PID_v1.h mode change 100644 => 100755 Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.0.pde mode change 100644 => 100755 Moteur DC/ServoStrap/LICENSE rename Moteur DC/ServoStrap/{ => OLD}/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/Driver_printer/Driver_printer.ino (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/Driver_printer/libraries/digitalWriteFast.h (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.0.pde (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.ino (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino (100%) mode change 100644 => 100755 rename Moteur DC/ServoStrap/{ => OLD}/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h (100%) mode change 100644 => 100755 mode change 100644 => 100755 Moteur DC/ServoStrap/README.md mode change 100644 => 100755 Moteur DC/ServoStrap/STM32_olimexino_stepper_motor_emulator mode change 100644 => 100755 Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_0/ServoStrapPortManipulationLD293D_1_0.ino create mode 100755 Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_1/ServoStrapPortManipulationLD293D_1_1.ino create mode 100755 Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino mode change 100644 => 100755 Moteur DC/ServoStrap/digitalWriteFast.h mode change 100644 => 100755 Moteur DC/ServoStrap/keywords.txt mode change 100644 => 100755 Moteur DC/Test/Test_Moteur_DC/Test_Moteur_DC.ino mode change 100644 => 100755 Moteur DC/Test/Test_Roue_Codeuse/Test_Roue_Codeuse.ino mode change 100644 => 100755 README mode change 100644 => 100755 imprimante_3d_fablab/Imprimante_3d_fablab.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/pièces/Boulon M6 hexa 30mm.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/HeatingBed_214x214mm.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/NORDEX-ABX-C3-8.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/Rexroth-Default.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/Tasseau M6.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/User Library-fan - pabst 512f.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/ZERO MAX-SC030R-SC030R - 5 mm x 8 mm_SC030R_A-1.sldprt mode change 100644 => 100755 imprimante_3d_fablab/pièces/axe charnière.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/batant charnière.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/eclisse_40x40x222_bisote_x1.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/eclisse_40x40x404_bisote_x2.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/eclisse_40x40x670_bisote_x1.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/eclisse_40x40x704_bisote_x2.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_acrylique_380x210.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_acrylique_380x670.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_acrylique_380x680.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_acrylique_680x210.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_acrylique_680x670.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_acrylique_680x670_porte.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_repartition_aire_chaud_300x544mm.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/plaque_verre_428x210mm.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/porte.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/prof_40x40x300mm.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/prof_40x40x600mm.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/support_moteur_chassis.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/support_plateau.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/pièces/volume_d_impression.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Base.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Cuerpo.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Eje.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.STEP mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.STP mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17+support.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17.STL mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley Render.avi mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley Render.gif mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley.avi mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 2.JPG mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 3.JPG mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 with T5 Pulley.JPG mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/T5 Pulley.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/Tapa.SLDPRT mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/NEMA 17/radial ball bearing_68_am.sldprt mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/cadre_600x300mm.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/carcasse.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/charniere.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/couvercle.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/sous_assemblages/plateau_complet.SLDASM mode change 100644 => 100755 imprimante_3d_fablab/vue_carcasse+plateau_16.11.2015.png mode change 100644 => 100755 imprimante_3d_fablab/vue_carcasse+volume_impression_13.11.2015.png mode change 100644 => 100755 imprimante_3d_fablab/vue_carcasse_13.11.2015.png mode change 100644 => 100755 imprimante_3d_fablab/vue_carcasse_prof_only_16.11.2015.png mode change 100644 => 100755 imprimante_3d_fablab/vue_dessu_cotes_pour_core_XY_16.11.2015.png mode change 100644 => 100755 imprimante_3d_fablab/vue_plateau_15.11.2015.png diff --git a/Hello b/Hello old mode 100644 new mode 100755 diff --git a/Modification_du_corps/README b/Modification_du_corps/README old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example.pde b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example.pde old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/AutoTune_Example.pde b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/AutoTune_Example.pde old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.cpp b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.cpp old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.h b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.h old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.cpp b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.cpp old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.h b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.h old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/Examples/AutoTune_Example/AutoTune_Example.pde b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/Examples/AutoTune_Example/AutoTune_Example.pde old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.cpp b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.cpp old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.h b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.h old mode 100644 new mode 100755 diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/README.txt b/Moteur DC/Arduino-PID-AutoTune-Library/README.txt old mode 100644 new mode 100755 diff --git a/Moteur DC/DC_servo/DC_1servo.7z b/Moteur DC/DC_servo/DC_1servo.7z old mode 100644 new mode 100755 diff --git a/Moteur DC/DC_servo/DC_1servo/DC_1servo.ino b/Moteur DC/DC_servo/DC_1servo/DC_1servo.ino old mode 100644 new mode 100755 diff --git a/Moteur DC/DC_servo/DC_1servo/PID_v1.cpp b/Moteur DC/DC_servo/DC_1servo/PID_v1.cpp old mode 100644 new mode 100755 diff --git a/Moteur DC/DC_servo/DC_1servo/PID_v1.h b/Moteur DC/DC_servo/DC_1servo/PID_v1.h old mode 100644 new mode 100755 diff --git a/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.0.pde b/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.0.pde old mode 100644 new mode 100755 diff --git a/Moteur DC/ServoStrap/LICENSE b/Moteur DC/ServoStrap/LICENSE old mode 100644 new mode 100755 diff --git a/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde b/Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde rename to Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde diff --git a/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator b/Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator rename to Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator diff --git a/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt b/Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt rename to Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt diff --git a/Moteur DC/ServoStrap/Driver_printer/Driver_printer.ino b/Moteur DC/ServoStrap/OLD/Driver_printer/Driver_printer.ino old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/Driver_printer/Driver_printer.ino rename to Moteur DC/ServoStrap/OLD/Driver_printer/Driver_printer.ino diff --git a/Moteur DC/ServoStrap/Driver_printer/libraries/digitalWriteFast.h b/Moteur DC/ServoStrap/OLD/Driver_printer/libraries/digitalWriteFast.h old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/Driver_printer/libraries/digitalWriteFast.h rename to Moteur DC/ServoStrap/OLD/Driver_printer/libraries/digitalWriteFast.h diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.0.pde b/Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.0.pde old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.0.pde rename to Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.0.pde diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino b/Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino rename to Moteur DC/ServoStrap/OLD/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.ino b/Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.ino old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.ino rename to Moteur DC/ServoStrap/OLD/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/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino b/Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino rename to Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino b/Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino rename to Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino diff --git a/Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h b/Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h old mode 100644 new mode 100755 similarity index 100% rename from Moteur DC/ServoStrap/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h rename to Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h diff --git a/Moteur DC/ServoStrap/README.md b/Moteur DC/ServoStrap/README.md old mode 100644 new mode 100755 diff --git a/Moteur DC/ServoStrap/STM32_olimexino_stepper_motor_emulator b/Moteur DC/ServoStrap/STM32_olimexino_stepper_motor_emulator old mode 100644 new mode 100755 diff --git a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_0/ServoStrapPortManipulationLD293D_1_0.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_0/ServoStrapPortManipulationLD293D_1_0.ino old mode 100644 new mode 100755 diff --git a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_1/ServoStrapPortManipulationLD293D_1_1.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_1/ServoStrapPortManipulationLD293D_1_1.ino new file mode 100755 index 0000000..7fd71a5 --- /dev/null +++ b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_1/ServoStrapPortManipulationLD293D_1_1.ino @@ -0,0 +1,229 @@ +#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 14 //PC0; (A0) +#define X_MIN 4 //(4) + +volatile long encoder0Pos = 0; + +long target = 0; +long target1 = 0; +int StartRoutine=0; +//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) +//PID controller constants +float KP = 2.25 ; //position multiplier (gain) 2.25 +float KI = 0.25; // Intergral multiplier (gain) .25 +float KD = 1.0; // 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(X_MIN, INPUT); + + + pinModeFast(MotorIN1, OUTPUT); + pinModeFast(MotorIN2, OUTPUT); + + //ramps 1.4 motor control + pinModeFast(STEP_PIN, INPUT); + pinModeFast(DIR_PIN, INPUT); + + 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); +} + Fstart(); + if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time + + if (X_MIN == LOW) Serial.print("LOW"); + else Serial.print("HIGH"); + Serial.print(','); + Serial.print(encoder0Pos); + Serial.print(','); + Serial.println(target1); + previousTarget=millis(); + } + + target = target1; + docalc(); +} + +void Fstart(){ + + if (StartRoutine == 0) + { + digitalWrite ( MotorIN1 , LOW ); + analogWrite ( MotorIN2, 200 ); + delay(2000); + digitalWrite ( MotorIN2 , LOW ); + analogWrite ( MotorIN1, 255 ); + delay(200); + digitalWrite ( MotorIN1, LOW ); + analogWrite ( MotorIN2, 255 ); + delay(180); + digitalWrite ( MotorIN2, LOW ); + analogWrite ( MotorIN1, 255 ); + delay(100); + digitalWrite ( MotorIN1, LOW ); + analogWrite ( MotorIN2, 255 ); + delay(90); + digitalWrite ( MotorIN2, LOW ); + analogWrite ( MotorIN1, 255 ); + delay(100); + digitalWrite ( MotorIN1, LOW ); + analogWrite ( MotorIN2, 200 ); + delay(2000); + + + StartRoutine = 1; + encoder0Pos=0; + } + +} + +void docalc() { + + if (millis() - previousMillis > interval) + { + previousMillis = millis(); + 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 motorspeed = 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(motorspeed > 0){ + if( motorspeed >= 255) motorspeed=255; + digitalWrite ( MotorIN1 , LOW ); + analogWrite ( MotorIN2, motorspeed ); + } + if(motorspeed < 0){ + motorspeed = -1 * motorspeed; + if( motorspeed >= 255) motorspeed=255; + digitalWrite ( MotorIN2 , LOW ); + analogWrite ( MotorIN1, motorspeed ); + //digitalWrite ( MotorIN1 , HIGH ); + //analogWrite ( MotorIN2, 255 - motorspeed ); + } + } +} + +void doEncoderMotor0(){ + //if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2) + if(digitalRead(encoder0PinA)==HIGH){ + + 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)==HIGH; + 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/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino new file mode 100755 index 0000000..c281146 --- /dev/null +++ b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino @@ -0,0 +1,229 @@ +#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 14 //PC0; (A0) +#define X_MIN 4 //(4) + +volatile long encoder0Pos = 0; + +long target = 0; +long target1 = 0; +int StartRoutine=0; +//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) +//PID controller constants +float KP = 5 ; //position multiplier (gain) 2.25 +float KI = 0.1; // Intergral multiplier (gain) .25 +float KD = 2.0; // 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(X_MIN, INPUT); + + + pinModeFast(MotorIN1, OUTPUT); + pinModeFast(MotorIN2, OUTPUT); + + //ramps 1.4 motor control + pinModeFast(STEP_PIN, INPUT); + pinModeFast(DIR_PIN, INPUT); + + 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); +} + Fstart(); + if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time + +// if (X_MIN == LOW) Serial.print("LOW"); +// else Serial.print("HIGH"); +// Serial.print(','); + Serial.print(encoder0Pos); + Serial.print(','); + Serial.println(target1); + previousTarget=millis(); + } + + target = target1; + docalc(); +} + +void Fstart(){ + + if (StartRoutine == 0) + { + digitalWrite ( MotorIN1 , LOW ); + analogWrite ( MotorIN2, 200 ); + delay(2000); + digitalWrite ( MotorIN2 , LOW ); + analogWrite ( MotorIN1, 255 ); + delay(200); + digitalWrite ( MotorIN1, LOW ); + analogWrite ( MotorIN2, 255 ); + delay(180); + digitalWrite ( MotorIN2, LOW ); + analogWrite ( MotorIN1, 255 ); + delay(100); + digitalWrite ( MotorIN1, LOW ); + analogWrite ( MotorIN2, 255 ); + delay(90); + digitalWrite ( MotorIN2, LOW ); + analogWrite ( MotorIN1, 255 ); + delay(100); + digitalWrite ( MotorIN1, LOW ); + analogWrite ( MotorIN2, 200 ); + delay(2000); + + + StartRoutine = 1; + encoder0Pos=0; + } + +} + +void docalc() { + + if (millis() - previousMillis > interval) + { + previousMillis = millis(); + 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 motorspeed = 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(motorspeed > 0){ + if( motorspeed >= 255) motorspeed=255; + digitalWrite ( MotorIN1 , LOW ); + analogWrite ( MotorIN2, motorspeed ); + } + if(motorspeed < 0){ + motorspeed = -1 * motorspeed; + if( motorspeed >= 255) motorspeed=255; + digitalWrite ( MotorIN2 , LOW ); + analogWrite ( MotorIN1, motorspeed ); + //digitalWrite ( MotorIN1 , HIGH ); + //analogWrite ( MotorIN2, 255 - motorspeed ); + } + } +} + +void doEncoderMotor0(){ + //if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2) + if(digitalRead(encoder0PinA)==HIGH){ + + 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)==HIGH; + 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/digitalWriteFast.h b/Moteur DC/ServoStrap/digitalWriteFast.h old mode 100644 new mode 100755 diff --git a/Moteur DC/ServoStrap/keywords.txt b/Moteur DC/ServoStrap/keywords.txt old mode 100644 new mode 100755 diff --git a/Moteur DC/Test/Test_Moteur_DC/Test_Moteur_DC.ino b/Moteur DC/Test/Test_Moteur_DC/Test_Moteur_DC.ino old mode 100644 new mode 100755 diff --git a/Moteur DC/Test/Test_Roue_Codeuse/Test_Roue_Codeuse.ino b/Moteur DC/Test/Test_Roue_Codeuse/Test_Roue_Codeuse.ino old mode 100644 new mode 100755 diff --git a/README b/README old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/Imprimante_3d_fablab.SLDASM b/imprimante_3d_fablab/Imprimante_3d_fablab.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/Boulon M6 hexa 30mm.SLDPRT b/imprimante_3d_fablab/pièces/Boulon M6 hexa 30mm.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/HeatingBed_214x214mm.SLDPRT b/imprimante_3d_fablab/pièces/HeatingBed_214x214mm.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/NORDEX-ABX-C3-8.SLDPRT b/imprimante_3d_fablab/pièces/NORDEX-ABX-C3-8.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/Rexroth-Default.SLDPRT b/imprimante_3d_fablab/pièces/Rexroth-Default.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/Tasseau M6.SLDPRT b/imprimante_3d_fablab/pièces/Tasseau M6.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/User Library-fan - pabst 512f.SLDPRT b/imprimante_3d_fablab/pièces/User Library-fan - pabst 512f.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/ZERO MAX-SC030R-SC030R - 5 mm x 8 mm_SC030R_A-1.sldprt b/imprimante_3d_fablab/pièces/ZERO MAX-SC030R-SC030R - 5 mm x 8 mm_SC030R_A-1.sldprt old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/axe charnière.SLDPRT b/imprimante_3d_fablab/pièces/axe charnière.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/batant charnière.SLDPRT b/imprimante_3d_fablab/pièces/batant charnière.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/eclisse_40x40x222_bisote_x1.SLDPRT b/imprimante_3d_fablab/pièces/eclisse_40x40x222_bisote_x1.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/eclisse_40x40x404_bisote_x2.SLDPRT b/imprimante_3d_fablab/pièces/eclisse_40x40x404_bisote_x2.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/eclisse_40x40x670_bisote_x1.SLDPRT b/imprimante_3d_fablab/pièces/eclisse_40x40x670_bisote_x1.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/eclisse_40x40x704_bisote_x2.SLDPRT b/imprimante_3d_fablab/pièces/eclisse_40x40x704_bisote_x2.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_acrylique_380x210.SLDPRT b/imprimante_3d_fablab/pièces/plaque_acrylique_380x210.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_acrylique_380x670.SLDPRT b/imprimante_3d_fablab/pièces/plaque_acrylique_380x670.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_acrylique_380x680.SLDPRT b/imprimante_3d_fablab/pièces/plaque_acrylique_380x680.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_acrylique_680x210.SLDPRT b/imprimante_3d_fablab/pièces/plaque_acrylique_680x210.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_acrylique_680x670.SLDPRT b/imprimante_3d_fablab/pièces/plaque_acrylique_680x670.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_acrylique_680x670_porte.SLDPRT b/imprimante_3d_fablab/pièces/plaque_acrylique_680x670_porte.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_repartition_aire_chaud_300x544mm.SLDPRT b/imprimante_3d_fablab/pièces/plaque_repartition_aire_chaud_300x544mm.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/plaque_verre_428x210mm.SLDPRT b/imprimante_3d_fablab/pièces/plaque_verre_428x210mm.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/porte.SLDPRT b/imprimante_3d_fablab/pièces/porte.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/prof_40x40x300mm.SLDPRT b/imprimante_3d_fablab/pièces/prof_40x40x300mm.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/prof_40x40x600mm.SLDPRT b/imprimante_3d_fablab/pièces/prof_40x40x600mm.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/support_moteur_chassis.SLDPRT b/imprimante_3d_fablab/pièces/support_moteur_chassis.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/support_plateau.SLDPRT b/imprimante_3d_fablab/pièces/support_plateau.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/pièces/volume_d_impression.SLDPRT b/imprimante_3d_fablab/pièces/volume_d_impression.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Base.SLDPRT b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Base.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Cuerpo.SLDPRT b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Cuerpo.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Eje.SLDPRT b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Eje.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.SLDASM b/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.STEP b/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.STEP old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.STP b/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17 with T5 Pulley.STP old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17+support.SLDASM b/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17+support.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17.SLDASM b/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17.STL b/imprimante_3d_fablab/sous_assemblages/NEMA 17/NEMA 17.STL old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley Render.avi b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley Render.avi old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley Render.gif b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley Render.gif old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley.avi b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA 17 with T5 Pulley.avi old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 2.JPG b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 2.JPG old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 3.JPG b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 3.JPG old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 with T5 Pulley.JPG b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Renderings/NEMA17 with T5 Pulley.JPG old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/T5 Pulley.SLDPRT b/imprimante_3d_fablab/sous_assemblages/NEMA 17/T5 Pulley.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/Tapa.SLDPRT b/imprimante_3d_fablab/sous_assemblages/NEMA 17/Tapa.SLDPRT old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/NEMA 17/radial ball bearing_68_am.sldprt b/imprimante_3d_fablab/sous_assemblages/NEMA 17/radial ball bearing_68_am.sldprt old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/cadre_600x300mm.SLDASM b/imprimante_3d_fablab/sous_assemblages/cadre_600x300mm.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/carcasse.SLDASM b/imprimante_3d_fablab/sous_assemblages/carcasse.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/charniere.SLDASM b/imprimante_3d_fablab/sous_assemblages/charniere.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/couvercle.SLDASM b/imprimante_3d_fablab/sous_assemblages/couvercle.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/sous_assemblages/plateau_complet.SLDASM b/imprimante_3d_fablab/sous_assemblages/plateau_complet.SLDASM old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/vue_carcasse+plateau_16.11.2015.png b/imprimante_3d_fablab/vue_carcasse+plateau_16.11.2015.png old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/vue_carcasse+volume_impression_13.11.2015.png b/imprimante_3d_fablab/vue_carcasse+volume_impression_13.11.2015.png old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/vue_carcasse_13.11.2015.png b/imprimante_3d_fablab/vue_carcasse_13.11.2015.png old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/vue_carcasse_prof_only_16.11.2015.png b/imprimante_3d_fablab/vue_carcasse_prof_only_16.11.2015.png old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/vue_dessu_cotes_pour_core_XY_16.11.2015.png b/imprimante_3d_fablab/vue_dessu_cotes_pour_core_XY_16.11.2015.png old mode 100644 new mode 100755 diff --git a/imprimante_3d_fablab/vue_plateau_15.11.2015.png b/imprimante_3d_fablab/vue_plateau_15.11.2015.png old mode 100644 new mode 100755 From 7b91fc14ddfd569e17aeb59f6fcf08c7746d8b25 Mon Sep 17 00:00:00 2001 From: xctd692 Date: Thu, 3 Mar 2016 17:34:36 +0100 Subject: [PATCH 10/14] Mise au propre du code YATA --- .../ServoStrapPortManipulationLD293D_1_2.ino | 63 ++++++++++--------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino index c281146..66da4d2 100755 --- a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino +++ b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino @@ -1,30 +1,14 @@ #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. + /* +Fortement inspiré du projet : 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 : + Utilisation de la librairies digitalWriteFast qui permet d'utiliser la manipulation de port pour accélérer les lectures écritures de certaine entree/sortie : + 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) @@ -32,34 +16,46 @@ https://github.com/danithebest91/ServoStrap D (digital pins 0 to 7) https://www.arduino.cc/en/Reference/PortManipulation + Petit rappel sur la puce arduino : 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) +// --------------- INPUT --------------------------------------------------- + +// Les ports de l'encoder sont 2 pour bénéficier de l'interuption sur A et 8 pour utiliser le PortManipulation. Attention sur le port manipulation à ne pas utiliser les pin 9 à 13 ! +#define encoder0PinA 2 // PD2 (INT0) #define encoder0PinB 8 // PB0; (I8) +// Utilisation de 5 et 6 pour la commande moteur via le driver L293D. Le enable est connecté au vcc et le pwm est envoyé directement dans les input. #define MotorIN1 5 //(I5) IN1 #define MotorIN2 6 //(I6) IN2 -//from ramps 1.4 stepper driver +// Recuperation des step de la carte avec interuption sur le port 3 et PortManipulation sur le port A0. Attention sur le port manipulation à ne pas utiliser les pin A1 à A5 ! #define STEP_PIN 3 //PD3 (INT1) (I3) #define DIR_PIN 14 //PC0; (A0) + +// Récupération du fin de course x_min pour l'initialisation du moteur #define X_MIN 4 //(4) +// ------------------------------------------------------------------------ + +// ------------- CONSTANTE DU PID ------------- + +float KP = 5 ; //Porportionnel +float KI = 0.1; // Intergrale +float KD = 2.0; // derive + +// ------------- CONSTANTE DU PID ------------- + +// Initialisation de la position encoder. Attention celle-ci est incrémenté via un CHANGE sur l'interuption 0. Le nombre de pas de la roue codeuse est donc double ! volatile long encoder0Pos = 0; long target = 0; long target1 = 0; -int StartRoutine=0; -//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) -//PID controller constants -float KP = 5 ; //position multiplier (gain) 2.25 -float KI = 0.1; // Intergral multiplier (gain) .25 -float KD = 2.0; // derivative multiplier (gain) 1.0 - int lastError = 0; int sumError = 0; +int StartRoutine=0; //Integral term min/max (random value and not yet tested/verified) int iMax = 100; @@ -84,12 +80,16 @@ void setup() { pinModeFast(MotorIN1, OUTPUT); pinModeFast(MotorIN2, OUTPUT); - //ramps 1.4 motor control +// Initailisation recuperation commande carte pinModeFast(STEP_PIN, INPUT); pinModeFast(DIR_PIN, INPUT); +// INTERUPTION + attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2 attachInterrupt(1, countStep, RISING); //on pin 3 + +// INTERUPTION Serial.begin (115200); Serial.println("start"); // a personal quirk @@ -125,7 +125,8 @@ void loop(){ } void Fstart(){ - +//Fonction start joue une seul fois +// Devellopement de l'autotune en cours a intergrer au demarrage du moteur ainsi que le homing if (StartRoutine == 0) { digitalWrite ( MotorIN1 , LOW ); From 40deb5d84e375aceaa560006087420dc01697397 Mon Sep 17 00:00:00 2001 From: arthur L Date: Thu, 3 Mar 2016 20:55:27 +0100 Subject: [PATCH 11/14] Modif test arthur --- coucou.txt | 1 - 1 file changed, 1 deletion(-) delete mode 100644 coucou.txt diff --git a/coucou.txt b/coucou.txt deleted file mode 100644 index f526d2a..0000000 --- a/coucou.txt +++ /dev/null @@ -1 +0,0 @@ -hello the word From 7ee520b8e5708210410c527fe1028e1bc9274e82 Mon Sep 17 00:00:00 2001 From: arthur L Date: Thu, 3 Mar 2016 20:55:48 +0100 Subject: [PATCH 12/14] Test arthur 2 --- Coucou.txt | 1 - Hello | 14 -------------- 2 files changed, 15 deletions(-) delete mode 100644 Coucou.txt delete mode 100644 Hello diff --git a/Coucou.txt b/Coucou.txt deleted file mode 100644 index a802506..0000000 --- a/Coucou.txt +++ /dev/null @@ -1 +0,0 @@ -Bonjour je suis cool ! \ No newline at end of file diff --git a/Hello b/Hello deleted file mode 100644 index 0a69a17..0000000 --- a/Hello +++ /dev/null @@ -1,14 +0,0 @@ - - a@@@@a - a@@@@@@@@@@@@a - a@@@@@@by@@@@@@@@a - a@@@@@S@C@E@S@W@@@@@@a - @@@@@@@@@@@@@@@@@@@@@@ - `@@@@@@`\\//'@@@@@@' - ,,/ || \,, - /(-\ || /.)m - ,---' /`-'||`-'\ `----, - /( )__)) || ((,==( )\ -_ /_//___\\ __|| ___\\ __\\ ___ - `` `` /MM\ '' '' -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ From 3fa09f41be901092763f66435469413532554bd5 Mon Sep 17 00:00:00 2001 From: xctd692 Date: Fri, 4 Mar 2016 11:48:18 +0100 Subject: [PATCH 13/14] Ajout d'une fonction pour mesurer la vitesse du moteur --- .../ServoStrapPortManipulationLD293D_1_2.ino | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino index 66da4d2..c6c53b4 100755 --- a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino +++ b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino @@ -57,6 +57,10 @@ int lastError = 0; int sumError = 0; int StartRoutine=0; +float OldTime = 0; +float Oldencoder0Pos = 0; +float MySpeed = 0; + //Integral term min/max (random value and not yet tested/verified) int iMax = 100; int iMin = 0; @@ -158,6 +162,12 @@ void Fstart(){ } +void Speed(){ +MySpeed = (Oldencoder0Pos - encoder0Pos) / ( OldTime - millis()); +OldTime = millis(); +Oldencoder0Pos = encoder0Pos; +} + void docalc() { if (millis() - previousMillis > interval) From 1165a683e20c98aaf4dd9aaca5daaaa8dabf9573 Mon Sep 17 00:00:00 2001 From: Vincent M Date: Thu, 10 Mar 2016 18:02:07 +0100 Subject: [PATCH 14/14] nouvvelle version du code servostrap --- .../ServoStrapPortManipulationLD293D_1_3.ino} | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) rename Moteur DC/ServoStrap/{ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino => ServoStrapPortManipulationLD293D_1_3/ServoStrapPortManipulationLD293D_1_3.ino} (94%) mode change 100755 => 100644 diff --git a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_3/ServoStrapPortManipulationLD293D_1_3.ino old mode 100755 new mode 100644 similarity index 94% rename from Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino rename to Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_3/ServoStrapPortManipulationLD293D_1_3.ino index c6c53b4..04ae617 --- a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino +++ b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_3/ServoStrapPortManipulationLD293D_1_3.ino @@ -42,9 +42,9 @@ https://github.com/danithebest91/ServoStrap // ------------- CONSTANTE DU PID ------------- -float KP = 5 ; //Porportionnel -float KI = 0.1; // Intergrale -float KD = 2.0; // derive +float KP = 3 ; //Porportionnel +float KI = 2; // Intergrale +float KD = 15; // derive // ------------- CONSTANTE DU PID ------------- @@ -70,9 +70,9 @@ 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; +//bool newStep = false; +//bool oldStep = false; +//bool dir = false; void setup() { pinModeFast(2, INPUT); @@ -188,6 +188,8 @@ void docalc() { } else if(sumError < iMin){ sumError = iMin; } + + Serial.println(motorspeed); if(motorspeed > 0){ if( motorspeed >= 255) motorspeed=255; @@ -226,15 +228,19 @@ void doEncoderMotor0(){ else { encoder0Pos-- ; // CCW } - } } void countStep(){ //dir=digitalRead(DIR_PIN)==HIGH; - dir = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital; + //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--; + if (PINC&B0000001) { + target1++; + } + else + { + target1--; + } }