diff --git a/Moteur DC/DC_servo/DC_1servo.7z b/Moteur DC/DC_servo/DC_1servo.7z new file mode 100644 index 0000000..73b4cea Binary files /dev/null and b/Moteur DC/DC_servo/DC_1servo.7z differ diff --git a/Moteur DC/DC_servo/DC_1servo/DC_1servo.ino b/Moteur DC/DC_servo/DC_1servo/DC_1servo.ino new file mode 100644 index 0000000..7e44491 --- /dev/null +++ b/Moteur DC/DC_servo/DC_1servo/DC_1servo.ino @@ -0,0 +1,209 @@ +/* This one is not using any PinChangeInterrupt library */ + +/* + This program uses an Arduino for a closed-loop control of a DC-motor. + Motor motion is detected by a quadrature encoder. + Two inputs named STEP and DIR allow changing the target position. + Serial port prints current position and target position every second. + Serial input can be used to feed a new location for the servo (no CR LF). + + Pins used: + Digital inputs 2 & 8 are connected to the two encoder signals (AB). + Digital input 3 is the STEP input. + Analog input 0 is the DIR input. + Digital outputs 9 & 10 control the PWM outputs for the motor (I am using half L298 here). + Please note PID gains kp, ki, kd need to be tuned to each different setup. +*/ +#include +#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 +