Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
# include "main.h"
//modélise les deux moteur séparément ? -> deux K et tau distinc et donc 2 controller différent aussi
#define K 1
#define Tau 1
#define Kp 1
#define Ki 1
#define Kp_dist 1
#define Kp_angl 1
#define pwm_start 565
#define pwm_end 665
double e[2];
double e_pos[2];
void low_level(regulation *myregulation){
//printf("LOW level unit\n");
double timestep = 0.01; //a définir plus tard soit avec une clock soit un import time
e[0] = myregulation->myspeed->w_ref[0]-myregulation->myspeed->speed[0];
e[1] = myregulation->myspeed->w_ref[1]-myregulation->myspeed->speed[1];
//integral part
myregulation->myspeed->e_int[0] += e[0]*timestep;
myregulation->myspeed->e_int[1] += e[1]*timestep;
//REGULATEUR PI
myregulation->myspeed->wheel_command[0] = e[0]*Kp + myregulation->myspeed->e_int[0]*Ki;
myregulation->myspeed->wheel_command[1] = e[1]*Kp + myregulation->myspeed->e_int[1]*Ki;
limiter(myregulation);
convert_pwm(myregulation);
}
void middle_level(regulation *myregulation){
//printf("MIDDLE level unit\n");
/*
if (myregulation->myspeed->w_robot[1]==0.0){ //d abord annuler l erreur sur l angle
myregulation->myspeed->w_ref[0]=myregulation->myspeed->w_robot[0]; //vitesse linéaire => vitesse de chaque roue égale
myregulation->myspeed->w_ref[1]=-myregulation->myspeed->w_robot[0];
}else{
myregulation->myspeed->w_ref[0]=dist_roue_axe*myregulation->myspeed->w_robot[1];
myregulation->myspeed->w_ref[1]=dist_roue_axe*myregulation->myspeed->w_robot[1]; //positif ou négatif ?
}
*/
//%%%%tourner et rouler en meme temps//
myregulation->myspeed->w_ref[0]=(myregulation->myspeed->w_robot[0]-myregulation->myspeed->w_robot[1]*dist_roue_axe)/rayon_roue;
myregulation->myspeed->w_ref[1]=-(myregulation->myspeed->w_robot[0]+myregulation->myspeed->w_robot[1]*dist_roue_axe)/rayon_roue;
low_level(myregulation);
}
void high_level(regulation *myregulation){
//printf("HIGH level unit\n");
e_pos[0]=myregulation->mybeacon->beacon_data[0]; //ici dis_ref et angle_ref = 0 car on veut venir proche sur beacon
e_pos[1]=myregulation->mybeacon->beacon_data[1];
//REGULATEUR P
myregulation->myspeed->w_robot[0]=Kp_dist*e_pos[0];
myregulation->myspeed->w_robot[1]=Kp_angl*e_pos[1];
middle_level(myregulation);
}
void limiter(regulation *myregulation){
//printf("LIMITER unit\n");
int i;
for(i=0; i<2; i++){ //pour chaque roue on regarde si on est pas hors de la zone liénaire
if (myregulation->myspeed->wheel_command[i] > 100){
myregulation->myspeed->wheel_command[i] = 100;
}
else if (myregulation->myspeed->wheel_command[i] < -100){
myregulation->myspeed->wheel_command[i] = -100;
}
}
}
void convert_pwm(regulation *myregulation){
//printf("PWM CONVERTOR unit\n");
//pour chaque roue venir regarder dans quel sens on veut qu'elle aille et de la venir assigner
//la valeur 0 ou 1 selon la roue considéré !! à vérifier
if (myregulation->myspeed->wheel_command[0]>0){
myregulation->myspeed->wheel_direction[0]=0;
}
else if (myregulation->myspeed->wheel_command[0]<0){
myregulation->myspeed->wheel_direction[0]=1;
}
if (myregulation->myspeed->wheel_command[1]<0){
myregulation->myspeed->wheel_direction[1]=1;
}
else if (myregulation->myspeed->wheel_command[1]>0){
myregulation->myspeed->wheel_direction[1]=0;
}
//0 -> 565 ?
//100 -> 655?
myregulation->myspeed->wheel_command[0]=pwm_start + ((pwm_end-pwm_start)/100)*fabs(myregulation->myspeed->wheel_command[0]);
myregulation->myspeed->wheel_command[1]=pwm_start + ((pwm_end-pwm_start)/100)*fabs(myregulation->myspeed->wheel_command[1]);
}