Estimación de la posición de un robot con ruedas con arduino. (4 / 5 paso)

Paso 4: vamos a esta posición le bot poco!

Por lo tanto, tienes un robot que sabe donde está pero hay que controlar todos sus movimientos. No es muy buena!
Vamos a solucionarlo mediante el control de la robusteza solamente con waypoints, que es mucho más divertido.

Para ello, no necesitas otra mecánica ni electrónica! Es puro software ;)

Vamos a ver el código (es un poco largo y no integral a la primera vista:

/*
* ----------------------------------------------------------------------------
* "La cerveza vajilla de licencia" (revisión 42):
* JBot escribió este archivo. Mientras conserve este aviso
* puede hacer lo que quieras con este material. Si nos encontramos algún día, y piensas
* Este material vale la pena, usted me puede comprar una cerveza a cambio.
* ----------------------------------------------------------------------------
*/

Otras incluyen
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >

/***********/
/ * Define * /
/***********/
#define TICK_PER_MM_LEFT 9.2628378129 / / 90.9456817668
#define TICK_PER_MM_RIGHT 9.2628378129 / / 90.9456817668
#define 270.4 diámetro //275.0 / / 166.0 / distancia entre las 2 ruedas

#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radianes a grados de conversión * /

Tipos de motor
#define ALPHA_MOTOR 0
#define DELTA_MOTOR 1
#define LEFT_MOTOR 2
#define RIGHT_MOTOR 3

#define ALPHADELTA 0
#define LEFTRIGHT 1

#define COMMAND_DONE 0
#define PROCESSING_COMMAND 1
#define WAITING_BEGIN 2
#define ERROR 3

#define ALPHA_MAX_SPEED 25000 //24000 //25000//13000 / / 9000
#define ALPHA_MAX_ACCEL 300
#define ALPHA_MAX_DECEL 3500 //2500
#define DELTA_MAX_SPEED //45000 45000 //50000//37000
#define DELTA_MAX_SPEED_BACK //45000 35000 //50000//37000
#define DELTA_MAX_SPEED_BACK_PAWN 45000
#define DELTA_MAX_ACCEL 1000 //900 //600
#define DELTA_MAX_DECEL //4000 10000 //1800

/***********************/
/ * Estructuras específicas * /
/***********************/
struct {motor
int tipo;
firmado des_speed largo;
firmado cur_speed largo;
last_error largo;
largo error_sum;
int kP;
int kI;
int kD;
accel largo firmado;
firmado tiempo decel;
firmado max_speed largo;
doble distancia;
};

struct {} de robot
doble pos_X;
doble pos_Y;
doble theta;
desvío de flotador;
paso del flotador;
rodillo de flotador;
Float yaw_offset;
};

struct RobotCommand {}
Estado de Char;
doble current_distance;
doble desired_distance;
};

struct punto {}
int x;
int y;
};

/********************/
/ * Variables globales * /
/********************/
estructura motor left_motor;
estructura motor right_motor;
estructura motor alpha_motor;
estructura motor delta_motor;

struct robot maximus;

struct RobotCommand bot_command_delta;
struct RobotCommand prev_bot_command_delta;
struct RobotCommand bot_command_alpha;

volátil left_cnt largo = 0;
volátil right_cnt largo = 0;

int last_left = 0;
int last_right = 0;

int left_diff = 0;
int right_diff = 0;

doble total_distance = 0.0;

/***********************/
/ * FUNCIONES DE INTERRUPCIÓN * /
/***********************/

Rutina de servicio de interrupción 4 externo = > PIN2
ISR(INT4_vect)
{
Si ((PINB & 0x10)! = 0) {}
Si ((PINE & 0x10)! = 0)
left_cnt--;
otra cosa
left_cnt ++;
} else {}
Si ((PINE & 0x10) == 0)
left_cnt--;
otra cosa
left_cnt ++;
}

}

Rutina de servicio de interrupción 5 externo = > PIN3
ISR(INT5_vect)
{
Si ((PINK & 0x80)! = 0) {}
Si ((PINE & 0x20)! = 0)
right_cnt ++;
otra cosa
right_cnt--;
} else {}
Si ((PINE & 0x20) == 0)
right_cnt ++;
otra cosa
right_cnt--;
}

}

PIN cambio 0-7 interrupción servicio rutina = > PIN10
ISR(PCINT0_vect)
{
Si ((PINE & 0x10)! = 0) {}
Si ((PINB & 0x10)! = 0) {}
left_cnt ++;
} else
left_cnt--;
} else {}
Si ((PINB & 0x10) == 0) {}
left_cnt ++;
} else
left_cnt--;
}

}

Cambio de perno de la rutina de servicio de interrupción de 16-23 = > PIN-ADC15
ISR(PCINT2_vect)
{
Si ((PINE & 0x20)! = 0) {}
Si ((PINK & 0x80)! = 0)
right_cnt--;
otra cosa
right_cnt ++;
} else {}
Si ((PINK & 0x80) == 0)
right_cnt--;
otra cosa
right_cnt ++;
}

}

Rutina de servicio de temporizador 1 desbordamiento interrupción
ISR(TIMER1_OVF_vect)
{
SEI(); habilitar interrupciones
get_Odometers();

do_motion_control();
move_motors(ALPHADELTA); Actualización de la velocidad del motor

}

/*************************/
/ * INICIALIZACIÓN DEL SISTEMA * /
/*************************/
void setup()
{

Inicialización de puertos de entrada/salida
Una inicialización del puerto
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = en
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTA = 0 X 00;
DDRA = 0 X 00;

Inicialización del puerto B
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = salida
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTB = 0 X 00;
DDRB = 0 X 00;

Inicialización del puerto C
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = en
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTC = 0 X 00;
DDR = 0 X 00;

Inicialización del puerto D
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = en
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTD = 0 X 00;
DDRD = 0 X 00;

Inicialización del puerto E
Func2 = en Func1 = de Func0 = en
State2 = T estado1 = State0 T = T
PORTE = 0 X 00;
DDRE = 0 X 00;

PORTK = 0 X 00;
DDRK = 0 X 00;

pinMode (13, salida);

Inicialización del temporizador/contador 1
Fuente de reloj: reloj del sistema
Valor del reloj: 62.500 kHz
Modo: PH correcto PWM parte superior = 00FFh
OC1A salida: desconectado.
OC1B salida: desconectado.
OC1C salida: desconectado.
Supresor de ruido: Off
Entrada captura en flanco descendente
Temporizador de interrupción 1 desbordamiento: en
Interrupción de la captura de entrada: Off
Comparar una interrupción del partido: Off
Compara B partido interrupción: Off
Comparar C partido interrupción: Off
TCCR1A = 0 X 01;
TCCR1B = 0 X 04;
TCNT1H = 0 X 00;
TCNT1L = 0 X 00;
ICR1H = 0 X 00;
ICR1L = 0 X 00;
OCR1AH = 0 X 00;
OCR1AL = 0 X 00;
OCR1BH = 0 X 00;
OCR1BL = 0 X 00;
OCR1CH = 0 X 00;
OCR1CL = 0 X 00;

Inicialización de Interrupt(s) externo
EICRA = 0 X 00;
EICRB = 0 X 05;
EIMSK = 0 X 30;
EIFR = 0 X 30;
Interrumpir el PCINT
PCICR = 0 X 05;
PCIFR = 0 X 05;
PCMSK0 = 0 X 10;
PCMSK1 = 0 X 00;
PCMSK2 = 0 X 80;

Temporizadores / contadores inicialización de Interrupt(s)
TIMSK1 | = 0 X 01;
TIFR1 | = 0 X 01;

/******************************/
/ * Inicialización del código * /
/******************************/
init_motors(); Inicio motores
init_Robot(&maximus); Estado de robot init

init_Command(&bot_command_delta); Comando de la robusteza de init
init_Command(&bot_command_alpha); Comando de la robusteza de init
init_Command(&prev_bot_command_delta);

Habilitación global de interrupciones
SEI();

Delay(10);

}

/******************/
/ * BUCLE DE CÓDIGO PRINCIPAL * /
/******************/
void loop()
{
Coloque el código aquí

Delay(20);

}

/****************************/
/ * FUNCIONES DE INICIALIZACIÓN * /
/****************************/
void init_Robot (struct robot * my_robot)
{
my_robot -> pos_X = 0;
my_robot -> pos_Y = 0;
my_robot -> theta = 0;
my_robot -> desvío = 0.0;
my_robot -> pitch = 0.0;
my_robot -> rollo = 0.0;
my_robot -> yaw_offset = 0.0;
}

void init_Command (struct RobotCommand * cmd)
{
cmd -> Estado = COMMAND_DONE;
cmd -> current_distance = 0;
cmd -> desired_distance = 0;
}

void init_motors(void)
{
/ * Inicialización del motor a la izquierda * /
left_motor.Type = LEFT_MOTOR;
left_motor.des_speed = 0;
left_motor.cur_speed = 0;
left_motor.Last_Error = 0;
left_motor.error_sum = 0;
left_motor.KP = 12;
left_motor.Ki = 6;
left_motor.KD = 1;
left_motor.Accel = 5;
left_motor.DECEL = 5;
left_motor.max_speed = 30;
left_motor.Distance = 0.0;

/ * Inicialización del motor derecho * /
right_motor.Type = RIGHT_MOTOR;
right_motor.des_speed = 0;
right_motor.cur_speed = 0;
right_motor.Last_Error = 0;
right_motor.error_sum = 0;
right_motor.KP = 12;
right_motor.Ki = 6;
right_motor.KD = 1;
right_motor.Accel = 5;
right_motor.DECEL = 5;
right_motor.max_speed = 30;
right_motor.Distance = 0.0;

/ * Inicialización motor alfa * /
alpha_motor.Type = ALPHA_MOTOR;
alpha_motor.des_speed = 0;
alpha_motor.cur_speed = 0;
alpha_motor.Last_Error = 0;
alpha_motor.error_sum = 0;
alpha_motor.KP = 230; 250 / / 350 / / 600
alpha_motor.Ki = 0;
alpha_motor.KD = 340; //180 300 / / 200
alpha_motor.Accel = ALPHA_MAX_ACCEL; 300; 350 / / 200; 300
alpha_motor.DECEL = ALPHA_MAX_DECEL; 1300; 1200; //1100; //1200; 500
alpha_motor.max_speed = ALPHA_MAX_SPEED; 7000; 8000
alpha_motor.Distance = 0.0;

/ * Inicialización motor delta * /
delta_motor.Type = DELTA_MOTOR;
delta_motor.des_speed = 0;
delta_motor.cur_speed = 0;
delta_motor.Last_Error = 0;
delta_motor.error_sum = 0;
delta_motor.KP = 600; 600
delta_motor.Ki = 0;
delta_motor.KD = 200; 100 * 1,09
delta_motor.Accel = DELTA_MAX_ACCEL; 600; 400; //500;
delta_motor.DECEL = DELTA_MAX_DECEL; 1800; 1350; //1100; //1200;
delta_motor.max_speed = DELTA_MAX_SPEED; 25000; //35000;
delta_motor.Distance = 0.0;
}

/*******************************/
/ * FUNCIONES DE CONTROL DE MOVIMIENTO * /
/*******************************/
void do_motion_control(void)
{

Ángulo de PID
alpha_motor.des_speed = compute_position_PID (& bot_command_alpha & alpha_motor);

Distancia de PID
Si ((bot_command_alpha.state == WAITING_BEGIN) || (bot_command_alpha.State == PROCESSING_COMMAND)) {/ / Si motor alfa no ha terminado su movimiento

} else {}
Si ((bot_command_delta.state! = PROCESSING_COMMAND) & & (prev_bot_command_delta.state == WAITING_BEGIN)) {}
prev_bot_command_delta.State = PROCESSING_COMMAND;
set_new_command (& bot_command_delta, prev_bot_command_delta.desired_distance);
}

}
delta_motor.des_speed = compute_position_PID (& bot_command_delta & delta_motor);

}

void set_new_command (struct RobotCommand * cmd, larga distancia)
{
cmd -> Estado = WAITING_BEGIN;
cmd -> current_distance = 0;
cmd -> desired_distance = distancia;
}

compute_position_PID largo (struct RobotCommand * cmd, struct motor * used_motor)
{
largo P, I, D;
largo errDif, err;
largo tmp = 0;

Si (cmd -> Estado == WAITING_BEGIN) {}
cmd -> Estado = PROCESSING_COMMAND;
}

Si (used_motor -> tipo == ALPHA_MOTOR)
ERR = cmd -> desired_distance * 10 - cmd -> current_distance * 10 * RAD2DEG;
otra cosa
ERR = cmd -> desired_distance - cmd -> current_distance;

used_motor -> error_sum += err; Suma de error
if (used_motor -> error_sum > 10)
used_motor -> error_sum = 10;
Si (used_motor -> error_sum < -10)
used_motor -> error_sum = -10;

errDif = err - used_motor -> last_error; Calcular la variación del error

used_motor -> last_error = err;

P = err * used_motor -> kP; Proporcional
I = used_motor -> error_sum * used_motor -> kI; Integral
D = errDif * used_motor -> kD; Derivado de la

tmp = (P + I + D);

Si (abs(tmp) < abs (used_motor -> des_speed)) {/ / desaceleración
Si (tmp > (used_motor -> des_speed + used_motor -> decel))
tmp = (used_motor -> des_speed + used_motor -> decel);
else if (tmp < (used_motor -> des_speed - used_motor -> decel))
tmp = (used_motor -> des_speed - used_motor -> decel);
} else {/ / aceleración
Si (tmp > (used_motor -> des_speed + used_motor -> accel))
tmp = (used_motor -> des_speed + used_motor -> accel);
else if (tmp < (used_motor -> des_speed - used_motor -> accel))
tmp = (used_motor -> des_speed - used_motor -> accel);
}

Si (tmp > (used_motor -> max_speed))
tmp = (used_motor -> max_speed);
Si (tmp <-(used_motor -> max_speed))
tmp =-(used_motor -> max_speed);

Si (used_motor -> tipo == ALPHA_MOTOR) {}
Si ((cmd -> Estado == PROCESSING_COMMAND) & & (abs(err) < 3)
& & (abs(errDif) < 3)) {/ / 2 antes de
cmd -> Estado = COMMAND_DONE;
}
} else {}
Si ((cmd -> Estado == PROCESSING_COMMAND) & & (abs(err) < 6)
& & (abs(errDif) < 5)) {/ / 2 antes de
cmd -> Estado = COMMAND_DONE;
}
}

volver tmp;
}

Calcular la distancia a ir a (x, y)
doble distance_coord (struct robot * my_robot, doble x1, doble y1)
{
doble x = 0;
x = sqrt (pow (fabs (x1-my_robot -> pos_X), 2) + pow (fabs (y1 - my_robot -> pos_Y), 2));
return x;
}

Calcular el ángulo que hacer para ir a (x, y)
doble angle_coord (struct robot * my_robot, doble x1, doble y1)
{
doble angletodo = 0;
Si ((x1 < my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {}
angletodo = -PI / 2 - atan (fabs ((x1-my_robot -> pos_X) / (y1 - my_robot -> pos_Y)));
} else if ((x1 > my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {}
angletodo = - atan (fabs ((y1 - my_robot -> pos_Y) / (x1-my_robot -> pos_X)));
} else if ((x1 > my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {}
angletodo = atan (fabs ((y1 - my_robot -> pos_Y) / (x1-my_robot -> pos_X)));
} else if ((x1 < my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {}
angletodo = PI / 2 + atan (fabs ((x1-my_robot -> pos_X) / (y1 - my_robot -> pos_Y)));
} else if ((x1 < my_robot -> pos_X) & & (y1 == my_robot -> pos_Y)) {/ /
angletodo = -PI;
} else if ((x1 > my_robot -> pos_X) & & (y1 == my_robot -> pos_Y)) {/ /
angletodo = 0;
} else if ((x1 == my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {/ /
angletodo = -PI / 2;
} else if ((x1 == my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {/ /
angletodo = PI / 2;
} else
angletodo = 0;

angletodo = angletodo - my_robot -> theta;

Si (angletodo > PI)
angletodo = angletodo - 2 * PI;
Si (angletodo < -PI)
angletodo = 2 * PI + angletodo;

volver angletodo;
}

void goto_xy (doble x y doble)
{
doble ang, dist;

Ang = angle_coord (& maximus, x, y) * RAD2DEG;
set_new_command (y bot_command_alpha, ang);

Dist = distance_coord (& maximus, x, y);
set_new_command (y prev_bot_command_delta, dist);
bot_command_delta.State = WAITING_BEGIN;

}

void goto_xy_back (doble x y doble)
{
doble ang, dist;

Ang = angle_coord (& maximus, x, y);
Si (ang < 0)
Ang = (ang + PI) * RAD2DEG;
otra cosa
Ang = (ang - PI) * RAD2DEG;
set_new_command (y bot_command_alpha, ang);

Dist = - distance_coord (& maximus, x, y);
set_new_command (y prev_bot_command_delta, dist);
bot_command_delta.State = WAITING_BEGIN;
}

/********************/
/ * FUNCIONES DE MOTORES * /
/********************/
void move_motors(char type)
{
Si (tipo == ALPHADELTA) {}
write_RoboClaw_speed_M1M2 (128, delta_motor.des_speed - alpha_motor.des_speed, delta_motor.des_speed + alpha_motor.des_speed);
PON TU CÓDIGO DE MOTOR DE COCHE AQUÍ
}
Else {}
write_RoboClaw_speed_M1M2 (128, left_motor.des_speed, right_motor.des_speed);
PON TU CÓDIGO DE MOTOR DE COCHE AQUÍ
}
}

void update_motor (struct motor * used_motor)
{
interruptor (used_motor -> tipo) {}
caso LEFT_MOTOR:
used_motor -> cur_speed = left_diff;
rotura;
caso RIGHT_MOTOR:
used_motor -> cur_speed = right_diff;
rotura;
caso ALPHA_MOTOR:
used_motor -> cur_speed = left_diff - right_diff;
rotura;
caso DELTA_MOTOR:
used_motor -> cur_speed = (left_diff + right_diff) / 2;
rotura;
por defecto:
rotura;
}
}

/********************************/
/ * FUNCIÓN DE ESTIMACIÓN DE LA POSICIÓN * /
/********************************/

/ * Calcular la posición del robot * /
void get_Odometers(void)
{
left_wheel largo = 0;
right_wheel largo = 0;

doble left_mm = 0.0;
doble right_mm = 0.0;

doble distancia = 0.0;

left_wheel = left_cnt;
right_wheel = right_cnt;

left_diff = last_left - left_wheel;
right_diff = last_right - right_wheel;

last_left = left_wheel;
last_right = right_wheel;

left_mm = (left_diff (doble)) / TICK_PER_MM_LEFT;
right_mm = (right_diff (doble)) / TICK_PER_MM_RIGHT;

distancia = (left_mm + right_mm) / 2;
total_distance += distancia;
bot_command_delta.current_distance += distancia;

Maximus.Theta += (right_mm - left_mm) / diámetro;
bot_command_alpha.current_distance += (right_mm - left_mm) / diámetro;

Si (maximus.theta > PI)
Maximus.Theta-= TWOPI;
Si (maximus.theta < (-PI))
Maximus.Theta += TWOPI;

Maximus.pos_Y += distancia * sin(maximus.theta);
Maximus.pos_X += distancia * cos(maximus.theta);

update_motor(&left_motor);
update_motor(&right_motor);
update_motor(&alpha_motor);
update_motor(&delta_motor);

}

Por lo tanto, vamos a ver lo que hace (no es muy comprensible a primera vista):
-init_motors(void)
en su todas las variables de los motores como las constantes del PID, la aceleración y la velocidad máxima que queremos

-do_motion_control(void)
Esta función es muy importante. Calcular el ángulo y el valor de distancia utilizando la función PID y ponerlo en el motor correspondiente

-set_new_command (struct RobotCommand * cmd, larga distancia)
Dar un nuevo comando a un motor, por ejemplo para el motor de la distancia (delta) podemos decir le para ir a 100millimeters

-largo compute_position_PID (struct RobotCommand * cmd, struct motor * used_motor)
Esta función calcula el PID de un motor usando su velocidad constante, aceleración y máximo

-goto_xy (doble x y doble) y goto_xy_back (doble x y doble)
Estas funciones se utilizan para dar un punto de referencia para el robot

Ahora se puede hacer algo como eso (dar punto a tu robot y verlo ir allí solo):