vmeril
/
3DPrinterMadeInPlateformeC
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Ajout code Arduino moteur DC

Browse Source
ConversionFreeCAD
Vincent MERIL 9 years ago
parent 34e15834f7
commit 27dc04b1ba
4 changed files with 475 additions and 0 deletions
Show Stats Download Patch File Download Diff File

BIN
Moteur DC/DC_servo/DC_1servo.7z
View File

Binary file not shown.

209
Moteur DC/DC_servo/DC_1servo/DC_1servo.ino
Unescape View File

@ -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 <EEPROM.h>
#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();
}

186
Moteur DC/DC_servo/DC_1servo/PID_v1.cpp
Unescape View File

@ -0,0 +1,186 @@
/**********************************************************************************************
* Arduino PID Library - Version 1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Code is licensed under a Creative Commons Attribution-ShareAlike 3.0 Unported License.
**********************************************************************************************/
#include <Arduino.h>
#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;}

80
Moteur DC/DC_servo/DC_1servo/PID_v1.h
Unescape View File

@ -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
Write Preview
Loading…
Cancel
Save