From 27dc04b1bae76d2456766f7a951627e2c8f298d3 Mon Sep 17 00:00:00 2001 From: Vincent M Date: Tue, 26 Jan 2016 22:04:10 +0100 Subject: [PATCH] Ajout code Arduino moteur DC --- Moteur DC/DC_servo/DC_1servo.7z | Bin 0 -> 5836 bytes Moteur DC/DC_servo/DC_1servo/DC_1servo.ino | 209 +++++++++++++++++++++ Moteur DC/DC_servo/DC_1servo/PID_v1.cpp | 186 ++++++++++++++++++ Moteur DC/DC_servo/DC_1servo/PID_v1.h | 80 ++++++++ 4 files changed, 475 insertions(+) create mode 100644 Moteur DC/DC_servo/DC_1servo.7z create mode 100644 Moteur DC/DC_servo/DC_1servo/DC_1servo.ino create mode 100644 Moteur DC/DC_servo/DC_1servo/PID_v1.cpp create mode 100644 Moteur DC/DC_servo/DC_1servo/PID_v1.h diff --git a/Moteur DC/DC_servo/DC_1servo.7z b/Moteur DC/DC_servo/DC_1servo.7z new file mode 100644 index 0000000000000000000000000000000000000000..73b4cea8bf5775b8070b63e15f3adc0877ac1dc7 GIT binary patch literal 5836 zcmV;-7BlHLdc3bE8~_7uz6X?v761SM0000Z0000000018w0Xe*5t>NH5R(_){lxaa zGgk>viD9i3Nn?pz<~#NY5Oc%~(NWT2v$JCX11Sw0-k>!yRnTI8D5z?^x-u6p=@QdU z2^fUPcghTH_alVKSUpYT^>lGXJflc+O^A8cCFS)BoCIbE^WW`opo-<>oCqr1MAm>m z+Z%t-0fXP09S(t(Xylt$41!TXB=vI{X&Ha}Kt1))EV_s*?cbqwv?1sg4SiDN{yXHz zuzDs?D3q2;JtAYlbdAn0Oyjw1oFMS>8-zl%Qss|YM^S;I`DY%AhYNoUQvi29KE?h7 zq#W=XGJS6dv5R5oAv<&}c6Th5BgfFK)UJc-3&57+f0hHv3Kkv%Wp91BOe5Tlo$EhRdqxlswP1EheZ@qh<@~sXLkWuu1>nCtUNBI z%WvsuwV9;g$o#w!1=&x?Tvf)@LBuX(!TA3Wdg*qbb4D5xRA=BNdBY%F&V*pk$-X~ilu66an6z2HRQXSeA&?^l;UzOpwN3Yk@#Cl#ce zwB$Yb1Az?aU0mSEVNm(tF@b?E>UJYgzDhsl><)bV(Fw$C{8^zC`&2gPtZCh0sod;W zaBdcO_wvk~8u5GdtsaJgyaDi);KjPnQoYOusqFodJqWgb&~Cn?4gRp?>FIMZh<+X~ z_*A}?%_EUrqaNR_G8i>Na=jz{>6%oJSs4oKfkKdK(q$HX%?EgD?vC!yfhd(mNvRBp zD*93vVjB#fPr8aI<9nYvCc=(F5Cp7zC1a~AcqDuE3#ZyS4~d?bjAM=2o6jvuoX(&k z5L5q`%6S5X;f>iu(iPGTs@qw1RhZKtXn#x418FqCPhdvV8Mg+X6qjNBS5gQjv*i*e z;D~Dv+*!E|r&-$g&_U}dOeg>Tb;u14i5T63_ZE;-XnDd-RPPS^P4HHe5JFCDK$I~y zZN?{P?5WhO1lbg$lMLQA9_`oIS=e=hgde={Yq1L6bX}&1?5By>%^d|DI9hrU3JBad z)TBgwS4*2%B@Q(9$%KJNrxH#41Dq4C%;`OIK~^1uMF*-&D;iBkrmcXmapPy8`G~{M zM&STeqG|j?ov6oYZ)}6IV2zxF?9tPs-1Y;ym~>D(($r#=N*uQi(XJqf3A$B+Sky@$ zi;nf!9VO7A%;puqpk|FRk4(bHG$lOTm*IS>TFbCifKw~|(*y%eX{S&IDQp`%cb9tk zIC>bJn7?TY_=tnkvqv8nsP5<{=|SW|4F$V@hc5>pj2KMx1<5iyA-(adZeu%}1NlIt z>jM$gt+#76xk9{l{1gD%KE-RaA!F`{nf)d{YqXpiMmpuImVSZc=0#>_ay7h46y+48 zUW;BlqPMc|n9oU=kvfR&8;c>&K2+au4&_H48}7MfDvy@UZI5?`K5yCK4LTf2`k)8b z3u}WgG~jnF`{K7&Y#5&jgcRk-uxs8WN?MzUPd&llHbH_>Y$e#nm8}Qmj1N?`yNa_$ zE)gqs1!1(*-)21!j6-{otwSI}8rsv zx#Mt3uy1F6T>xo)=^FDuCzKPG(Z+(GIFDht{LJIvBiIyMiYT3Z&rV zy8)h}N)uL8^sC8bRAyiOOr!7}1sadKqow|d1j=MAg?jbHG~vweGwKK=@T*EBvs+3q zKmu84gmRNn4M|mpH5kBp^!F-cjEdEjAp6hA4-=%Gk>iew5GyF>Av()&{c@UzP%?$h zgiIcsQc}4&A(?x0E0@G|cKLh&A`tzHG5H-hxNA{E%%2-z(leqvx9~%XOAD&eO#CMu z64=V%w5;djOEC@QF4RttN7+d;AX0F#j3XPcO}FQK7@ZQZ&XTq+bXSQZPQiqf0;h8oXQJs{EMFIc{5b%HT z0igSdA#gJ50_Vv^Uz9zPA3B?4Gu1f#M0#mw5IWBBa3cim z*RdG)57)#41#0$u6Hvu@YEG<3|1|->4FXGhBwyAE?+}w`_cFcf6@WiWSRjI-v)2auPDN8!M@KfnHDo%czgn9{ih%AR?*Ehz2zbzR4f(s zs$9KKkx+I2@9b&Cs58M%aGlqX-lJ;)60O4D>at`q$5c0x(RmA3k6Im-9W}($KFNdxd!chfiNjLn9&UD*>=(f!qgE=RY8!14$l0ghnhWa!rIbRST z0wEDb6!saE1GSy*WHz5g6dXz$O+UkuZ;dE`BMN_Nh&4jEORP~V)#6Et`3u!{#c zGHM|)=*>SR@Z3qMNQ$DOM%o!;hRUmo~(5N*^Y0cFmV#=^9byETCadfcbTe8{yWGF{qqPMJ%=? z^B|{wxNfyQ6|N5qFliNv?sCs#Zrw617jDNSpazy)=a0lS8ZM{+ScT(wx9!M*N8D=GJ~7aSihxAPec}cKiBV9pwnqzBaQ5n z+=QzBj!jNo;e1Qx5%%*yUL6yCi1V2Fz`w#`o{i9xsKf|rXp5Avc|Q|{kfbTW;`)}q zSA*Gf_@0iGWg##C)7IE824aJRTMzPCrWRtlmZZoMGRN7e0gH$VXzG)3+Sc&E)LH9h zcd=AqIjd%>Z=$Txr!$9k8PwKbpGSdeKS$xsNxKFZ-qFXmCr}l+O?L$h6GjSEWGeVJ z_&za+&EcRyiSwNaoV7t&Bh5&(vt~#xJ1KYbTJl;7k_7$^>rc1jW3F*7W=hTZhQ%~G zQe!)0*FetFULYhX2mVb+k29`f@|ua~UB?K)%(7x%lVBW; zV-T7I#<7j7wlf8sg8XR!NLFAsp(XXxJI3|ROQ{-8WI!jF7ufFjJ%+ zOy68pkkCEsuvAA-SBl`4ubw=y$g2hdtU!|;EZSM!_bAqRPl?`eD`rQ(0QzwX1;dju z$RvJoSYfxRl8K=L4X*C(Z2RI5Y)90(SFp>IM*~OftF**50BmZl!Cn%=&hG;HFf)d8g~OW%I5>Q7YJwFr#N-rlz`ulpUgc zKeO!~kbXpKyK1fh+irQys>+{!vb|v*u9<(s_#>*RB0b}otNPXWRMn_wN_dBsq{Wfs zF~2?QizCR17GG-xd@Ba-CLFB|Pbx%k3bL;1?A>3g`XYNPxOb9=nz(?DxYM&3Zwszz zZ(;&CJ<*~31sgR1d8A6$Qs_%`Xf&NRKwJK)!&RAf5dr>S^(1=SqkahDci(W1r@&eU ztrpQJ*16zhaSg=PO&V2^HjvR1vse5rs(RZ)-}1Y)gU(l1G0*UCP}ABTgaFapCoB=Ze!?50<)p7(aTAVBz}Cf@a!;5s-qw}hVg zWoW*u#Sq%@D<^oJ`I&6PTjELAuKSc$21T)&E-iY&e(!>2{N#aWg`!f@v;S?X&C==$ z&%>KtA>N|Y&CTg^e-@kl)ChH+;nv5&5VE*)Z zN6QMQDg?+s&?z*M%H8WygC2PA-@Z?_dOlcw{FM4lR+FudUsurPj2N6L;jf&5+xube zXt>WYBF_WJR8ofy}palIg4_iHf>8>|*lwE!Kyt6Zzo zr~wa&P)1yEA*rYOQ%3lGYE|D*Fh+P5NI5A7XPA2?M+1GXXM3|A(#C#0$flY*_LCP% z6sxJ5;3!4VY=FalK@iMXK7QfF1LJCVz_?Qr;ilz&>+q&?eUUEfQ#I^3i}L^N`t5}F z23GSYcH$^{UF9@2Y{S!oED$%tO^mhQcg724yP6O)_`zsAKxl3>RT3Nd5KV=+uSkrm_f>#h!9NMih1 zKSQXS2VRU~r&KtS#;DA)QbQfYryP=nGE28JW)JNyY>hApy|nMF5DOq-3I`JhOxu>d zpGXIAVK%k_X$+!~vcrxc8j2MOZk`7#=$tEu`nXCrg3x9iDZ^1cWmv4mtQQw&A!PU0BjwG^;j(-#2l=PINBOjOIf;eo4_JrBS;PFlU+(1t*h??Boj zDKX}2GZ@yWh?{i+IKfO#bwwORq+*VpVLC7(qxc1fr(Tn9VJkf2N9X7exNDD;i8~ZB zN@;C>r|C&rG3`Fq2G?RI<3;{r^bxj2JaApsVSBA>Wja z7il)OEPwC9!2tjKxmM?eZRC{2GT=|xB=UIkSv*{pB3eC5VjVw#&L3D6w}51%l?0#Zl|Y0G&^v zmRA5g1?6e&m{JND^lcZY%K76?feSX*_#(!O16j1K!C}vHemv}oSE7@c%< z2zzhxV+Ol01SpvTKE3sWu|!x^CS&mtek}Xg?T&`Xg!Bx|e|SHD$>sJ@d&yw7!NeIo z1fr=ju)D-+S4yHs#HvftY5P}%!&`lS$D_Xb@YBw-=RcPQj5Zq$BpL<6s+W|RC!uvTOkXzE;hv@#nWBXE zmc+6ItJBuTN-vh(oMVuN*|SvY9Biji8|T@>rW-EPrY-eDN=d!W)?Fd4PQRo@9B>67 z1LHA!l^>G9yta~bK-{OtN$#^tPqCAq)!rO4 z!sM3)PtCaF)(n>%e95E#IA!+8e8Awt84#v`khiOoxBnT6PoJDE`^i`~YUlWMv?PRK zkUnRgHhtzK;Gk|P0jS+?rk7cU($X1H#3ssyrs@<&S)nJmj--|B?rW`IjdVHC9w}x@ z+eU;;g^G*o-bHQ+T%HOYeItRvrZZ#vVaq|eu(cBX&^y2Y7Y3E;0SSPg00#>J00AQd W0RaVF01yBG41o*^0rE +#include "PID_v1.h" +#define encoder0PinA 2 // PD2; +#define encoder0PinB 8 // PC0; +#define M1 9 +#define M2 10 // motor's PWM outputs + +byte pos[1000]; int p = 0; + +double kp = 3, ki = 0, kd = 0.0; +double input = 0, output = 0, setpoint = 0; +PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); +volatile long encoder0Pos = 0; +boolean auto1 = false, auto2 = false, counting = false; +long previousMillis = 0; // will store last time LED was updated + +long target1 = 0; // destination location at any moment + +//for motor control ramps 1.4 +bool newStep = false; +bool oldStep = false; +bool dir = false; +byte skip = 0; + +// Install Pin change interrupt for a pin, can be called multiple times +void pciSetup(byte pin) +{ + *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin + PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt + PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group +} + +void setup() { + pinMode(encoder0PinA, INPUT); + pinMode(encoder0PinB, INPUT); + digitalWrite(encoder0PinA, HIGH); + digitalWrite(encoder0PinB, HIGH); // active pullup ou pulldown + digitalWrite(3, HIGH); // active pullup sur entree steep + pciSetup(encoder0PinB); + attachInterrupt(0, encoderInt, CHANGE); // encoder pin on interrupt 0 - pin 2 + attachInterrupt(1, countStep , RISING); // step input on interrupt 1 - pin 3 + TCCR1B = TCCR1B & 0b11111000 | 1; // set 31Kh PWM + Serial.begin (115200); + help(); + recoverPIDfromEEPROM(); + //Setup the pid + myPID.SetMode(AUTOMATIC); + myPID.SetSampleTime(1); + myPID.SetOutputLimits(-255, 255); +} + +void loop() { + input = encoder0Pos; + setpoint = target1; + myPID.Compute(); + if (Serial.available()) process_line(); // it may induce a glitch to move motion, so use it sparingly + pwmOut(output); + if (auto1) if (millis() % 3000 == 0) { + target1 = random(50000); // that was for self test with no input from main controller + //Serial.println("----------------------------------"); + } + if (auto2) if (millis() % 1000 == 0) printPos(); + //if(counting && abs(input-target1)<15) counting=false; + if (counting && (skip++ % 5) == 0 ) { + pos[p] = encoder0Pos; + if (p < 999) p++; + else counting = false; + } +} +void pwmOut(int out) { + if (out < 0) { + analogWrite(M1, 0); + analogWrite(M2, abs(out)); + + + } + else { + analogWrite(M2, 0); + analogWrite(M1, abs(out)); + } +} + +const int QEM [16] = {0, -1, 1, 2, 1, 0, 2, -1, -1, 2, 0, 1, 2, 1, -1, 0}; // Quadrature Encoder Matrix +static unsigned char New, Old; +ISR (PCINT0_vect) { // handle pin change interrupt for D8 + Old = New; + New = (PINB & 1 ) + ((PIND & 4) >> 1); // + //Serial.print("."); + encoder0Pos += QEM [Old * 4 + New]; +} +void encoderInt() { // handle pin change interrupt for D2 + Old = New; + New = (PINB & 1 ) + ((PIND & 4) >> 1); // + encoder0Pos += QEM [Old * 4 + New]; + //Serial.print(":"); +} + + +void countStep() { + if (PINC & B0000001) target1--; // pin A0 represents direction + else target1++; +} + +void process_line() { + char cmd = Serial.read(); + if (cmd > 'Z') cmd -= 32; + switch (cmd) { + case 'P': kp = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'D': kd = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'I': ki = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case '?': printPos(); break; + case 'X': target1 = Serial.parseInt(); p = 0; counting = true; for (int i = 0; i < 300; i++) pos[i] = 0; break; + case 'T': auto1 = !auto1; break; + case 'A': auto2 = !auto2; break; + case 'Q': Serial.print("P="); Serial.print(kp); Serial.print(" I="); Serial.print(ki); Serial.print(" D="); Serial.println(kd); break; + case 'H': help(); break; + case 'W': writetoEEPROM(); break; + case 'K': eedump(); break; + case 'R': recoverPIDfromEEPROM() ; break; + case 'S': for (int i = 0; i < p; i++) Serial.println(pos[i]); break; + } + while (Serial.read() != 10); // dump extra characters till LF is seen (you can use CRLF or just LF) +} + +void printPos() { + Serial.print(F(" PID_output=")); Serial.print(output); + Serial.print(F(" Position->")); + Serial.print(encoder0Pos); Serial.print(F(":")); Serial.print(setpoint); + Serial.print(F("<-Target")); + Serial.print(F(" Diff: ")); + Serial.println(encoder0Pos - setpoint); +} +void help() { + Serial.println(F("\nPID DC motor controller and stepper interface emulator")); + Serial.println(F("by misan")); + Serial.println(F("Available serial commands: (lines end with CRLF or LF)")); + Serial.println(F("P123.34 sets proportional term to 123.34")); + Serial.println(F("I123.34 sets integral term to 123.34")); + Serial.println(F("D123.34 sets derivative term to 123.34")); + Serial.println(F("? prints out current encoder, output and setpoint values")); + Serial.println(F("X123 sets the target destination for the motor to 123 encoder pulses")); + Serial.println(F("T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that")); + Serial.println(F("Q will print out the current values of P, I and D parameters")); + Serial.println(F("W will store current values of P, I and D parameters into EEPROM")); + Serial.println(F("H will print this help message again")); + Serial.println(F("A will toggle on/off showing regulator status every second\n")); +} + +void writetoEEPROM() { // keep PID set values in EEPROM so they are kept when arduino goes off + eeput(kp, 0); + eeput(ki, 4); + eeput(kd, 8); + double cks = 0; + for (int i = 0; i < 12; i++) cks += EEPROM.read(i); + eeput(cks, 12); + Serial.println("\nPID values stored to EEPROM"); + //Serial.println(cks); +} + +void recoverPIDfromEEPROM() { + double cks = 0; + double cksEE; + for (int i = 0; i < 12; i++) cks += EEPROM.read(i); + cksEE = eeget(12); + //Serial.println(cks); + if (cks == cksEE) { + Serial.println(F("*** Found PID values on EEPROM")); + kp = eeget(0); + ki = eeget(4); + kd = eeget(8); + myPID.SetTunings(kp, ki, kd); + } + else Serial.println(F("*** Bad checksum")); +} + +void eeput(double value, int dir) { // Snow Leopard keeps me grounded to 1.0.6 Arduino, so I have to do this :-( + char * addr = (char * ) &value; + for (int i = dir; i < dir + 4; i++) EEPROM.write(i, addr[i - dir]); +} + +double eeget(int dir) { // Snow Leopard keeps me grounded to 1.0.6 Arduino, so I have to do this :-( + double value; + char * addr = (char * ) &value; + for (int i = dir; i < dir + 4; i++) addr[i - dir] = EEPROM.read(i); + return value; +} + +void eedump() { + for (int i = 0; i < 16; i++) { + Serial.print(EEPROM.read(i), HEX); + Serial.print(" "); + } Serial.println(); +} diff --git a/Moteur DC/DC_servo/DC_1servo/PID_v1.cpp b/Moteur DC/DC_servo/DC_1servo/PID_v1.cpp new file mode 100644 index 0000000..661537b --- /dev/null +++ b/Moteur DC/DC_servo/DC_1servo/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/DC_servo/DC_1servo/PID_v1.h b/Moteur DC/DC_servo/DC_1servo/PID_v1.h new file mode 100644 index 0000000..2b3874f --- /dev/null +++ b/Moteur DC/DC_servo/DC_1servo/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 +