Revision 257 branch/belin/Emb_App/programme_principal_etud.c
| programme_principal_etud.c | ||
|---|---|---|
| 120 | 120 |
//'I'/73/Ox49 : D?finition du nom du v?hicule. Doit d?buter par le caract?re '#' et entraine le chargement de la configuration de piste |
| 121 | 121 |
// correspondant au nom du v?hicule si le nom se termine par '*' |
| 122 | 122 |
|
| 123 |
|
|
| 124 |
unsigned short Distance; |
|
| 125 | 123 |
unsigned short pos_tourelle; |
| 124 |
unsigned short capt=0; |
|
| 125 |
unsigned short GEL=0; |
|
| 126 | 126 |
|
| 127 |
|
|
| 128 | 127 |
|
| 129 | 128 |
|
| 130 | 129 |
void asservissementTourelle(){
|
| ... | ... | |
| 148 | 147 |
retour=periph[ADDR('R')].val;
|
| 149 | 148 |
|
| 150 | 149 |
//Asservissement |
| 151 |
correction=5*(consigne-retour);
|
|
| 150 |
correction=4*(consigne-retour);
|
|
| 152 | 151 |
comm.data.id='T'; |
| 153 | 152 |
comm.data.rtr = 0; |
| 154 | 153 |
comm.data.val = correction; |
| 155 | 154 |
|
| 156 | 155 |
snd_dtq(CanTx,comm.msg); |
| 157 |
dly_tsk(100);
|
|
| 156 |
dly_tsk(10); |
|
| 158 | 157 |
} |
| 159 | 158 |
|
| 160 | 159 |
} |
| ... | ... | |
| 168 | 167 |
snd_dtq(CanTx,commMoteur.msg); |
| 169 | 168 |
} |
| 170 | 169 |
|
| 171 |
void CalculDistance(){
|
|
| 170 |
void position(){
|
|
| 172 | 171 |
CanFrame req; |
| 173 | 172 |
UINT flag; |
| 174 |
|
|
| 173 |
CanFrame comm; |
|
| 174 |
unsigned short correction,Distance,consigne; |
|
| 175 |
consigne=700; |
|
| 176 |
|
|
| 175 | 177 |
while(1){
|
| 176 |
|
|
| 178 |
req.data.id = 'U'; |
|
| 179 |
req.data.rtr = 1; |
|
| 180 |
snd_dtq(CanTx, req.msg); |
|
| 181 |
Distance=periph[ADDR('U')].val;
|
|
| 182 |
if(Distance>1500 || GEL==1){
|
|
| 183 |
comm.data.id='D'; |
|
| 184 |
comm.data.rtr = 0; |
|
| 185 |
comm.data.val = 0; |
|
| 186 |
dly_tsk(10); |
|
| 187 |
} |
|
| 188 |
else{
|
|
| 189 |
//Lecture de distance |
|
| 190 |
req.data.id = 'U'; |
|
| 191 |
req.data.rtr = 1; |
|
| 192 |
snd_dtq(CanTx, req.msg); |
|
| 193 |
Distance=periph[ADDR('U')].val;
|
|
| 194 |
//Asservissement |
|
| 195 |
correction=1*(Distance-consigne); |
|
| 196 |
comm.data.id='D'; |
|
| 197 |
comm.data.rtr = 0; |
|
| 198 |
comm.data.val = correction; |
|
| 177 | 199 |
|
| 178 |
req.data.id = 'U'; |
|
| 179 |
req.data.rtr = 1; |
|
| 180 |
periph[ADDR('R')].ev=0x01;
|
|
| 181 |
snd_dtq(CanTx, req.msg); |
|
| 182 |
wai_flg(ev_periph,0x01,TWF_ANDW, &flag); |
|
| 183 |
|
|
| 184 |
Distance=periph[ADDR('U')].val;
|
|
| 185 |
dly_tsk(100); |
|
| 186 |
} |
|
| 200 |
snd_dtq(CanTx,comm.msg); |
|
| 201 |
dly_tsk(10); |
|
| 202 |
} |
|
| 203 |
dly_tsk(10); |
|
| 204 |
} |
|
| 187 | 205 |
} |
| 188 | 206 |
|
| 189 |
void position(){
|
|
| 190 |
unsigned short correction; |
|
| 207 |
void capteur(){
|
|
| 208 |
CanFrame req; |
|
| 209 |
UINT flag; |
|
| 191 | 210 |
CanFrame comm; |
| 192 |
while(1){
|
|
| 193 |
correction=1*(720-Distance); |
|
| 194 |
comm.data.id='D'; |
|
| 195 |
comm.data.rtr = 0; |
|
| 196 |
comm.data.val = correction; |
|
| 197 |
snd_dtq(CanTx,comm.msg); |
|
| 198 |
dly_tsk(100); |
|
| 199 |
LED_R=1; |
|
| 211 |
while (1){
|
|
| 212 |
req.data.id = 'C'; |
|
| 213 |
req.data.rtr = 1; |
|
| 214 |
snd_dtq(CanTx, req.msg); |
|
| 215 |
capt=periph[ADDR('C')].val;
|
|
| 216 |
GEL=0; |
|
| 217 |
while(capt==0x7610){
|
|
| 218 |
GEL=1; |
|
| 219 |
req.data.id = 'C'; |
|
| 220 |
req.data.rtr = 1; |
|
| 221 |
snd_dtq(CanTx, req.msg); |
|
| 222 |
capt=periph[ADDR('C')].val;
|
|
| 223 |
commMoteur(45); |
|
| 224 |
comm.data.id='D'; |
|
| 225 |
comm.data.rtr = 0; |
|
| 226 |
comm.data.val = 0; |
|
| 227 |
snd_dtq(CanTx,comm.msg); |
|
| 228 |
dly_tsk(100); |
|
| 229 |
} |
|
| 230 |
dly_tsk(10); |
|
| 200 | 231 |
} |
| 201 | 232 |
} |
| 202 | 233 |
|
| 203 |
|
|
| 234 |
|
|
| 235 |
|
|
| 204 | 236 |
void main() |
| 205 | 237 |
{
|
| 206 | 238 |
ports_mcu(); |
| 207 | 239 |
lcd_init(); |
| 208 | 240 |
periph_init(); |
| 209 |
periph_nom("Matthias");
|
|
| 241 |
periph_nom("#BelinMob*");
|
|
| 210 | 242 |
can_init(); |
| 211 | 243 |
clavier_init(1); |
| 212 | 244 |
capture_init(); |
| ... | ... | |
| 214 | 246 |
|
| 215 | 247 |
|
| 216 | 248 |
sta_tsk(ID_periph_rx); |
| 217 |
commMoteur(5); |
|
| 218 | 249 |
sta_tsk(ID_tourelle); |
| 219 |
sta_tsk(ID_CalculDistance); |
|
| 220 | 250 |
sta_tsk(ID_position); |
| 221 |
|
|
| 251 |
sta_tsk(ID_capteur); |
|
| 252 |
dly_tsk(1500); |
|
| 253 |
commMoteur(30); |
|
| 222 | 254 |
|
| 223 | 255 |
while(1) |
| 224 | 256 |
{
|
Also available in: Unified diff