From 3b2c0b87516b162fdb5c9a6262954623f7bb2122 Mon Sep 17 00:00:00 2001 From: Vincent M Date: Tue, 26 Jan 2016 22:25:53 +0100 Subject: [PATCH] Ajout suite code Arduino moteur DC --- .../PID_AutoTune_v0/AutoTune_Example.pde | 161 +++++++++ .../AutoTune_Example/AutoTune_Example.pde | 161 +++++++++ .../AutoTune_Example/PID_AutoTune_v0.cpp | 196 ++++++++++ .../AutoTune_Example/PID_AutoTune_v0.h | 55 +++ .../AutoTune_Example/PID_v1.cpp | 186 ++++++++++ .../PID_AutoTune_v0/AutoTune_Example/PID_v1.h | 80 +++++ .../AutoTune_Example/AutoTune_Example.pde | 161 +++++++++ .../PID_AutoTune_v0/PID_AutoTune_v0.cpp | 196 ++++++++++ .../PID_AutoTune_v0/PID_AutoTune_v0.h | 55 +++ .../Arduino-PID-AutoTune-Library/README.txt | 37 ++ .../Arduino_stepper_motor_emulator_v1.0.0.pde | 180 ++++++++++ ...ulator_v1.0.1_direct_port_manipulation.pde | 191 ++++++++++ .../STM32_olimexino_stepper_motor_emulator | 175 +++++++++ .../keywords.txt | 20 ++ .../Driver_printer/Driver_printer.ino | 192 ++++++++++ .../libraries/digitalWriteFast.h | 165 +++++++++ Moteur DC/ServoStrap/LICENSE | 339 ++++++++++++++++++ Moteur DC/ServoStrap/README.md | 46 +++ .../STM32_olimexino_stepper_motor_emulator | 175 +++++++++ Moteur DC/ServoStrap/digitalWriteFast.h | 165 +++++++++ Moteur DC/ServoStrap/keywords.txt | 20 ++ 21 files changed, 2956 insertions(+) create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example.pde create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/AutoTune_Example.pde create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.cpp create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.h create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.cpp create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.h create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/Examples/AutoTune_Example/AutoTune_Example.pde create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.cpp create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.h create mode 100644 Moteur DC/Arduino-PID-AutoTune-Library/README.txt create mode 100644 Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.0.pde create mode 100644 Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde create mode 100644 Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator create mode 100644 Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt create mode 100644 Moteur DC/ServoStrap/Driver_printer/Driver_printer.ino create mode 100644 Moteur DC/ServoStrap/Driver_printer/libraries/digitalWriteFast.h create mode 100644 Moteur DC/ServoStrap/LICENSE create mode 100644 Moteur DC/ServoStrap/README.md create mode 100644 Moteur DC/ServoStrap/STM32_olimexino_stepper_motor_emulator create mode 100644 Moteur DC/ServoStrap/digitalWriteFast.h create mode 100644 Moteur DC/ServoStrap/keywords.txt 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 new file mode 100644 index 0000000..64131f3 --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example.pde @@ -0,0 +1,161 @@ +#include +#include + +byte ATuneModeRemember=2; +double input=80, output=50, setpoint=180; +double kp=2,ki=0.5,kd=2; + +double kpmodel=1.5, taup=100, theta[50]; +double outputStart=5; +double aTuneStep=50, aTuneNoise=1, aTuneStartValue=100; +unsigned int aTuneLookBack=20; + +boolean tuning = false; +unsigned long modelTime, serialTime; + +PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT); +PID_ATune aTune(&input, &output); + +//set to false to connect to the real world +boolean useSimulation = true; + +void setup() +{ + if(useSimulation) + { + for(byte i=0;i<50;i++) + { + theta[i]=outputStart; + } + modelTime = 0; + } + //Setup the pid + myPID.SetMode(AUTOMATIC); + + if(tuning) + { + tuning=false; + changeAutoTune(); + tuning=true; + } + + serialTime = 0; + Serial.begin(9600); + +} + +void loop() +{ + + unsigned long now = millis(); + + if(!useSimulation) + { //pull the input in from the real world + input = analogRead(0); + } + + if(tuning) + { + byte val = (aTune.Runtime()); + if (val!=0) + { + tuning = false; + } + if(!tuning) + { //we're done, set the tuning parameters + kp = aTune.GetKp(); + ki = aTune.GetKi(); + kd = aTune.GetKd(); + myPID.SetTunings(kp,ki,kd); + AutoTuneHelper(false); + } + } + else myPID.Compute(); + + if(useSimulation) + { + theta[30]=output; + if(now>=modelTime) + { + modelTime +=100; + DoModel(); + } + } + else + { + analogWrite(0,output); + } + + //send-receive with processing if it's time + if(millis()>serialTime) + { + SerialReceive(); + SerialSend(); + serialTime+=500; + } +} + +void changeAutoTune() +{ + if(!tuning) + { + //Set the output to the desired starting frequency. + output=aTuneStartValue; + aTune.SetNoiseBand(aTuneNoise); + aTune.SetOutputStep(aTuneStep); + aTune.SetLookbackSec((int)aTuneLookBack); + AutoTuneHelper(true); + tuning = true; + } + else + { //cancel autotune + aTune.Cancel(); + tuning = false; + AutoTuneHelper(false); + } +} + +void AutoTuneHelper(boolean start) +{ + if(start) + ATuneModeRemember = myPID.GetMode(); + else + myPID.SetMode(ATuneModeRemember); +} + + +void SerialSend() +{ + Serial.print("setpoint: ");Serial.print(setpoint); Serial.print(" "); + Serial.print("input: ");Serial.print(input); Serial.print(" "); + Serial.print("output: ");Serial.print(output); Serial.print(" "); + if(tuning){ + Serial.println("tuning mode"); + } else { + Serial.print("kp: ");Serial.print(myPID.GetKp());Serial.print(" "); + Serial.print("ki: ");Serial.print(myPID.GetKi());Serial.print(" "); + Serial.print("kd: ");Serial.print(myPID.GetKd());Serial.println(); + } +} + +void SerialReceive() +{ + if(Serial.available()) + { + char b = Serial.read(); + Serial.flush(); + if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune(); + } +} + +void DoModel() +{ + //cycle the dead time + for(byte i=0;i<49;i++) + { + theta[i] = theta[i+1]; + } + //compute the input + input = (kpmodel / taup) *(theta[0]-outputStart) + input*(1-1/taup) + ((float)random(-10,10))/100; + +} 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 new file mode 100644 index 0000000..64131f3 --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/AutoTune_Example.pde @@ -0,0 +1,161 @@ +#include +#include + +byte ATuneModeRemember=2; +double input=80, output=50, setpoint=180; +double kp=2,ki=0.5,kd=2; + +double kpmodel=1.5, taup=100, theta[50]; +double outputStart=5; +double aTuneStep=50, aTuneNoise=1, aTuneStartValue=100; +unsigned int aTuneLookBack=20; + +boolean tuning = false; +unsigned long modelTime, serialTime; + +PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT); +PID_ATune aTune(&input, &output); + +//set to false to connect to the real world +boolean useSimulation = true; + +void setup() +{ + if(useSimulation) + { + for(byte i=0;i<50;i++) + { + theta[i]=outputStart; + } + modelTime = 0; + } + //Setup the pid + myPID.SetMode(AUTOMATIC); + + if(tuning) + { + tuning=false; + changeAutoTune(); + tuning=true; + } + + serialTime = 0; + Serial.begin(9600); + +} + +void loop() +{ + + unsigned long now = millis(); + + if(!useSimulation) + { //pull the input in from the real world + input = analogRead(0); + } + + if(tuning) + { + byte val = (aTune.Runtime()); + if (val!=0) + { + tuning = false; + } + if(!tuning) + { //we're done, set the tuning parameters + kp = aTune.GetKp(); + ki = aTune.GetKi(); + kd = aTune.GetKd(); + myPID.SetTunings(kp,ki,kd); + AutoTuneHelper(false); + } + } + else myPID.Compute(); + + if(useSimulation) + { + theta[30]=output; + if(now>=modelTime) + { + modelTime +=100; + DoModel(); + } + } + else + { + analogWrite(0,output); + } + + //send-receive with processing if it's time + if(millis()>serialTime) + { + SerialReceive(); + SerialSend(); + serialTime+=500; + } +} + +void changeAutoTune() +{ + if(!tuning) + { + //Set the output to the desired starting frequency. + output=aTuneStartValue; + aTune.SetNoiseBand(aTuneNoise); + aTune.SetOutputStep(aTuneStep); + aTune.SetLookbackSec((int)aTuneLookBack); + AutoTuneHelper(true); + tuning = true; + } + else + { //cancel autotune + aTune.Cancel(); + tuning = false; + AutoTuneHelper(false); + } +} + +void AutoTuneHelper(boolean start) +{ + if(start) + ATuneModeRemember = myPID.GetMode(); + else + myPID.SetMode(ATuneModeRemember); +} + + +void SerialSend() +{ + Serial.print("setpoint: ");Serial.print(setpoint); Serial.print(" "); + Serial.print("input: ");Serial.print(input); Serial.print(" "); + Serial.print("output: ");Serial.print(output); Serial.print(" "); + if(tuning){ + Serial.println("tuning mode"); + } else { + Serial.print("kp: ");Serial.print(myPID.GetKp());Serial.print(" "); + Serial.print("ki: ");Serial.print(myPID.GetKi());Serial.print(" "); + Serial.print("kd: ");Serial.print(myPID.GetKd());Serial.println(); + } +} + +void SerialReceive() +{ + if(Serial.available()) + { + char b = Serial.read(); + Serial.flush(); + if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune(); + } +} + +void DoModel() +{ + //cycle the dead time + for(byte i=0;i<49;i++) + { + theta[i] = theta[i+1]; + } + //compute the input + input = (kpmodel / taup) *(theta[0]-outputStart) + input*(1-1/taup) + ((float)random(-10,10))/100; + +} 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 new file mode 100644 index 0000000..4d0e11e --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.cpp @@ -0,0 +1,196 @@ +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include + + +PID_ATune::PID_ATune(double* Input, double* Output) +{ + input = Input; + output = Output; + controlType =0 ; //default to PI + noiseBand = 0.5; + running = false; + oStep = 30; + SetLookbackSec(10); + lastTime = millis(); + +} + + + +void PID_ATune::Cancel() +{ + running = false; +} + +int PID_ATune::Runtime() +{ + justevaled=false; + if(peakCount>9 && running) + { + running = false; + FinishUp(); + return 1; + } + unsigned long now = millis(); + + if((now-lastTime)absMax)absMax=refVal; + if(refValsetpoint+noiseBand) *output = outputStart-oStep; + else if (refVal=0;i--) + { + double val = lastInputs[i]; + if(isMax) isMax = refVal>val; + if(isMin) isMin = refVal2) + { //we've transitioned. check if we can autotune based on the last peaks + double avgSeparation = (abs(peaks[peakCount-1]-peaks[peakCount-2])+abs(peaks[peakCount-2]-peaks[peakCount-3]))/2; + if( avgSeparation < 0.05*(absMax-absMin)) + { + FinishUp(); + running = false; + return 1; + + } + } + justchanged=false; + return 0; +} +void PID_ATune::FinishUp() +{ + *output = outputStart; + //we can generate tuning parameters! + Ku = 4*(2*oStep)/((absMax-absMin)*3.14159); + Pu = (double)(peak1-peak2) / 1000; +} + +double PID_ATune::GetKp() +{ + return controlType==1 ? 0.6 * Ku : 0.4 * Ku; +} + +double PID_ATune::GetKi() +{ + return controlType==1? 1.2*Ku / Pu : 0.48 * Ku / Pu; // Ki = Kc/Ti +} + +double PID_ATune::GetKd() +{ + return controlType==1? 0.075 * Ku * Pu : 0; //Kd = Kc * Td +} + +void PID_ATune::SetOutputStep(double Step) +{ + oStep = Step; +} + +double PID_ATune::GetOutputStep() +{ + return oStep; +} + +void PID_ATune::SetControlType(int Type) //0=PI, 1=PID +{ + controlType = Type; +} +int PID_ATune::GetControlType() +{ + return controlType; +} + +void PID_ATune::SetNoiseBand(double Band) +{ + noiseBand = Band; +} + +double PID_ATune::GetNoiseBand() +{ + return noiseBand; +} + +void PID_ATune::SetLookbackSec(int value) +{ + if (value<1) value = 1; + + if(value<25) + { + nLookBack = value * 4; + sampleTime = 250; + } + else + { + nLookBack = 100; + sampleTime = value*10; + } +} + +int PID_ATune::GetLookbackSec() +{ + return nLookBack * sampleTime / 1000; +} 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 new file mode 100644 index 0000000..f029c06 --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_AutoTune_v0.h @@ -0,0 +1,55 @@ +#ifndef PID_AutoTune_v0 +#define PID_AutoTune_v0 +#define LIBRARY_VERSION 0.0.1 + +class PID_ATune +{ + + + public: + //commonly used functions ************************************************************************** + PID_ATune(double*, double*); // * Constructor. links the Autotune to a given PID + int Runtime(); // * Similar to the PID Compue function, returns non 0 when done + void Cancel(); // * Stops the AutoTune + + void SetOutputStep(double); // * how far above and below the starting value will the output step? + double GetOutputStep(); // + + void SetControlType(int); // * Determies if the tuning parameters returned will be PI (D=0) + int GetControlType(); // or PID. (0=PI, 1=PID) + + void SetLookbackSec(int); // * how far back are we looking to identify peaks + int GetLookbackSec(); // + + void SetNoiseBand(double); // * the autotune will ignore signal chatter smaller than this value + double GetNoiseBand(); // this should be acurately set + + double GetKp(); // * once autotune is complete, these functions contain the + double GetKi(); // computed tuning parameters. + double GetKd(); // + + private: + void FinishUp(); + bool isMax, isMin; + double *input, *output; + double setpoint; + double noiseBand; + int controlType; + bool running; + unsigned long peak1, peak2, lastTime; + int sampleTime; + int nLookBack; + int peakType; + double lastInputs[101]; + double peaks[10]; + int peakCount; + bool justchanged; + bool justevaled; + double absMax, absMin; + double oStep; + double outputStart; + double Ku, Pu; + +}; +#endif + 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 new file mode 100644 index 0000000..661537b --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.cpp @@ -0,0 +1,186 @@ +/********************************************************************************************** + * Arduino PID Library - Version 1 + * by Brett Beauregard brettbeauregard.com + * + * This Code is licensed under a Creative Commons Attribution-ShareAlike 3.0 Unported License. + **********************************************************************************************/ + +#include +#include "PID_v1.h" + +/*Constructor (...)********************************************************* + * The parameters specified here are those for for which we can't set up + * reliable defaults, so we need to have the user set them. + ***************************************************************************/ +PID::PID(double* Input, double* Output, double* Setpoint, + double Kp, double Ki, double Kd, int ControllerDirection) +{ + PID::SetOutputLimits(0, 255); //default output limit corresponds to + //the arduino pwm limits + + SampleTime = 100; //default Controller Sample Time is 0.1 seconds + + PID::SetControllerDirection(ControllerDirection); + PID::SetTunings(Kp, Ki, Kd); + + lastTime = millis()-SampleTime; + inAuto = false; + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + +} + + +/* Compute() ********************************************************************** + * This, as they say, is where the magic happens. this function should be called + * every time "void loop()" executes. the function will decide for itself whether a new + * pid Output needs to be computed + **********************************************************************************/ +void PID::Compute() +{ + if(!inAuto) return; + unsigned long now = millis(); + int timeChange = (now - lastTime); + if(timeChange>=SampleTime) + { + /*Compute all the working error variables*/ + double input = *myInput; + double error = *mySetpoint - input; + ITerm+= (ki * error); + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + double dInput = (input - lastInput); + + /*Compute PID Output*/ + double output = kp * error + ITerm- kd * dInput; + + if(output > outMax) output = outMax; + else if(output < outMin) output = outMin; + *myOutput = output; + + /*Remember some variables for next time*/ + lastInput = input; + lastTime = now; + } +} + + +/* SetTunings(...)************************************************************* + * This function allows the controller's dynamic performance to be adjusted. + * it's called automatically from the constructor, but tunings can also + * be adjusted on the fly during normal operation + ******************************************************************************/ +void PID::SetTunings(double Kp, double Ki, double Kd) +{ + if (Kp<0 || Ki<0 || Kd<0) return; + + dispKp = Kp; dispKi = Ki; dispKd = Kd; + + double SampleTimeInSec = ((double)SampleTime)/1000; + kp = Kp; + ki = Ki * SampleTimeInSec; + kd = Kd / SampleTimeInSec; + + if(controllerDirection ==REVERSE) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } +} + +/* SetSampleTime(...) ********************************************************* + * sets the period, in Milliseconds, at which the calculation is performed + ******************************************************************************/ +void PID::SetSampleTime(int NewSampleTime) +{ + if (NewSampleTime > 0) + { + double ratio = (double)NewSampleTime + / (double)SampleTime; + ki *= ratio; + kd /= ratio; + SampleTime = (unsigned long)NewSampleTime; + } +} + +/* SetOutputLimits(...)**************************************************** + * This function will be used far more often than SetInputLimits. while + * the input to the controller will generally be in the 0-1023 range (which is + * the default already,) the output will be a little different. maybe they'll + * be doing a time window and will need 0-8000 or something. or maybe they'll + * want to clamp it from 0-125. who knows. at any rate, that can all be done + * here. + **************************************************************************/ +void PID::SetOutputLimits(double Min, double Max) +{ + if(Min >= Max) return; + outMin = Min; + outMax = Max; + + if(inAuto) + { + if(*myOutput > outMax) *myOutput = outMax; + else if(*myOutput < outMin) *myOutput = outMin; + + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + } +} + +/* SetMode(...)**************************************************************** + * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + * when the transition from manual to auto occurs, the controller is + * automatically initialized + ******************************************************************************/ +void PID::SetMode(int Mode) +{ + bool newAuto = (Mode == AUTOMATIC); + if(newAuto == !inAuto) + { /*we just went from manual to auto*/ + PID::Initialize(); + } + inAuto = newAuto; +} + +/* Initialize()**************************************************************** + * does all the things that need to happen to ensure a bumpless transfer + * from manual to automatic mode. + ******************************************************************************/ +void PID::Initialize() +{ + ITerm = *myOutput; + lastInput = *myInput; + if(ITerm > outMax) ITerm = outMax; + else if(ITerm < outMin) ITerm = outMin; +} + +/* SetControllerDirection(...)************************************************* + * The PID will either be connected to a DIRECT acting process (+Output leads + * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + * know which one, because otherwise we may increase the output when we should + * be decreasing. This is called from the constructor. + ******************************************************************************/ +void PID::SetControllerDirection(int Direction) +{ + if(inAuto && Direction !=controllerDirection) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + controllerDirection = Direction; +} + +/* Status Funcions************************************************************* + * Just because you set the Kp=-1 doesn't mean it actually happened. these + * functions query the internal state of the PID. they're here for display + * purposes. this are the functions the PID Front-end uses for example + ******************************************************************************/ +double PID::GetKp(){ return dispKp; } +double PID::GetKi(){ return dispKi;} +double PID::GetKd(){ return dispKd;} +int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} +int PID::GetDirection(){ return controllerDirection;} + 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 new file mode 100644 index 0000000..2b3874f --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/AutoTune_Example/PID_v1.h @@ -0,0 +1,80 @@ +#ifndef PID_v1_h +#define PID_v1_h +#define LIBRARY_VERSION 1.0.0 + +class PID +{ + + + public: + + //Constants used in some of the functions below + #define AUTOMATIC 1 + #define MANUAL 0 + #define DIRECT 0 + #define REVERSE 1 + + //commonly used functions ************************************************************************** + PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and + double, double, double, int); // Setpoint. Initial tuning parameters are also set here + + void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) + + void Compute(); // * performs the PID calculation. it should be + // called every time loop() cycles. ON/OFF and + // calculation frequency can be set using SetMode + // SetSampleTime respectively + + void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but + //it's likely the user will want to change this depending on + //the application + + + + //available but not commonly used functions ******************************************************** + void SetTunings(double, double, // * While most users will set the tunings once in the + double); // constructor, this function gives the user the option + // of changing tunings during runtime for Adaptive control + void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT + // means the output will increase when error is positive. REVERSE + // means the opposite. it's very unlikely that this will be needed + // once it is set in the constructor. + void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which + // the PID calculation is performed. default is 100 + + + + //Display functions **************************************************************** + double GetKp(); // These functions query the pid for interal values. + double GetKi(); // they were created mainly for the pid front-end, + double GetKd(); // where it's important to know what is actually + int GetMode(); // inside the PID. + int GetDirection(); // + + private: + void Initialize(); + + double dispKp; // * we'll hold on to the tuning parameters in user-entered + double dispKi; // format for display purposes + double dispKd; // + + double kp; // * (P)roportional Tuning Parameter + double ki; // * (I)ntegral Tuning Parameter + double kd; // * (D)erivative Tuning Parameter + + int controllerDirection; + + double *myInput; // * Pointers to the Input, Output, and Setpoint variables + double *myOutput; // This creates a hard link between the variables and the + double *mySetpoint; // PID, freeing the user from having to constantly tell us + // what these values are. with pointers we'll just know. + + unsigned long lastTime; + double ITerm, lastInput; + + int SampleTime; + double outMin, outMax; + bool inAuto; +}; +#endif + 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 new file mode 100644 index 0000000..64131f3 --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/Examples/AutoTune_Example/AutoTune_Example.pde @@ -0,0 +1,161 @@ +#include +#include + +byte ATuneModeRemember=2; +double input=80, output=50, setpoint=180; +double kp=2,ki=0.5,kd=2; + +double kpmodel=1.5, taup=100, theta[50]; +double outputStart=5; +double aTuneStep=50, aTuneNoise=1, aTuneStartValue=100; +unsigned int aTuneLookBack=20; + +boolean tuning = false; +unsigned long modelTime, serialTime; + +PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT); +PID_ATune aTune(&input, &output); + +//set to false to connect to the real world +boolean useSimulation = true; + +void setup() +{ + if(useSimulation) + { + for(byte i=0;i<50;i++) + { + theta[i]=outputStart; + } + modelTime = 0; + } + //Setup the pid + myPID.SetMode(AUTOMATIC); + + if(tuning) + { + tuning=false; + changeAutoTune(); + tuning=true; + } + + serialTime = 0; + Serial.begin(9600); + +} + +void loop() +{ + + unsigned long now = millis(); + + if(!useSimulation) + { //pull the input in from the real world + input = analogRead(0); + } + + if(tuning) + { + byte val = (aTune.Runtime()); + if (val!=0) + { + tuning = false; + } + if(!tuning) + { //we're done, set the tuning parameters + kp = aTune.GetKp(); + ki = aTune.GetKi(); + kd = aTune.GetKd(); + myPID.SetTunings(kp,ki,kd); + AutoTuneHelper(false); + } + } + else myPID.Compute(); + + if(useSimulation) + { + theta[30]=output; + if(now>=modelTime) + { + modelTime +=100; + DoModel(); + } + } + else + { + analogWrite(0,output); + } + + //send-receive with processing if it's time + if(millis()>serialTime) + { + SerialReceive(); + SerialSend(); + serialTime+=500; + } +} + +void changeAutoTune() +{ + if(!tuning) + { + //Set the output to the desired starting frequency. + output=aTuneStartValue; + aTune.SetNoiseBand(aTuneNoise); + aTune.SetOutputStep(aTuneStep); + aTune.SetLookbackSec((int)aTuneLookBack); + AutoTuneHelper(true); + tuning = true; + } + else + { //cancel autotune + aTune.Cancel(); + tuning = false; + AutoTuneHelper(false); + } +} + +void AutoTuneHelper(boolean start) +{ + if(start) + ATuneModeRemember = myPID.GetMode(); + else + myPID.SetMode(ATuneModeRemember); +} + + +void SerialSend() +{ + Serial.print("setpoint: ");Serial.print(setpoint); Serial.print(" "); + Serial.print("input: ");Serial.print(input); Serial.print(" "); + Serial.print("output: ");Serial.print(output); Serial.print(" "); + if(tuning){ + Serial.println("tuning mode"); + } else { + Serial.print("kp: ");Serial.print(myPID.GetKp());Serial.print(" "); + Serial.print("ki: ");Serial.print(myPID.GetKi());Serial.print(" "); + Serial.print("kd: ");Serial.print(myPID.GetKd());Serial.println(); + } +} + +void SerialReceive() +{ + if(Serial.available()) + { + char b = Serial.read(); + Serial.flush(); + if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune(); + } +} + +void DoModel() +{ + //cycle the dead time + for(byte i=0;i<49;i++) + { + theta[i] = theta[i+1]; + } + //compute the input + input = (kpmodel / taup) *(theta[0]-outputStart) + input*(1-1/taup) + ((float)random(-10,10))/100; + +} 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 new file mode 100644 index 0000000..4d0e11e --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.cpp @@ -0,0 +1,196 @@ +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include + + +PID_ATune::PID_ATune(double* Input, double* Output) +{ + input = Input; + output = Output; + controlType =0 ; //default to PI + noiseBand = 0.5; + running = false; + oStep = 30; + SetLookbackSec(10); + lastTime = millis(); + +} + + + +void PID_ATune::Cancel() +{ + running = false; +} + +int PID_ATune::Runtime() +{ + justevaled=false; + if(peakCount>9 && running) + { + running = false; + FinishUp(); + return 1; + } + unsigned long now = millis(); + + if((now-lastTime)absMax)absMax=refVal; + if(refValsetpoint+noiseBand) *output = outputStart-oStep; + else if (refVal=0;i--) + { + double val = lastInputs[i]; + if(isMax) isMax = refVal>val; + if(isMin) isMin = refVal2) + { //we've transitioned. check if we can autotune based on the last peaks + double avgSeparation = (abs(peaks[peakCount-1]-peaks[peakCount-2])+abs(peaks[peakCount-2]-peaks[peakCount-3]))/2; + if( avgSeparation < 0.05*(absMax-absMin)) + { + FinishUp(); + running = false; + return 1; + + } + } + justchanged=false; + return 0; +} +void PID_ATune::FinishUp() +{ + *output = outputStart; + //we can generate tuning parameters! + Ku = 4*(2*oStep)/((absMax-absMin)*3.14159); + Pu = (double)(peak1-peak2) / 1000; +} + +double PID_ATune::GetKp() +{ + return controlType==1 ? 0.6 * Ku : 0.4 * Ku; +} + +double PID_ATune::GetKi() +{ + return controlType==1? 1.2*Ku / Pu : 0.48 * Ku / Pu; // Ki = Kc/Ti +} + +double PID_ATune::GetKd() +{ + return controlType==1? 0.075 * Ku * Pu : 0; //Kd = Kc * Td +} + +void PID_ATune::SetOutputStep(double Step) +{ + oStep = Step; +} + +double PID_ATune::GetOutputStep() +{ + return oStep; +} + +void PID_ATune::SetControlType(int Type) //0=PI, 1=PID +{ + controlType = Type; +} +int PID_ATune::GetControlType() +{ + return controlType; +} + +void PID_ATune::SetNoiseBand(double Band) +{ + noiseBand = Band; +} + +double PID_ATune::GetNoiseBand() +{ + return noiseBand; +} + +void PID_ATune::SetLookbackSec(int value) +{ + if (value<1) value = 1; + + if(value<25) + { + nLookBack = value * 4; + sampleTime = 250; + } + else + { + nLookBack = 100; + sampleTime = value*10; + } +} + +int PID_ATune::GetLookbackSec() +{ + return nLookBack * sampleTime / 1000; +} 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 new file mode 100644 index 0000000..f029c06 --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/PID_AutoTune_v0/PID_AutoTune_v0.h @@ -0,0 +1,55 @@ +#ifndef PID_AutoTune_v0 +#define PID_AutoTune_v0 +#define LIBRARY_VERSION 0.0.1 + +class PID_ATune +{ + + + public: + //commonly used functions ************************************************************************** + PID_ATune(double*, double*); // * Constructor. links the Autotune to a given PID + int Runtime(); // * Similar to the PID Compue function, returns non 0 when done + void Cancel(); // * Stops the AutoTune + + void SetOutputStep(double); // * how far above and below the starting value will the output step? + double GetOutputStep(); // + + void SetControlType(int); // * Determies if the tuning parameters returned will be PI (D=0) + int GetControlType(); // or PID. (0=PI, 1=PID) + + void SetLookbackSec(int); // * how far back are we looking to identify peaks + int GetLookbackSec(); // + + void SetNoiseBand(double); // * the autotune will ignore signal chatter smaller than this value + double GetNoiseBand(); // this should be acurately set + + double GetKp(); // * once autotune is complete, these functions contain the + double GetKi(); // computed tuning parameters. + double GetKd(); // + + private: + void FinishUp(); + bool isMax, isMin; + double *input, *output; + double setpoint; + double noiseBand; + int controlType; + bool running; + unsigned long peak1, peak2, lastTime; + int sampleTime; + int nLookBack; + int peakType; + double lastInputs[101]; + double peaks[10]; + int peakCount; + bool justchanged; + bool justevaled; + double absMax, absMin; + double oStep; + double outputStart; + double Ku, Pu; + +}; +#endif + diff --git a/Moteur DC/Arduino-PID-AutoTune-Library/README.txt b/Moteur DC/Arduino-PID-AutoTune-Library/README.txt new file mode 100644 index 0000000..60811db --- /dev/null +++ b/Moteur DC/Arduino-PID-AutoTune-Library/README.txt @@ -0,0 +1,37 @@ +/********************************************************************************************** + * Arduino PID AutoTune Library - Version 0.0.1 + * by Brett Beauregard brettbeauregard.com + * + * This Library is ported from the AutotunerPID Toolkit by William Spinelli + * (http://www.mathworks.com/matlabcentral/fileexchange/4652) + * Copyright (c) 2004 + * + * This Library is licensed under the BSD License: + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the distribution + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + **********************************************************************************************/ + + Note: I'd really hoped to have this more polished before release, but with the + osPID coming out I felt that this needed to be out there NOW. if you + encounter any issues please contact me, or post to the diy-pid-control + google group. + \ No newline at end of file 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 new file mode 100644 index 0000000..af5f70a --- /dev/null +++ b/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.0.pde @@ -0,0 +1,180 @@ +/* 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 + +*/ + +#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. + +#define encoder0PinA 2 +#define encoder0PinB 4 + +#define SpeedPin 9 +#define DirectionPin 8 + +//from ramps 1.4 stepper driver +#define STEP_PIN 3 +#define DIR_PIN 12 +#define ENABLE_PIN 13 + + +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() { + + pinMode(encoder0PinA, INPUT); + pinMode(encoder0PinB, INPUT); + + pinMode(DirectionPin, OUTPUT); + pinMode(SpeedPin, OUTPUT); + + //ramps 1.4 motor control + pinMode(STEP_PIN, INPUT); + pinMode(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); +} + + /*if(millis() - previousTarget > 500){ //enable this code only for test purposes + 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){ + digitalWrite ( DirectionPin ,HIGH ); + } + if(ms < 0){ + digitalWrite ( DirectionPin , LOW ); + ms = -1 * ms; + } + + int motorspeed = map(ms,0,amp,0,255); + if( motorspeed >= 255) motorspeed=255; + //analogWrite ( SpeedPin, (255 - motorSpeed) ); + analogWrite ( SpeedPin, motorspeed ); + //Serial.print ( ms ); + //Serial.print ( ',' ); + //Serial.println ( motorspeed ); + } +} + +void doEncoderMotor0(){ + if (digitalReadFast2(encoder0PinA) == HIGH) { // found a low-to-high on channel A + if (digitalReadFast2(encoder0PinB) == LOW) { // check channel B to see which way + // encoder is turning + encoder0Pos = encoder0Pos - 1; // CCW + } + else { + encoder0Pos = encoder0Pos + 1; // CW + } + } + else // found a high-to-low on channel A + { + if (digitalReadFast2(encoder0PinB) == LOW) { // check channel B to see which way + // encoder is turning + encoder0Pos = encoder0Pos + 1; // CW + } + else { + encoder0Pos = encoder0Pos - 1; // CCW + } + + } + +} + +void countStep(){ + dir = digitalRead(DIR_PIN); + if (dir) target1++; + else target1--; +} 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/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde new file mode 100644 index 0000000..fd98143 --- /dev/null +++ b/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde @@ -0,0 +1,191 @@ +#include + +/* 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 + +*/ + +#define encoder0PinA 2 // PD2; +#define encoder0PinB 8 // PB0; + +#define SpeedPin 6 +#define DirectionPin 15 //PC1; + +//from ramps 1.4 stepper driver +#define STEP_PIN 3 //PD3; +#define DIR_PIN 14 //PC0; +//#define ENABLE_PIN 13 //PB5; for now is USELESS + +//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 + } + if(ms < 0){ + PORTC &=(B11111101); //digitalWriteFast2 ( DirectionPin , LOW ); write PC1 LOW + 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 PB0 + // because PD0is used for serial, i will change in the stable version TO USE PD2 + if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 + // 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 + // 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/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator b/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator new file mode 100644 index 0000000..04abab1 --- /dev/null +++ b/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator @@ -0,0 +1,175 @@ +/* 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 SerialUSB 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 + +*/ + +#define encoder0PinA 2 +#define encoder0PinB 4 + +#define SpeedPin 9 +#define DirectionPin 8 + +//from ramps 1.4 stepper driver +#define STEP_PIN 3 +#define DIR_PIN 12 +#define ENABLE_PIN 13 + + +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() { + + pinMode(encoder0PinA, INPUT); + pinMode(encoder0PinB, INPUT); + + pinMode(DirectionPin, OUTPUT); + pinMode(SpeedPin, OUTPUT); + + //ramps 1.4 motor control + pinMode(STEP_PIN, INPUT); + pinMode(DIR_PIN, INPUT); + + attachInterrupt(2, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2 + attachInterrupt(3, countStep, RISING); //on pin 3 + + SerialUSB.println("start"); // a personal quirk + +} + +void loop(){ + + while (SerialUSB.available() > 0) { + KP = SerialUSB.read(); + KD = SerialUSB.read(); + KI = SerialUSB.read(); + + + SerialUSB.println(KP); + SerialUSB.println(KD); + SerialUSB.println(KI); +} + + if(millis() - previousTarget > 500){ //enable this code only for test purposes + SerialUSB.print(encoder0Pos); + SerialUSB.print(','); + SerialUSB.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){ + digitalWrite ( DirectionPin ,HIGH ); + } + if(ms < 0){ + digitalWrite ( DirectionPin , LOW ); + ms = -1 * ms; + } + + int motorspeed = map(ms,0,amp,0,255); + if( motorspeed >= 255) motorspeed=255; + //analogWrite ( SpeedPin, (255 - motorSpeed) ); + analogWrite ( SpeedPin, motorspeed ); + //SerialUSB.print ( ms ); + //SerialUSB.print ( ',' ); + //SerialUSB.println ( motorspeed ); + } +} + +void doEncoderMotor0(){ + if (digitalRead(encoder0PinA) == HIGH) { // found a low-to-high on channel A + if (digitalRead(encoder0PinB) == LOW) { // check channel B to see which way + // encoder is turning + encoder0Pos = encoder0Pos - 1; // CCW + } + else { + encoder0Pos = encoder0Pos + 1; // CW + } + } + else // found a high-to-low on channel A + { + if (digitalRead(encoder0PinB) == LOW) { // check channel B to see which way + // encoder is turning + encoder0Pos = encoder0Pos + 1; // CW + } + else { + encoder0Pos = encoder0Pos - 1; // CCW + } + + } + +} + +void countStep(){ + dir = digitalRead(DIR_PIN); + if (dir) target1++; + else target1--; +} diff --git a/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt b/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt new file mode 100644 index 0000000..c387a60 --- /dev/null +++ b/Moteur DC/ServoStrap/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt @@ -0,0 +1,20 @@ +####################################### +# Syntax Coloring Map For DigitalWriteFast +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +DigitalWriteFast KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +digitalWriteFast KEYWORD2 +digitalWriteFast2 KEYWORD2 +pinModeFast KEYWORD2 +pinModeFast2 KEYWORD2 +digitalReadFast KEYWORD2 +digitalReadFast2 KEYWORD2 diff --git a/Moteur DC/ServoStrap/Driver_printer/Driver_printer.ino b/Moteur DC/ServoStrap/Driver_printer/Driver_printer.ino new file mode 100644 index 0000000..81a82c1 --- /dev/null +++ b/Moteur DC/ServoStrap/Driver_printer/Driver_printer.ino @@ -0,0 +1,192 @@ +#include + +/* 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 + +*/ + +#define encoder0PinA 7 // PD2; +#define encoder0PinB 8 // PB0; + +#define SpeedPin 10 +#define DirectionPin 9 //PC1; + +//from ramps 1.4 stepper driver +#define STEP_PIN 3 //PD3; +#define DIR_PIN 5 //PC0; +//#define ENABLE_PIN 13 //PB5; for now is USELESS + +//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 + } + if(ms < 0){ + PORTC &=(B11111101); //digitalWriteFast2 ( DirectionPin , LOW ); write PC1 LOW + 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 PB0 + // because PD0is used for serial, i will change in the stable version TO USE PD2 + if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 + // 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 + // 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/Driver_printer/libraries/digitalWriteFast.h b/Moteur DC/ServoStrap/Driver_printer/libraries/digitalWriteFast.h new file mode 100644 index 0000000..b8c2618 --- /dev/null +++ b/Moteur DC/ServoStrap/Driver_printer/libraries/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 diff --git a/Moteur DC/ServoStrap/LICENSE b/Moteur DC/ServoStrap/LICENSE new file mode 100644 index 0000000..d7f1051 --- /dev/null +++ b/Moteur DC/ServoStrap/LICENSE @@ -0,0 +1,339 @@ +GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + {description} + Copyright (C) {year} {fullname} + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + {signature of Ty Coon}, 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/Moteur DC/ServoStrap/README.md b/Moteur DC/ServoStrap/README.md new file mode 100644 index 0000000..a365d27 --- /dev/null +++ b/Moteur DC/ServoStrap/README.md @@ -0,0 +1,46 @@ +ServoStrap +========== + +servo-controlled reprap 3D printer: + +this code take as input stepper information from a standard 3D printer motherboard +and use it to control a servo-motor with active position tracker. + + 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 to make an integrated board aesy to add to ramps 1.4 or other printer motherboard + + improvements: + + 1)moore faster movements on x-y axys, it mean less time to wait to print a part + + 2)less noise from the motors, it will be silent + + 3)the couple of the motor not decrease with the speed (like in a stepper motor) + + 4)active position tracking, no more step losses, + almost all prints will end in perfect condition because if something stop + the head it will return to the print position + + 5)less price to build a printer, almost all electronic woste (like 2D printers) + have inside dc motors with all needed to control it + + 6)resolution increased by fine setting PID costants and using angular encoder, doesn't matter if is slit disk or magnetic + + 7)potentially endstops are not needed because the timing strip have special code at the begin/end + that can be interpreted as endstop + + + To use the code you need first to put the two files called digitalWriteFast.h and Keywords.txt in a folder inside arduino/libraries + + + here the youtube link of the test with this code: http://goo.gl/gAia5y diff --git a/Moteur DC/ServoStrap/STM32_olimexino_stepper_motor_emulator b/Moteur DC/ServoStrap/STM32_olimexino_stepper_motor_emulator new file mode 100644 index 0000000..04abab1 --- /dev/null +++ b/Moteur DC/ServoStrap/STM32_olimexino_stepper_motor_emulator @@ -0,0 +1,175 @@ +/* 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 SerialUSB 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 + +*/ + +#define encoder0PinA 2 +#define encoder0PinB 4 + +#define SpeedPin 9 +#define DirectionPin 8 + +//from ramps 1.4 stepper driver +#define STEP_PIN 3 +#define DIR_PIN 12 +#define ENABLE_PIN 13 + + +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() { + + pinMode(encoder0PinA, INPUT); + pinMode(encoder0PinB, INPUT); + + pinMode(DirectionPin, OUTPUT); + pinMode(SpeedPin, OUTPUT); + + //ramps 1.4 motor control + pinMode(STEP_PIN, INPUT); + pinMode(DIR_PIN, INPUT); + + attachInterrupt(2, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2 + attachInterrupt(3, countStep, RISING); //on pin 3 + + SerialUSB.println("start"); // a personal quirk + +} + +void loop(){ + + while (SerialUSB.available() > 0) { + KP = SerialUSB.read(); + KD = SerialUSB.read(); + KI = SerialUSB.read(); + + + SerialUSB.println(KP); + SerialUSB.println(KD); + SerialUSB.println(KI); +} + + if(millis() - previousTarget > 500){ //enable this code only for test purposes + SerialUSB.print(encoder0Pos); + SerialUSB.print(','); + SerialUSB.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){ + digitalWrite ( DirectionPin ,HIGH ); + } + if(ms < 0){ + digitalWrite ( DirectionPin , LOW ); + ms = -1 * ms; + } + + int motorspeed = map(ms,0,amp,0,255); + if( motorspeed >= 255) motorspeed=255; + //analogWrite ( SpeedPin, (255 - motorSpeed) ); + analogWrite ( SpeedPin, motorspeed ); + //SerialUSB.print ( ms ); + //SerialUSB.print ( ',' ); + //SerialUSB.println ( motorspeed ); + } +} + +void doEncoderMotor0(){ + if (digitalRead(encoder0PinA) == HIGH) { // found a low-to-high on channel A + if (digitalRead(encoder0PinB) == LOW) { // check channel B to see which way + // encoder is turning + encoder0Pos = encoder0Pos - 1; // CCW + } + else { + encoder0Pos = encoder0Pos + 1; // CW + } + } + else // found a high-to-low on channel A + { + if (digitalRead(encoder0PinB) == LOW) { // check channel B to see which way + // encoder is turning + encoder0Pos = encoder0Pos + 1; // CW + } + else { + encoder0Pos = encoder0Pos - 1; // CCW + } + + } + +} + +void countStep(){ + dir = digitalRead(DIR_PIN); + if (dir) target1++; + else target1--; +} diff --git a/Moteur DC/ServoStrap/digitalWriteFast.h b/Moteur DC/ServoStrap/digitalWriteFast.h new file mode 100644 index 0000000..b8c2618 --- /dev/null +++ b/Moteur DC/ServoStrap/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 diff --git a/Moteur DC/ServoStrap/keywords.txt b/Moteur DC/ServoStrap/keywords.txt new file mode 100644 index 0000000..c387a60 --- /dev/null +++ b/Moteur DC/ServoStrap/keywords.txt @@ -0,0 +1,20 @@ +####################################### +# Syntax Coloring Map For DigitalWriteFast +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +DigitalWriteFast KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +digitalWriteFast KEYWORD2 +digitalWriteFast2 KEYWORD2 +pinModeFast KEYWORD2 +pinModeFast2 KEYWORD2 +digitalReadFast KEYWORD2 +digitalReadFast2 KEYWORD2