A Path Based on an Arc of a Circle

Hello!

I have a very simple robotic project, and I need to implement a turn based on the arc of a circle. I'm working with Polulu's Zumo Robot platform, who have an accelerometer and gyroscope, but no encoder. I've already implemented turn codes to 180 degrees and 90 degrees, both with PID controller, but I have no idea how to make a 180 degree turn (move the robot in the opposite direction) based on the arc of the circle.

I know this is possible by making it move every X degrees, where the next angle will be the sum of the angles (angle + = X) until it reaches 180 degrees, since the tangent line of a circle of radius R has the same slope, i see some pure pursuit formulations, but I'm having trouble implementing this. Can anybody help me? I attached a code for a 25 degree turn, where after the counter reaches 600, it will return to the 0 degree position (position where it was originally placed at the beginning of the loop).

#include <Wire.h>
#include <ZumoMotors.h>
#include <LSM303.h>
#include <L3G.h>
#include <Pushbutton.h>

#define LED_PIN 13
#define TURN_BASE_SPEED       75 //Velocidade Base ao efetuar a Rotação


L3G gyro;
LSM303 compass;
Pushbutton button(ZUMO_BUTTON);
ZumoMotors motors;

// Variável da medição média adquirida pelo giroscópio no eixo Y durante a calibração
float gyroOffsetY;
float gyroOffsetX;
float gyroOffsetZ;
float angleY;


//Estimação que o Robô está perfeitamente horizontal
float angle = 0; //ângulo compasso

//Mesmo que o anterior mas para o acelerômetro
float aAngle = 0;

void setup()
{
  //Início da Porta Serial.
  Serial.begin(9600);
  // Início da Biblioteca Wire colocando o protocolo I2C bus
  Wire.begin();
  // Inicia o Giroscópio L3GD20H
  gyro.init();
  // 800 Hz para saída de dados,
  // Filtro passa-baixa em 100 Hz.
  gyro.writeReg(L3G::CTRL1, 0b11111111);
  // 2000 dps full scale.
  gyro.writeReg(L3G::CTRL4, 0b00100000);
  // Filtro High-pass disabilitado.
  gyro.writeReg(L3G::CTRL5, 0b00000000);
  // Inicia o acelerômetro LSM303D.
  compass.init();
  // 50 Hz para saída de dados.
  compass.writeReg(LSM303::CTRL1, 0x57);
  // 8 g full-scale
  compass.writeReg(LSM303::CTRL2, 0x18);

  Serial.print("Calibração do Giroscópio....");
  Serial.println();
  digitalWrite(LED_PIN, HIGH);
  delay(1000);
  
  // Calibração do Giroscópio.
  for (uint16_t i = 0; i < 1024; i++)
  {
    // Espera por um novo dado, depois efetua a leitura.
    while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
    gyro.read();

    // Adiciona a leitura dos eixos X, Y e Z.
    gyroOffsetX += gyro.g.x;
    gyroOffsetY += gyro.g.y;
    gyroOffsetZ += gyro.g.z;
  }
    gyroOffsetX /= 1024;
    gyroOffsetY /= 1024;
    gyroOffsetZ /= 1024;

    digitalWrite(LED_PIN, LOW);
    
    // Dá update no ângulo usando o giroscôpio..
    updateAngleGyro();

    // A cada 20 ms (50 Hz), faz a correção do ângulo
    // correto pelo acelerômetro e efetua o print.
    static uint8_t lastCorrectionTime = 0;
    uint8_t m = millis();
    if ((uint8_t)(m - lastCorrectionTime) >= 20)
    {
    lastCorrectionTime = m;
    correctAngleAccel();
    Serial.print("Ângulo: "); Serial.println(angle);
    Serial.println();
    Serial.print("aÂngulo: "); Serial.print(aAngle);
    Serial.println();
    }
  delay(2000);
  
}
//Variáveis Utilizadas no Loop
int counter = 0; //Contador utilizado para efetuar a rotina via 20 ms
float targetAngle = 0;
float targetAngle1 = 0;
float nextAngle = 0;
float error;
float error1;
float error2;
float error3;
float SPEED=0;
float SPEED1=0;


void loop()
{
   // Dá update no ângulo usando o giroscôpio sempre que possível.
  updateAngleGyro();

  // A cada 20 ms (50 Hz), faz a correção do ângulo utilizando o
  // o acelerômetro, dá print e aciona os motores
  static byte lastCorrectionTime = 0;
  byte m = millis();
  if ((byte)(m - lastCorrectionTime) >= 20)
  {
    lastCorrectionTime = m;
    correctAngleAccel();

    int32_t speed;
          
          //Início da Primeira Rotina
          //Início do Contador
          if(counter++<300){ 
          
          //Cálculo do erro para o ângulo 0
          error = angle-targetAngle;
          if(error<0.5 || abs(error)>-0.5){
          //Controle PD
          speed = error *5;
          //Limitador de Velocidade de no máximo 400 (SPEED + speed)
          speed = constrain(speed, -300, 300);
          //RAMPA DE VELOCIDADE
          
          if(SPEED<100){
            if(SPEED>=100){
              SPEED=100;
            }else{
              SPEED++;
            }
          }
          //Controlador em Linha + ou - speed
          motors.setSpeeds(SPEED+speed, SPEED-speed);
    
    }else{
              error = angle-targetAngle;
              speed = error*10;
              speed = constrain(speed, -300, 300);
              if(speed<300){
                if(speed>=300){
                  speed=300;
                }else{
                  speed++;
                }
              }
              motors.setSpeeds(0, TURN_BASE_SPEED+speed);  
          } 
    //Início da Segunda Rotina 
    }else if(counter>=301 && counter<600)
    {
      //Definição para o robô girar 180 graus
      targetAngle1 = 25;
      error1 = angle-targetAngle1;
        //Definição dos erros aceitáveis para a linha de VOLTA
        if(abs(error1)<0.5 || abs(error1)>-0.5){
                error2 = angle-targetAngle1;
                speed = error2 * 5;
                speed = constrain(speed, -300, 300);
                 //RAMPA DE VELOCIDADE
                if(SPEED1<100){
                  if(SPEED1>=100){
                    SPEED1=100;
                  }else{
                    SPEED1++;
                  }
                }               
                motors.setSpeeds(SPEED1+speed, SPEED1-speed);
                
         //Função de Rotação
         }else{
              error3 = angle-targetAngle1;
              speed = speed*10;
              speed = constrain(speed, -300, 300);
              if(speed<300){
                if(speed>=300){
                  speed=300;
                }else{
                  speed++;
                }
              }
              motors.setSpeeds(0, TURN_BASE_SPEED+speed);
         }
               }else{
                  //Reset do Contador e do ângulo Inicial e Velocidade
                  counter = 0;
                  targetAngle=0;
                  SPEED=0;
                  SPEED1=0;

                
              }
          }
  }
// Faz a Leitura do giroscópio para dar update na estimação do ângulo
void updateAngleGyro()
{
  // Descobre quanto se passou desde o último update.
  static uint16_t lastUpdate = 0;
  uint16_t m = micros();
  uint16_t dt = m - lastUpdate;
  lastUpdate = m;

  gyro.read();
  
  // Calcula quanto o ângulo mudou em degraus e adiciona
  // à estimação do ângulo atual do giroscópio. A sensitividade
  // é de 0.07 dps por digito.
  angle += ((float)gyro.g.z - gyroOffsetZ) * 70 * dt / 1000000000;
   if(angle>300);
     angle -= 360;
   if(angle<0);
     angle += 360;
  return angle;
}

// Faz a leitura do acelerômetro e usa para ajustar o ângulo.
void correctAngleAccel()
{
  compass.read();

   // Calcula o ângulo de acordo com o acelerômetro.
  aAngle = -atan2(compass.a.y, -compass.a.x) * 180 / M_PI;
    if(aAngle>300);
      aAngle -= 360;
    if(aAngle<0);
      aAngle += 360;
  return aAngle;


  // Calcula a magnitude do vetor de aceleração em unidades de g.
  LSM303::vector<float> const aInG = {
    (float)compass.a.x / 4096,
    (float)compass.a.y / 4096,
    (float)compass.a.z / 4096}
  ;
}

Algorithms to draw the path of a circular arc were worked out long ago, see for example Bresenham's circle algorithm. There is plenty of C code around for it.

Not sure if this will work well without an encoder, so consider adding encoders to the Zumo.

jremington:
Algorithms to draw the path of a circular arc were worked out long ago, see for example Bresenham's circle algorithm. There is plenty of C code around for it.

Not sure if this will work well without an encoder, so consider adding encoders to the Zumo.

Adding the encoder it's not a option, because the possible damage to the internal circuitry of the robot. But I'll take a look at the link provided, thanks for the help!

Yes, it is possible that you might damage the robot by adding encoders, but lots of people, including me, have done it. Adding Quadrature Encoder to the Zumo Chassis | MCU on Eclipse