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}
;
}