Written by admin
Hemos puesto una consulta en el foro de Arduino, y la respuesta ha sido rápida. La verdad es que no creíamos que, con lo específico que era el problema, que hubiera tantas respuestas tan rápidamente.
Bueno, nos han dado una pista sobre el problema. Resulta que el código pone en el bucle principal una pausa de 100 ms y otra de 1000 ms, y el cálculo del PID , que mueve el motor, se hace en este bucle, con lo que el sistema del motor se vuelve inestable, porque con la pausa no le da tiempo a corregir la velocidad y la posición, así que oscila o se inestabiliza. El profesor nos dice que debía haberlo visto antges y darse cuenta del error. En este caso el bucle loop() debe ser lo más rápido posible.
Cambiamos el código y le quitamos las pausas. Le quitamos también las escrituras por el puertoserie, porque pueden ralentizar igualmente el bucle.El programa nos queda:
//
// Servo controller - high power
// Uses motor H-bridge HIP4081
// Motor control in loop
//
#include <Wire.h>
#include <PID_v1.h>
// General definitions
#define dDirServo 4 // I2C Address for servo
#define dDirMaestro 1 // I2C address for master
// Motor definitions
#define A 9 // H-bridge pins
#define B 11
#define ENABLE 10
#define FINCA 8 // Pin for end-travel
#define dMaxVel 255 // Maximum motor speed
#define dMedVel 150 // Medium motor speed
#define dMinVel 15 // Minimum motor speed
// Encoder definitions
#define dMinEnc 10 // Minimum encoder position for reset
#define dMaxEnc 1000 // Maximum encoder position
#define encoderPinA 2 // Pins for encoder data - interrupt
#define encoderPinB 3
volatile long lPosEnc = 0; // Encoder counter
unsigned int lastReportedPos = 1; // change management
static boolean rotating=false; // debounce management
int ii;
char cTemp[200];
long lTemp;
long PosServo; // Posición requerida del servo
char cComando[50]; // Char string for commands
int iiCad; // Counter for string
char ch;
int newposition;
int oldposition;
int newtime;
int oldtime;
int estVelMotor; // Motor speed
int estAntVelMotor; // Previous motor speed (in loop)
float vel;
boolean A_set = false; // interrupt service routine vars
boolean B_set = false;
double Setpoint, Input, Output; // Pid parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=2, consKi=0.1, consKd=0.1;
PID myPID(&Input, &Output, &Setpoint,consKp,consKi,consKd, DIRECT);
void setup()
{
pinMode(A, OUTPUT);
pinMode(B, OUTPUT);
pinMode(ENABLE, OUTPUT);
digitalWrite(A, LOW);
digitalWrite(B, LOW);
digitalWrite(ENABLE, LOW);
delay(500);
digitalWrite(ENABLE, HIGH);
Serial.begin(9600);
Serial.println("Ready");
Wire.begin(dDirServo); // Activate I2C with servo address
Wire.onReceive(receiveEvent); // register event
pinMode(A, OUTPUT); // Pins for motor direction and speed control
pinMode(B, OUTPUT);
pinMode(ENABLE, OUTPUT);
pinMode(FINCA, INPUT); // Pin for end-travel
pinMode(encoderPinA, INPUT);
pinMode(encoderPinB, INPUT);
// turn on pullup resistors
digitalWrite(encoderPinA, HIGH);
digitalWrite(encoderPinB, HIGH);
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, doEncoderB, CHANGE);
// Initialize motor
estVelMotor=estAntVelMotor=0;
delay(500);
ii=0;
reset();
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-dMaxVel, dMaxVel);
}
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
void loop()
{
rotating = true; // Reset the debouncer
//·······························
// Checks for a received command
if (iiCad>0)
{
ProcesaComando(cComando);
iiCad=0;
}
//····································
// Calculates new motor movement
CalculaMueveMotor();
//································
// Checks if motor needs to move
if (estVelMotor!=estAntVelMotor)
{
// Checks for a stop
if (estVelMotor==0)
{
analogWrite(A, 0); // Stops
analogWrite(B, 0);
}
else
{
// Checks new motor direction
if (estVelMotor>0)
{
analogWrite(B, 0); // Turns right
analogWrite(A, estVelMotor);
}
else
{
analogWrite(A, 0); // Turns left
analogWrite(B, -estVelMotor);
}
}
estAntVelMotor=estVelMotor;
}
//*******************************************************************************************
// Calculate parameters for motor movement
//*******************************************************************************************
void CalculaMueveMotor(void)
{
//initialize the variables we're linked to
Input = lPosEnc;
Setpoint = PosServo ;
myPID.Compute();
if (abs(Output)<dMinVel)
{
if (abs(Output)>dMinVel/2.5) Output=dMinVel*Output/abs(Output);
else Output=0;
}
estVelMotor=-Output;
}
//*******************************************************************************************
// Resets the servo in four steps
// a) Moves servo quickly till the end-travel
// b) Moves servo till releases end-travel
// c) Moves servo slowly till the end-travel again and resets encoder position
// d) Moves servo till encoder minimum
//*******************************************************************************************
void reset()
{
int ii;
int EstadoFinca=LOW; // End-travel state
// A) Moves servo quickly till the end-travel
analogWrite(B, 0);
analogWrite(A, dMinVel+10);
// Wait for switching end-travel
do
{
EstadoFinca = digitalRead(FINCA);
delay(10);
} while (EstadoFinca == LOW);
analogWrite(A,0); // Stops
delay(500);
// B) Moves servo till releases end-travel
analogWrite(A, 0);
analogWrite(B, dMinVel);
// Wait till releasing end-travel
for(ii=0;ii<200;ii++)
{
EstadoFinca = digitalRead(FINCA);
if (EstadoFinca == LOW) break;
delay(10);
}
delay(100);
analogWrite(B, 0); // Stops
delay(500);
// C) Moves servo slowly till the end-travel
analogWrite(B, 0);
analogWrite(A, dMinVel);
// Wait for switching end-travel
for(ii=0;ii<1000;ii++)
{
EstadoFinca = digitalRead(FINCA);
if (EstadoFinca == HIGH) break;
delay(10);
}
analogWrite(A, 0); // Stops
delay(500);
// D) Reset encoder position and moves servo to the minimum position, for security
analogWrite(A, 0);
analogWrite(B, dMinVel);
// Wait till releasing end-travel
for(ii=0;ii<50;ii++)
{
if (lPosEnc>=dMinEnc) break;
delay(10);
}
delay(100);
analogWrite(B, 0); // Stops
lPosEnc=0;
PosServo=0;
delay(500);
}
//*******************************************************************************************
// Encoder interrupt pin A, active in state change
//*******************************************************************************************
void doEncoderA()
{
// debounce
if ( rotating ) delay (1); // wait a little until the bouncing is done
// Test transition, did things really change?
if( digitalRead(encoderPinA) != A_set ) { // debounce once more
A_set = !A_set;
// adjust counter + if A leads B
if ( A_set && !B_set )
lPosEnc += 1;
rotating = false; // no more debouncing until loop() hits again
}
}
//*******************************************************************************************
// Encoder interrupt pin B, active in state change
//*******************************************************************************************
void doEncoderB()
{
if ( rotating ) delay (1);
if( digitalRead(encoderPinB) != B_set ) {
B_set = !B_set;
// adjust counter - 1 if B leads A
if( B_set && !A_set )
lPosEnc -= 1;
rotating = false;
}
}
//*******************************************************************************************
// I2C Interrupt
// Active when data from master is received,
//*******************************************************************************************
void receiveEvent(int howMany)
{
iiCad=0;
while(0 < Wire.available())
{
ch = Wire.read(); // receive byte as a character
cComando[iiCad]=ch;
iiCad++;
}
cComando[iiCad]=0;
}
//*******************************************************************************************
// Process received command
//*******************************************************************************************
void ProcesaComando(char *Cad)
{
char chCom;
chCom=Cad[0];
switch(chCom)
{
case 'R': // Reset
reset();
break;
case 'M': // Movement
PosServo=atol(&(Cad[1]));
break;
case 'P': // Returns position
sprintf(cComando,"P%ld",lPosEnc);
EnviaCad(dDirMaestro,cComando);
break;
default:
break;
}
}
//*******************************************************************************************
// Send char string through I2C
//*******************************************************************************************
void EnviaCad(int Dir,char *Mensaje)
{
Wire.beginTransmission(Dir);
Wire.write(Mensaje);
Wire.endTransmission();
}
Lo probamos inmediatamente y ¡Funciona! Aunque todavía no se parece a un servo estándar, el funcionamiento es alentador. Es rápido y no oscila nada. Suponemos que ajustando los parámetros del PID podemos mejorarlo.
Deja una respuesta