Étape 4: Let ' s go à ce poste vous petit bot !
Bienvenue dans la 4e étape. Pour faire cette étape, vous devez disposer de l’étape précédente fonctionne bien.Donc, vous avez un robot qui sait où il est, mais vous devez contrôler tous ses mouvements. Pas très bon !
Nous allons y remédier en contrôlant le robot uniquement avec les points de passage, qui est beaucoup plus amusant.
Pour ce faire, vous n’avez pas besoin d’autres mécanique ni électronique ! C’est purement logicielle;)
Nous allons voir le code (c’est un peu longue et non exhaustive au premier regard :
/*
* ----------------------------------------------------------------------------
* "La bière-WARE licence" (révision 42) :
* JBot a écrit ce fichier. Tant que vous conservez cette remarque que vous
* peut faire tout ce que vous voulez avec ce genre de choses. Si nous rencontrons un jour et que vous pensez
* ça en vaut la peine, vous pouvez m’acheter une bière en retour.
* ----------------------------------------------------------------------------
*/
Autres comprend
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >
/***********/
/ * Définit * /
/***********/
#define TICK_PER_MM_LEFT 9.2628378129 / / 90.9456817668
#define TICK_PER_MM_RIGHT 9.2628378129 / / 90.9456817668
#define diamètre 270.4 //275.0 / / 166,0 / / Distance entre les 2 roues
#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radians en degrés conversion * /
Types de moteur
#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 erreur 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
/***********************/
/ * Structures spécifiques * /
/***********************/
struct {moteur
type int ;
signé des_speed long ;
signé cur_speed long ;
long last_error ;
long error_sum ;
int kP ;
int kI ;
int kD ;
accel long signé ;
signé longue coupure ;
signé max_speed long ;
double distance ;
};
struct {de robot
double pos_X ;
double pos_Y ;
double theta ;
lacet de flotteur ;
hauteur du flotteur ;
rouleau de flotteur ;
float yaw_offset ;
};
struct RobotCommand {}
État de char ;
double current_distance ;
double desired_distance ;
};
struct Point {}
int x ;
int y ;
};
/********************/
/ * Variables globales * /
/********************/
struct left_motor moteur ;
struct right_motor moteur ;
struct alpha_motor moteur ;
struct delta_motor moteur ;
struct robot maximus ;
struct RobotCommand bot_command_delta ;
struct RobotCommand prev_bot_command_delta ;
struct RobotCommand bot_command_alpha ;
volatils long left_cnt = 0 ;
volatils long right_cnt = 0 ;
int last_left = 0 ;
int last_right = 0 ;
int left_diff = 0 ;
int right_diff = 0 ;
double total_distance = 0.0 ;
/***********************/
/ * INTERRUPTION FONCTIONS * /
/***********************/
Routine de service externe 4 interrompre = > code PIN2
ISR(INT4_vect)
{
Si ((PINB & 0x10)! = 0) {}
Si ((PINE & 0x10)! = 0)
left_cnt--;
d’autre
left_cnt ++ ;
} else {}
Si ((PINE & 0x10) == 0)
left_cnt--;
d’autre
left_cnt ++ ;
}
}
Routine de service externe interrompre 5 = > broche 3
ISR(INT5_vect)
{
Si ((PINK & 0x80)! = 0) {}
Si ((PINE & 0x20)! = 0)
right_cnt ++ ;
d’autre
right_cnt--;
} else {}
Si ((PINE & 0x20) == 0)
right_cnt ++ ;
d’autre
right_cnt--;
}
}
Goupille changement 0-7 interrupt service routine = > 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--;
}
}
Broche modifier la routine d’interruption du service 16-23 = > PIN-ADC15
ISR(PCINT2_vect)
{
Si ((PINE & 0x20)! = 0) {}
Si ((PINK & 0x80)! = 0)
right_cnt--;
d’autre
right_cnt ++ ;
} else {}
Si ((PINK & 0x80) == 0)
right_cnt--;
d’autre
right_cnt ++ ;
}
}
Routine de service de minuteur 1 débordement interruption
ISR(TIMER1_OVF_vect)
{
SEI() ; activer les interruptions
get_Odometers() ;
do_motion_control() ;
move_motors(ALPHADELTA) ; Mise à jour de la vitesse du moteur
}
/*************************/
/ * INITIALISATION DU SYSTÈME * /
/*************************/
void setup()
{
Initialisation de Ports d’entrée/sortie
Une initialisation de port
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = en
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTA = 0 X 00 ;
DDRA = 0 X 00 ;
Initialisation du port B
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = Out
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTB = 0 X 00 ;
DDRB = 0 X 00 ;
Initialisation du port C
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = en
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTC = 0 X 00 ;
DDRC = 0 X 00 ;
Initialisation du port D
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = en
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTD = 0 X 00 ;
DDDR = 0 X 00 ;
Initialisation du port E
Func2 = à Func1 = en Func0 = en
State2 = T State1 = State0 T = T
PORTE = 0 X 00 ;
DDRE = 0 X 00 ;
PORTK = 0 X 00 ;
DDRK = 0 X 00 ;
pinMode (13, sortie) ;
Initialisation du timer/compteur 1
Source de l’horloge : horloge système
Valeur d’horloge : 62 500 kHz
Mode : Haut pH correct PWM = 00FFh
OC1A sortie : aban.
OC1B sortie : aban.
OC1C sortie : aban.
Canceler de bruit : arrêt
Capture d’entrée sur front descendant
Minuterie 1 débordement interrompre : sur
Interruption de la prise d’entrée : Off
Comparer une interruption du Match : Off
Comparer B interruption de Match : Off
Comparer l’interruption du Match C: 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 ;
Initialisation de Interrupt(s) externe
EICRA = 0 X 00 ;
EICRB = 0 X 05 ;
EIMSK = 0 X 30 ;
EIFR = 0 X 30 ;
Interrompre le P.C.int
PCICR = 0 X 05 ;
PCIFR = 0 X 05 ;
PCMSK0 = 0 X 10 ;
PCMSK1 = 0 X 00 ;
PCMSK2 = 0 X 80 ;
Minuteries / ou les compteurs s Interrupt(s) d’initialisation
TIMSK1 | = 0 X 01 ;
TIFR1 | = 0 X 01 ;
/******************************/
/ * Initialisation du code * /
/******************************/
init_motors() ; Moteurs d’init
init_Robot(&maximus) ; Statut de robot init
init_Command(&bot_command_delta) ; Commande de robot init
init_Command(&bot_command_alpha) ; Commande de robot init
init_Command(&prev_bot_command_delta) ;
Enable global interruptions
SEI() ;
Delay(10) ;
}
/******************/
/ * MAIN CODE BOUCLE * /
/******************/
void loop()
{
Placez votre code ici
Delay(20) ;
}
/****************************/
/ * FONCTIONS D’INITIALISATION * /
/****************************/
void init_Robot (struct robot * my_robot)
{
my_robot -> pos_X = 0 ;
my_robot -> pos_Y = 0 ;
my_robot -> thêta = 0 ;
my_robot -> lacet = 0.0 ;
my_robot -> terrain = 0.0 ;
my_robot -> Pain = 0.0 ;
my_robot -> yaw_offset = 0.0 ;
}
void init_Command (struct RobotCommand * cmd)
{
cmd -> Etat = COMMAND_DONE ;
cmd -> current_distance = 0 ;
cmd -> desired_distance = 0 ;
}
Sub init_motors(void)
{
/ * Initialisation moteur à gauche * /
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 ;
/ * Initialisation droite moteur * /
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 ;
/ * Initialisation moteur alpha * /
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 hommes ; //1100 ; //1200 ; 500
alpha_motor.MAX_SPEED = ALPHA_MAX_SPEED ; 7000 ; 8000
alpha_motor.distance = 0.0 ;
/ * Initialisation moteur 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 ;
}
/*******************************/
/ * FONCTION DE COMMANDE DE MOUVEMENT * /
/*******************************/
Sub do_motion_control(void)
{
Angle de PID
alpha_motor.des_speed = compute_position_PID (& bot_command_alpha, & alpha_motor) ;
Distance de PID
Si ((bot_command_alpha.state == WAITING_BEGIN) || (bot_command_alpha.State == PROCESSING_COMMAND)) {/ / Si alpha moteur n’ont pas fini de son mouvement
} 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, longue distance)
{
cmd -> Etat = WAITING_BEGIN ;
cmd -> current_distance = 0 ;
cmd -> desired_distance = distance ;
}
compute_position_PID long (struct RobotCommand * cmd, struct moteur * used_motor)
{
longue P, I, D ;
long errDif, err ;
long tmp = 0 ;
Si (cmd -> État == WAITING_BEGIN) {}
cmd -> Etat = PROCESSING_COMMAND ;
}
Si (used_motor -> type == ALPHA_MOTOR)
ERR = cmd -> desired_distance * 10 - cmd -> current_distance * 10 * RAD2DEG ;
d’autre
ERR = cmd -> desired_distance - cmd -> current_distance ;
used_motor -> error_sum += err ; Somme d’erreur
Si (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 ; Calculer la variation de l’erreur
used_motor -> last_error = err ;
P = err * used_motor -> kP ; Proportionnelle
J’ai = used_motor -> error_sum * used_motor -> kI ; Integral
D = errDif * used_motor -> kD ; Dérivé
tmp = (P + I + D) ;
Si (abs(tmp) < abs (used_motor -> des_speed)) {/ / décélération
Si (tmp > (used_motor -> des_speed + used_motor -> coupure))
tmp = (used_motor -> des_speed + used_motor -> coupure) ;
ElseIf (tmp < (used_motor -> des_speed - used_motor -> coupure))
tmp = (used_motor -> des_speed - used_motor -> coupure) ;
} else {/ / accélération
Si (tmp > (used_motor -> des_speed + used_motor -> accel))
tmp = (used_motor -> des_speed + used_motor -> accel) ;
ElseIf (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 -> type == ALPHA_MOTOR) {}
Si ((cmd -> État == PROCESSING_COMMAND) & & (abs(err) < 3)
& & (abs(errDif) < 3)) {/ / 2 avant
cmd -> Etat = COMMAND_DONE ;
}
} else {}
Si ((cmd -> État == PROCESSING_COMMAND) & & (abs(err) < 6)
& & (abs(errDif) < 5)) {/ / 2 avant
cmd -> Etat = COMMAND_DONE ;
}
}
retour de tmp ;
}
Calculer la distance à faire pour aller à (x, y)
distance_coord double (struct robot * my_robot, double x1, double y1)
{
double x = 0 ;
x = sqrt (pow (fabs (x1-my_robot -> pos_X), 2) + pow (fabs (y1 - my_robot -> pos_Y), 2)) ;
Return x ;
}
Calculer l’angle à faire pour aller à (x, y)
angle_coord double (struct robot * my_robot, double x1, double y1)
{
double 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 ;
Return angletodo ;
}
void goto_xy (double x, double y)
{
double ang, dist ;
ang = angle_coord (& maximus, x, y) * RAD2DEG ;
set_new_command (& bot_command_alpha, ang) ;
Dist = distance_coord (& maximus, x, y) ;
set_new_command (& prev_bot_command_delta, dist) ;
bot_command_delta.State = WAITING_BEGIN ;
}
void goto_xy_back (double x, double y)
{
double ang, dist ;
ang = angle_coord (& maximus, x, y) ;
Si (ang < 0)
ang = (ang + PI) * RAD2DEG ;
d’autre
ang = (ang - PI) * RAD2DEG ;
set_new_command (& bot_command_alpha, ang) ;
Dist = - distance_coord (& maximus, x, y) ;
set_new_command (& prev_bot_command_delta, dist) ;
bot_command_delta.State = WAITING_BEGIN ;
}
/********************/
/ * FONCTIONS MOTEURS * /
/********************/
Sub move_motors(char type)
{
Si (type == ALPHADELTA) {}
write_RoboClaw_speed_M1M2 (128, delta_motor.des_speed - alpha_motor.des_speed, delta_motor.des_speed + alpha_motor.des_speed) ;
METTEZ VOTRE CODE DE MOTEUR DE VOITURE ICI
}
else {}
write_RoboClaw_speed_M1M2 (128, left_motor.des_speed, right_motor.des_speed) ;
METTEZ VOTRE CODE DE MOTEUR DE VOITURE ICI
}
}
void update_motor (struct moteur * used_motor)
{
Switch (used_motor -> type) {}
affaire LEFT_MOTOR :
used_motor -> cur_speed = left_diff ;
rupture ;
affaire RIGHT_MOTOR :
used_motor -> cur_speed = right_diff ;
rupture ;
affaire ALPHA_MOTOR :
used_motor -> cur_speed = left_diff - right_diff ;
rupture ;
affaire DELTA_MOTOR :
used_motor -> cur_speed = (left_diff + right_diff) / 2 ;
rupture ;
par défaut :
rupture ;
}
}
/********************************/
/ * FONCTION D’ESTIMATION DE POSITION * /
/********************************/
/ * Calculer la position du robot * /
Sub get_Odometers(void)
{
long left_wheel = 0 ;
long right_wheel = 0 ;
double left_mm = 0.0 ;
double right_mm = 0.0 ;
double distance = 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 (double)) / TICK_PER_MM_LEFT ;
right_mm = (right_diff (double)) / TICK_PER_MM_RIGHT ;
distance = (left_mm + right_mm) / 2 ;
TOTAL_DISTANCE += distance ;
bot_command_delta.current_distance += distance ;
Maximus.Theta += (right_mm - left_mm) / diamètre ;
bot_command_alpha.current_distance += (right_mm - left_mm) / diamètre ;
Si (maximus.theta > PI)
Maximus.Theta-= TWOPI ;
Si (maximus.theta < (-PI))
Maximus.Theta += TWOPI ;
Maximus.pos_Y += distance * sin(maximus.theta) ;
Maximus.pos_X += distance * cos(maximus.theta) ;
update_motor(&left_motor) ;
update_motor(&right_motor) ;
update_motor(&alpha_motor) ;
update_motor(&delta_motor) ;
}
Alors, voyons voir ce qu’il fait (il n’est pas vraiment compréhensible au premier regard) :
-init_motors(void)
Il inits toutes les variables des moteurs tels que les constantes de PID, l’accélération et la vitesse max, nous voulons
-do_motion_control(void)
Cette fonction est très importante. Calculer l’angle et la valeur de la distance à l’aide de la fonction PID et mettez-le dans le moteur correspondant
-set_new_command (struct RobotCommand * cmd, longue distance)
Donner une nouvelle commande pour un moteur, par exemple pour le moteur de la distance (delta), on peut dire de lui pour aller d’un 100millimeters
-long compute_position_PID (struct RobotCommand * cmd, struct moteur * used_motor)
Cette fonction calcule le PID d’un moteur à l’aide de sa vitesse constante, l’accélération et max
-goto_xy (double x, double y) et goto_xy_back (double x, double y)
Ces fonctions servent à donner un point de passage au robot
Vous pouvez maintenant faire quelque chose comme ce (donnant le point de cheminement à votre robot et en le voyant y aller seul) :
