Simple asservissement polaire avec le robot MRduino2

Mise à jour : 10/02/2019
Un asservissement polaire permet de contrôler un robot mobile avec une consigne en distance et en orientation.
Voici un simple programme permettant de débuter en asservissement polaire avec un régulateur de type proportionnelle.
#include <mrduino2.h>

// coefficient PID distance
#define K_PID_R 15
#define K_PID_L 15

// coefficient PID orientation
#define KO_PID_R 15
#define KO_PID_L 15

// maximum speed
#define MAX_SPEED 40

int distanceRight, distanceLeft, distanceRobot, OrientationRobot;
int erreurDistance, erreurOrientation;
int cmdMoteurRight, cmdMoteurLeft;

// consigne en distance et orientation
int consigneDistance = 0;
int consigneOrientation = 400;

/*****************SETUP**************************************************************************/
/************************************************************************************************/
void setup()
{
  initRobot();

}

/*****************LOOP***************************************************************************/
/************************************************************************************************/
void loop()
{

  // lecture distance robot
  distanceRobot = read_distanceRobot();

  // lecture orientation du robot
  OrientationRobot = read_orientationRobot();

  // calcul erreur en distance
  erreurDistance = consigneDistance - distanceRobot;

  // calcul erreur en orientation
  erreurOrientation = consigneOrientation - OrientationRobot;

  // calcul des commandes
  cmdMoteurRight = erreurDistance*K_PID_R +          erreurOrientation*KO_PID_R;
cmdMoteurLeft = erreurDistance*K_PID_L - erreurOrientation*KO_PID_L;

// saturation de la commande
cmdMoteurRight = saturationCmd(cmdMoteurRight);
cmdMoteurLeft = saturationCmd(cmdMoteurLeft);

// gestion du sens de rotation des moteurs
if(cmdMoteurRight < 0 )
{
  motorRight(abs(cmdMoteurRight), 1);
}
else
{
  motorRight(abs(cmdMoteurRight), 0); 
}

// gestion du sens de rotation des moteurs
if(cmdMoteurLeft < 0 )
{
  motorLeft(abs(cmdMoteurLeft), 1);
}
else
{
  motorLeft(abs(cmdMoteurLeft), 0); 
}

// pour le debug
// Serial.println(distanceRobot);
// Serial.println(OrientationRobot);

}
/*******************************************************************************************************/
/*******************************************************************************************************/
/***********************PRIVATE FUNCTIONS **************************************************************/
/*******************************************************************************************************/
/*******************************************************************************************************/
/*******************************************************************************************************/
/*******************************************************************************************************/

// fonction de lecture de la distance du robot (unité = ticks)
int read_distanceRobot(void)
{
distanceLeft = -encoderLeft();
distanceRight = -encoderRight();

return((distanceRight + distanceLeft)/2);
}

// fonction de lecture de l'orientation du robot (unité = ticks)
int read_orientationRobot(void)
{

return(distanceRight - distanceLeft);
}

// fonction de saturation des commandes moteurs
int saturationCmd(int cmd)
{
if(cmd > MAX_SPEED)
{
cmd = MAX_SPEED;
}

if(cmd < -MAX_SPEED)
{
cmd = -MAX_SPEED;
}

return(cmd);
}

// end of file
Sources:
10 ans d’expérience