Il codice di prova e funzionante per metterlo alla prova è suffuciente un motore dc, un L293NE, un encoder in quadratura. Io ho usato una stampante dove nel carrello di stampa c'è un sensore ottico che tramite un timing strip (un nastro) realizzano un Encoder in quadratura da 150LPI.
Codice:
/*
Copyright 2010 Maurilio Pizzurro <maurotec@libero.it
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.
*/
/*
######################################################################################
# Programma per il controllo di posizione tramite motore dc, encoder in #
# quadratura da 150LPI con algortitmo PID. #
# --------------------------------------------------------------------------------- #
# Risoluzione encoder in quadratura 150LPI x 4 = 600LPI #
# Risoluzione in Inch 0.0016666 #
# Risoluzione in mm 0.0042333 #
# Spostamento ncount = (mm / 0.0042333) / 10. Unità millimetri #
# Spostamento ncount = (mill inch / 0.0016666) / 1000. Unità millesimi di pollice #
# Esempio: #
# ncount = (254mm / 0.0042333) / 10 = 6000.047244466491862 #
# ncount = (10.000mmInch / 0.0016666) / 1000 = 6000.240009600384015 #
######################################################################################
Pin impegnati:
PC6 1| |28 PC5 (AI 5)
(D 0) PD0 2| |27 PC4 (AI 4)
(D 1) PD1 3| |26 PC3 (AI 3)
-> (D 2) PD2 4| |25 PC2 (AI 2)
-> PWM+ (D 3) PD3 5| |24 PC1 (AI 1)
-> (D 4) PD4 6| |23 PC0 (AI 0)
VCC 7| |22 GND
GND 8| |21 AREF
PB6 9| |20 AVCC
PB7 10| |19 PB5 (D 13)
-> PWM+ (D 5) PD5 11| |18 PB4 (D 12)
PWM+ (D 6) PD6 12| |17 PB3 (D 11) PWM
(D 7) PD7 13| |16 PB2 (D 10) PWM
(D 8) PB0 14| |15 PB1 (D 9) PWM <-
*/
#include "pins_arduino.h"
#include <PID_Beta6.h>
// Contains EEPROM.read() and EEPROM.write()
#include <EEPROM.h>
// ID of the settings block
#define CONFIG_VERSION "V01"
// Tell it where to store your config data in EEPROM
#define CONFIG_START 32
// Example settings structure
struct StoreStruct {
// This is for mere detection if they are your settings
char version[4];
// The variables of your settings
float Kc;
float TauI;
float TauD;
float velocity;
} storage = {
CONFIG_VERSION,
// The default values
0.0, // Kc
0.0, // TauI
0.0, // TauD
5.0 // velocity
};
// **** Caricati da EEprom ****
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
// Parametri P I D, Kc, TauI, TauD
double Kc;
double TauI;
double TauD;
double velocity;
// ***** Caricati da EEprom *****
/* LoadAndSaveSettings
* Joghurt 2010
* Demonstrates how to load and save settings to the EEPROM
*/
void loadConfig() {
// To make sure there are settings, and they are YOURS!
// If nothing is found it will use the default settings.
if (EEPROM.read(CONFIG_START + 0) == CONFIG_VERSION[0] &&
EEPROM.read(CONFIG_START + 1) == CONFIG_VERSION[1] &&
EEPROM.read(CONFIG_START + 2) == CONFIG_VERSION[2])
{
for (unsigned int t=0; t<sizeof(storage); t++)
*((char*)&storage + t) = EEPROM.read(CONFIG_START + t);
}
}
void saveConfig() {
noInterrupts(); // disable IRQ
for (unsigned int t=0; t<sizeof(storage); t++)
EEPROM.write(CONFIG_START + t, *((char*)&storage + t));
interrupts(); // enable IRQ
}
// Software major and minor version
#define majorVersion 0
#define minorVersion 4
// version 0.3 introdotto il PID
// verione 0.4 ora il PID e la velocità sono impostabile da seriale
// Encoder Pin
#define encoder0PinA 4 //PIND 0b00010000 channel A
#define encoder0PinB 5 //PIND 0b00100000 channel B
volatile long encoder0Pos = 0; // posizione corrente
volatile unsigned int stateEncoder0PinA; // variabile di stato
volatile unsigned int stateEncoder0PinB; // variabile di stato
volatile unsigned int npulse = 0;
volatile unsigned int isrna = 0;
unsigned long startTime;
long period; // periodo
int position;
// modalità opertative
#define operative 0
#define settings 1
int mode; // mode = operative | settings
boolean mstop = false; // motor stop
boolean unsaved = false;
// routine interrupt
ISR(PCINT2_vect)
{
// Pin Arduino 4 (PD4 PIN6 PCINT20)
// se lo stato di encoder0PinA è cambiato, registra lo stato corrente
// e chiama encoder0ChA()
if ((PIND & 0b00010000) != stateEncoder0PinA) //if (digitalRead(encoder0PinA) != stateEncoder0PinA)
{
stateEncoder0PinA = (PIND & 0b00010000); //stateEncoder0PinA = digitalRead(encoder0PinA);
// misura il periodo della frequenza generata dall'encoder
isrna++; // contatore di eventi
if (isrna == 3)
{
period = micros() - startTime;
//velocity = 0.0211665 / (micros() - startTime) / 1000;
Input = 0.0211665 / (period / 1000000.0);
npulse = isrna;
isrna = 1;
}
if (isrna == 1) // salva il valore del timer in microsecondi
{
startTime = micros();
}
encoder0ChA();
}
else
{
// Pin Arduino 5 (PD5 PIN11 PCINT21)
// se lo stato di encoder0PinB è cambiato, registra lo stato corrente
// e chiama encoder0ChB()
//if ((PIND & 0b00100000) != stateEncoder0PinB)
//if (digitalRead(encoder0PinB) != stateEncoder0PinB)
stateEncoder0PinB = (PIND & 0b00100000); //stateEncoder0PinB = digitalRead(encoder0PinB);
encoder0ChB();
}
if (encoder0Pos == position) mstop = true;
}
void encoder0ChA()
{
if (stateEncoder0PinA) {
// check channel B to see which way encoder is turning
if (PIND & 0b00100000) //if (digitalRead(encoder0PinB) == HIGH)
{
encoder0Pos--; // CCW
}
else
{
encoder0Pos++; // CW
}
}
else
{
// check channel B to see which way encoder is turning
if (PIND & 0b00100000) //if (digitalRead(encoder0PinB) == HIGH)
{
encoder0Pos++; // CW
}
else
{
encoder0Pos--; // CCW
}
}
}
void encoder0ChB()
{
// check state changed on channel B
if (stateEncoder0PinB)
{
// check channel A to see which way encoder is turning
if (PIND & 0b00010000) //if (digitalRead(encoder0PinA) == HIGH)
{
encoder0Pos++; // CW
}
else
{
encoder0Pos--; // CCW
}
}
else
{
// check channel A to see which way encoder is turning
if (PIND & 0b00010000) //if (digitalRead(encoder0PinA) == HIGH)
{
encoder0Pos--; // CCW
}
else
{
encoder0Pos++; // CW
}
}
}
#define ledPin 13
#define pwmPin 9
#define motor_N 2
#define motor_P 3
////Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, Kc, TauI, TauD);
void setup()
{
// prescaler 8 credo?
TCCR1B |= (1<<CS11);
Serial.begin(115200);
Serial.flush();
pinMode(ledPin, OUTPUT);
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
pinMode(motor_N, OUTPUT);
pinMode(motor_P, OUTPUT);
pinMode(pwmPin, OUTPUT);
stateEncoder0PinA = (PIND & 0b00010000); //stateEncoder0PinA = digitalRead(encoder0PinA);
stateEncoder0PinB = (PIND & 0b00100000); //stateEncoder0PinB = digitalRead(encoder0PinB);
loadConfig(); // load data from EEprom
Kc = storage.Kc;
TauI = storage.TauI;
TauD = storage.TauD;
velocity = storage.velocity;
// set PID
myPID.SetInputLimits(0, 100);
myPID.SetOutputLimits(10, 255);
myPID.SetSampleTime(500);
myPID.SetTunings(Kc, TauI, TauD); // necessario in questo caso
myPID.SetMode(AUTO); //turn the PID on
// PIN CHANGED ENABLED
PCICR |= (1 << PCIE2); // Abilita l'iterrupt su PORTD
PCMSK2 |= (1 << PCINT20); // Abbilita il vettore PCINT20 PD4 ARDUINO(4)
PCMSK2 |= (1 << PCINT21); // Abbilita il vettore PCINT21 PD5 ARDUINO(5)
mode = operative;
}
int byteCommand; // byte ricevuti dalla seriale interpretati come comandi
#define onWards 1 // motore gira in senso orario
#define backWards -1 // motore gira in senso antiorario
#define motorOff 0 // motore spento
#define motorOn 1 // motore acceso
int startSlow; // partenza lenta
int goMotor = 0; // goMotore può essere onWard o backWards
int motorPower = motorOff; // motorPower può essere motorOff (default) o motorOn
double scaled;
char valueBuffer[9];
int target;
int action;
void operativeMode(int comm, char *value)
{
if (comm == 'g')
{
target = atoi(value);
if (target < encoder0Pos)
{
action = 'I';
position = target;
}
if (target > encoder0Pos)
{
action = 'A';
position = target;
}
}
if (action == 'I')
{
action = 0;
delay(2);
startSlow = 270;
Setpoint = velocity;
scaled = Setpoint / 67;
goMotor = backWards;
motorPower = motorOn;
isrna = 0; // azzera il contatore di interrupt del canale A
digitalWrite(motor_N, HIGH);
digitalWrite(motor_P, LOW);
}
if (action == 'A')
{
action = 0;
delay(2);
startSlow = position - 270;
Setpoint = velocity;
scaled = Setpoint / 67;
goMotor = onWards;
motorPower = motorOn;
isrna = 0; // azzera il contatore di interrupt del canale A
digitalWrite(motor_P, HIGH);
digitalWrite(motor_N, LOW);
}
if (comm == 'v') printVersion();
if (comm == 'S') mode = settings; // enter in setting mode
comm = 0;
value = "";
}
void settingsMode(int comm, char *value)
{
if (comm == 'v') printVersion(); // print software version
if (comm == 'x') // exit from settings mode return in operative mode
{
mode = operative;
myPID.SetTunings(Kc, TauI, TauD);
}
if (comm == 'P') // imposta il valore per il guadagno del PID
{
double old_kc = Kc;
Kc = atof(value);
if (Kc != old_kc) unsaved = true;
}
if (comm == 'I') // imposta il valore per l'ntegrale del PID
{
double old_TauI = TauI;
TauI = atof(value);
if (TauI != old_TauI) unsaved = true;
}
if (comm == 'D') // imposta il valore la derivata del PID
{
double old_TauD = TauD;
TauD = atof(value);
if (TauD != old_TauD) unsaved = true;
}
if (comm == 'V') // imposta la velocità
{
double old_velocity = velocity;
velocity = atof(value);
if (velocity != old_velocity) unsaved = true;
}
if (comm == 'Z')
{
Serial.println("");
Serial.println("Uman readable PID data dump:");
Serial.print("Kc = ");
Serial.println(Kc);
Serial.print("TauI = ");
Serial.println(TauI);
Serial.print("TauD = ");
Serial.println(TauD);
Serial.println("--------------");
Serial.print("Current position = ");
Serial.println(encoder0Pos);
Serial.print("Current setpoint = ");
Serial.println(Setpoint);
Serial.print("Velocity = ");
Serial.println(velocity);
if (unsaved)
{
Serial.println("** Unsaved **");
}
else
{
Serial.println("Saved Ok");
}
Serial.println("");
}
if (comm == 'z')
{
Serial.println("begin");
Serial.println(Kc);
Serial.println(TauI);
Serial.println(TauD);
Serial.println(encoder0Pos);
Serial.println(Setpoint);
Serial.println(velocity);
Serial.println("end");
}
if (comm == 's')
{
storage.Kc = Kc;
storage.TauI = TauI;
storage.TauD = TauD;
storage.velocity = velocity;
saveConfig();
myPID.SetTunings(Kc, TauI, TauD);
mode = operative;
unsaved = false;
}
comm = 0;
value = "";
}
void printVersion()
{
Serial.println("Software Major Versione");
Serial.println(majorVersion);
Serial.println("Software Minor Versione");
Serial.println(minorVersion);
}
int nByte;
int nByteSave;
boolean busyState = false;
void ready() // Invia 254 via seriale
{
delay(2);
busyState = false;
Serial.print(254); // ready state
}
void busy() // Invia 255 via seriale
{
delay(2);
busyState = true;
Serial.print(255); // busy state
}
// Loop infinito
void loop()
{
if (encoder0Pos == position)
{
digitalWrite(ledPin, HIGH);
}
else
{
digitalWrite(ledPin, LOW);
}
if (mstop) // il motore deve essere fermato
{
motorPower = motorOff;
digitalWrite(motor_P, LOW);
digitalWrite(motor_N, LOW);
analogWrite(pwmPin, 0);
mstop = false;
}
if (motorPower == motorOn) // Il motore è acceso
{
if (goMotor == onWards) // il motore deve girare in senso orario
{
if ((encoder0Pos > startSlow) && (npulse == 3))
{
npulse = 0;
Setpoint = Setpoint - scaled; // scala la velocità
}
//if (encoder0Pos < 200) i = i + 0.005;
myPID.Compute(); // computa il PID
analogWrite(pwmPin, int(Output)); // inposta la velocità calcolata dal PID
}
if (goMotor == backWards) // il motore deve girare in senso antiorario
{
if ((encoder0Pos < startSlow) && (npulse == 3))
{
npulse = 0;
Setpoint = Setpoint - scaled; // scala la velocità
}
myPID.Compute(); // computa il PID
analogWrite(pwmPin, int(Output)); // inposta la velocità calcolata dal PID
}
}
// se il micro non ha molto da fare nByte e sempre 1 anche se sono stati spediti più byte
// quindi nByte è indicatore di presenza dati nel buffer e non quantità
/*if ((motorPower == motorOff) && (busyState == true))
{
ready(); // ready state
}*/
nByte = Serial.available();
if (nByte > 0)
{
if (true) // ready state
//if (busyState == false) // ready state
{
//busy(); // busy state impegna il bus
delay(10); // attende 10ms per dare il tempo alla routine di interrupt seriale
// di accumulare caratteri, forse basta anche busy()
nByte = Serial.available(); // ora nByte ha valore quantitativo
if (nByte == 1)
{
byteCommand = Serial.read();
}
else if (nByte > 1)
{
byteCommand = Serial.read();
// Riempi il buffer di comando
for (int i = 0; i < 9; i++)
{
valueBuffer[i] = Serial.read();
}
}
Serial.flush();
if (mode == operative)
{
operativeMode(byteCommand, valueBuffer);
}
if (mode == settings)
{
settingsMode(byteCommand, valueBuffer);
//ready(); // ready state
}
}
else
{
if (motorPower == motorOn)
{
//busy(); // Il bus è impegnato ed sono arrivati altri dati ma non possono essere processati
// allora il bus risponde occupato
Serial.flush(); // pulisce il buffer
}
}
}
} // end loop