summaryrefslogtreecommitdiff
path: root/2003/n/pic/asserv/src/asserv.h
blob: 8fca6057250cb07792daf15ab969c602f8dda062 (plain)
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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
//---------------------------------------------------------------------
//---------------------------------------------------------------------
// D�finition des fichiers � inclure
//---------------------------------------------------------------------
//---------------------------------------------------------------------
   #include <18f452.h>		// D�finition des registres du PIC18
   #include <stdlib.h>		// D�finition de la librairie standard

// Pattes libres.
/*
40 rb7 (coupee)
39 rb6   : D3
38 rb5   : D2
37 rb4   : D1
36 rb3   : D0
35 rb2   : Clk busp
30 rd7   
29 rd6   : Capteur sharp
28 rd5   : Capteur doigts
27 rd4   : Capteur couleur
24 rc5
23 rc4
22 rd3   : Jack
21 rd2
18 rc3
10 re2
9  re1
8  re0
7  ra5
5  ra3
4  ra2
3  ra1

A1-5 B2-7 C4-5 D2-6 E0-2


*/

//---------------------------------------------------------------------
//---------------------------------------------------------------------
// General configuration
//---------------------------------------------------------------------
//---------------------------------------------------------------------
   #fuses HS,NOWDT,WDT1
   #use delay(clock=20000000)
   #use rs232(baud=9600,xmit=PIN_C6,parity=N,rcv=PIN_C7,bits=8)
//   #use standard_io(B)
//   #priority ext

//---------------------------------------------------------------------
//---------------------------------------------------------------------
// D�finition des constantes
//---------------------------------------------------------------------
//---------------------------------------------------------------------
   #define LG_INPBUF		50	// Longueur du buffeur de r�ception

   #define BOT			 '<'	// Caract�re de d�but de trame
   #define EOT			 '>'	// Caract�re de fin de trame
   #define SEP			 ';'	// Caract�re de s�paration de param�tres
   #define ECH			 '\\'	// Caract�re d'�chappement
   #define BCD			 '#'	// Caract�re indiquant un nombre cod� en BCD
   #define AV_FIX		 'F'	// Caract�re traduisant une avanc�e avec une distance fix�e
   #define AR_FIX		 'R'	// Caract�re traduisant un recul avec une distance fix�e
   #define DR_FIX		 'D'	// Caract�re traduisant une rotation � droite avec un angle fix�
   #define GA_FIX		 'G'	// Caract�re traduisant une rotation � gauche avec un angle fix�
   #define AV_LIB		 'f'	// Caract�re traduisant une avanc�e avec une distance libre
   #define AR_LIB		 'r'	// Caract�re traduisant un recul avec une distance libre
   #define DR_LIB		 'd'	// Caract�re traduisant une rotation � droite avec un angle libre
   #define GA_LIB		 'g'	// Caract�re traduisant une rotation � gauche avec un angle fix�
   #define STOP		 's'	// Caract�re demandant un arret du robot
   #define PSTOP		 'S'	// Caract�re demandant un arret du robot
   #define FIN		    'Y'	// Caract�re traduisant la fin du deplacement
   #define CALIB	    'C'	// Caract�re traduisant un calibrage
   #define RAZ        'Z'	// Caract�re traduisant un reglage de l'asservissement
   #define RCG        'a'
   #define RCD        'p'
   #define RSCG        'A'
   #define RSCD        'P'
   #define RM        'q'
   #define ERR          'u'
   #define PID          'y'
   #define ETA          'e'
   #define ETASUIV      'E'
   #define STR      'b'
   #define CAPTEUR      'c'
   #define REDEMARRE      'Y'
   #define REGD      'w'
   #define REGP      'x'
   #define PLUS      '+'
   #define MOINS      '-'
   #define READC        'h'
   #define SERVO        'H'

   #define ERREUR_ID		'?'	      // Caract�re indiquant une erreur dans la transmission
   #define ERREUR_TRAME		'1'	// Erreur dans la trame
   #define ERREUR_PARA		'2'	// Erreur dans les param�tres
   #define ERREUR_CMD		'3'	// Erreur dans les commandes

   #define KDISTANCE		11000	   // Valeur du coefficient pour transformer la distance
   #define KANG         23300	   // Valeur du coefficient pour transformer l'angle
   #define POS_DEF_FIX_C	10995750	// Position maximale pour faire le test de KP

   #define KVITESSE		1441896	   // Valeur du coefficient pour transformer la vitesse
   #define KACCEL       738250	      // Valeur du coefficient pour transformer l'acc�l�ration
   #define KFREQ        1441896	   // Valeur du coefficient pour transformer la vitesse de rotation
   #define KROTATION		738250       // Valeur du coefficient pour transformer l'acc�l�ration de rotation
   #define ACCEL_DEF_FIX_C 738250497	// Acc�l�ration par d�faut : 1m/s� soit 576 �/s�
   #define VLIM_DEF_FIX_C	1441895503	// Vitesse limite par d�faut si distance connue : 1m/s soit 576 �/s
   #define VLIM_DEF_LIB_C	144189550	// Vitesse limite par d�faut si distance non sp�cifi�e : 78mm/s et 45�/s

   #define KP_ASSERV 1     // Valeur du Kp pour un asservissement


//---------------------------------------------------------------------
//---------------------------------------------------------------------
// D�finitions de structure
//---------------------------------------------------------------------
//---------------------------------------------------------------------

   // Etat g�n�ral du d�placement
   struct {
   	unsigned infini   :1;	// Indique si le d�placement doit �tre fait en boucle
   	unsigned attente  :1;	// Indique si un d�placement est en attente d'ex�cution
   	unsigned stoppe   :1;	// Indique si un d�placement doit �tre arr�t�
      unsigned interr   :1;   // Masquage pour les interruptions
      unsigned calibr   :1;   // Calibration de la vitesse et de l'acceleration
      unsigned reset  :1;   // Calibration de la vitesse et de l'acceleration
   } etat, etatsuiv;

   struct moteur
   {
      unsigned phase  	 :1;	// Indique si dans la 1� ou 2� partie du d�placement
      unsigned neg_move  :1;	// Indique un mouvement dans le sens n�gatif
      unsigned neg_movesuiv  :1;	// Indique un mouvement dans le sens n�gatif
      unsigned encours   :1;	// Indique qu'un d�placement est en cours
      unsigned g   :1;	// Indique qu'un d�placement est en cours
      unsigned sens   :1;	// Indique qu'un d�placement est en cours
      unsigned long cptavt;
      unsigned long cptarr;		// Compteurs indiquant un pas fait en avant ou en arri�re
      unsigned long cptavt_p;
      unsigned long cptarr_p;		// Compteurs indiquant un pas fait en avant ou en arri�re
      unsigned int32 cptsat;		// Compte le nombre de p�riodes d'�chantillonnage pour lesquels Vlim est atteinte
      unsigned int32 vlim;
      unsigned int32 vlimsuiv;	// Vitesse limite pour le d�placement
      unsigned int32 acc;
      unsigned int32 accsuiv;	// Valeur de l'acc�l�ration pour le segment
      unsigned int32 pfin;
      unsigned int32 pfinsuiv;	// Position finale souhait�e
      signed int32 phase1dist;	// Distance que doit durer la phase1 (D/2)
      signed int32 vmes;		// Vitesse mesur�e
      signed int32 u;		// Erreur entre la consigne et la valeur mesur�e
      signed int32 ypid;		// Sauvegarde de la valeur en sortie du PID
      unsigned int32 pwm;		// Sauvegarde de la valeur en sortie du PID
      signed int32 vact;		// Vitesse command�e
      signed int32 pcom;		// Position command�e
      signed int32 pmes;		// Position mesur�e
      signed int32 tmp;
   };


//---------------------------------------------------------------------
//---------------------------------------------------------------------
// D�claration des variables
//---------------------------------------------------------------------
//---------------------------------------------------------------------
   char inpbuf[LG_INPBUF];                 // Buffer de commande pour la liaison avec un PC
   unsigned int idxbuf;                    // Index du buffer de commande
   unsigned long nb_int;                   // Indique le nombre d'interruption
   unsigned long vmcg, amcg;
   unsigned long vmcd, amcd;

   unsigned long kp;                       // Coefficients du PID pour l'asservissement
   unsigned int32 kdist;                    // Coefficient pour transformer la distance en nb d'impulsions
   unsigned int32 kvit;                     // Coefficient pour transformer la vitesse en nb d'impulsions
   unsigned int32 kacc;                     // Coefficient pour transformer l'acc�l�ration en nb d'impulsions
   unsigned int32 kangle;                   // Coefficient pour transformer l'angle en nb d'impulsions
   unsigned int32 kfre;                     // Coefficient pour transformer la vitesse de rotation en nb d'impulsions
   unsigned int32 krot;                     // Coefficient pour transformer l'acc�l�ration de rotation en nb d'impulsions
   unsigned long timer0Prime, timer1Prime; // Compteurde pas fait en avant et en arri�re

   int32 accel_def_fix, vlim_def_fix, vlim_def_lib, pos_def_fix;
   int wreg_sauv;

   struct moteur gauche, droit;

   unsigned char sauvegarde[256];
   unsigned char ptr;