abril 21, 2015

Servo 300 W – problemas arreglados

Written by

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.

Category : BRAZO ROBOT XL

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *

Proudly powered by WordPress and Sweet Tech Theme