BR_XL-Servo 300W – prueba de servo

Tenemos el servo montado y vamos a probar el funcionamiento. Conectamos los cables del segundo Arduino, de comunicación I2C y cargamos el programa del servo, versión anterior. Ahora lo modificamos para este motor, para usar el controlador puente H que hemos fabricado nosotros. En el programa cambia la parte del código de activación del motor, por tanto, ya que este controlador usa 3 cables, en lugar de cuatro, como el antiguo.

El programa nos queda:

---

// Servo controller
// Usa controlador H-bridge 
// Control de motor PID en bucle loop
//
#include <Wire.h>
#include <PID_v1.h>

  // General definitions
#define dDirServo 4      // I2C Address for servo  
#define dDirMaestro 1    // I2C address for central
  
  // Motor definitions
#define A 9
#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 150
#define dMinVel 15       // Minimum motor speed 80

  // Encoder definitions
#define dMinEnc 10        // Minimum encoder position for reset 
#define dMaxEnc 1000      // Maximum encoder position
#define encoderPinA  2    // Pins for encoder data
#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;         // Estado de velocidad del motor
int estAntVelMotor;      // Estado anterior de velocidad del motor
                    
float vel;
boolean A_set = false;  // interrupt service routine vars            
boolean B_set = false;
double Setpoint, Input, Output; // Pid
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=3.1, consKi=0.3, 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("Preparado");
  
  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);

  Serial.begin(115200);
  
    // Inicializa 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);
    Serial.println(cComando);
   iiCad=0;
  }
  
    //····································
    // Calcula nuevo movimiento del motor
  CalculaMueveMotor();  
    
    //································
    // Comprueba si se mueve el motor
  if (estVelMotor!=estAntVelMotor)
  {
    // Comprueba si hay que pararlo
   if (estVelMotor==0)
   {
    analogWrite(A, 0);     // Stops
    analogWrite(B, 0);
   }
   else
   { 
      // Comprueba nuevo sentido de giro
    if (estVelMotor>0)
    {
     analogWrite(B, 0);
     analogWrite(A, estVelMotor);
    }
    else
    {
     analogWrite(A, 0);
     analogWrite(B, -estVelMotor);
    }
   }
   estAntVelMotor=estVelMotor;
  }
    
  delay(100);
  ii++;
  if(ii>=20) 
  {
    Serial.print(PosServo);
    Serial.print(" - ");
    Serial.print(lPosEnc);
    Serial.print(" / ");
    Serial.print(estAntVelMotor);
    Serial.print(" - ");
    Serial.println(estVelMotor);
    ii=0;
    cTemp[0]=0;
  }  
  

 
delay(1000); 
}

//*******************************************************************************************
// Calcula los parámetros de movimiento del motor
//*******************************************************************************************
void CalculaMueveMotor(void)
{
    // Checks for movement direction
/* if (PosServo>lPosEnc)
 {
  estVelMotor=-dMinVel;
 } 
 else if (lPosEnc>PosServo)
 {
  estVelMotor=dMinVel;
 }
 else estVelMotor=0;
}
*/
  
  //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;
/*  Serial.print(Setpoint);
  Serial.print(" = ");
  Serial.print(Input);
  Serial.print(" = ");
  Serial.println(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
  Serial.println ("Inicio reset");
  analogWrite(B, 0);
  analogWrite(A, dMinVel+15);
  
    // Wait for switching end-travel
  do
  {
   EstadoFinca = digitalRead(FINCA);
   delay(10);
  } while (EstadoFinca == LOW);     
  analogWrite(A,0);     // Stops
  delay(500);
  Serial.println ("reset 1");

    // 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);
  Serial.println ("reset 2");

    // 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);
  Serial.println ("reset 3");
  
    // 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);
  Serial.println ("reset fin");
}

//*******************************************************************************************
// 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
//    Serial.println("RESET");
    reset();
    break;
    
  case 'M':     // Movement
    PosServo=atol(&(Cad[1]));
//    Serial.print("Mover a:");
//    Serial.println(lTemp);
    break;
    
  case 'P':    // Returns position
//    Serial.print("Posicion:");
//    Serial.println(lPosEnc);
    sprintf(cComando,"P%ld",lPosEnc);
    EnviaCad(dDirMaestro,cComando);
    break;

  default:
    Serial.println("Comando no valido");
    break;  
 } 
}

//*******************************************************************************************
// Send char string through I2C
//*******************************************************************************************
void EnviaCad(int Dir,char *Mensaje)
{
 Wire.beginTransmission(Dir);
 Wire.write(Mensaje);
 Wire.endTransmission();
}

 

---

Ahora lo ponemos a funcionar pero no nos funciona correctamente. El servo se desestabiliza de vez en cuando y oscila bastante alrededor de la posición de referencia: