Revision 238 branch/vilain_sebastien/Emb_App/programme_principal_etud.c
| programme_principal_etud.c | ||
|---|---|---|
| 124 | 124 |
void commande_angle_roues(short angle_roues); |
| 125 | 125 |
short lecture_angle(); |
| 126 | 126 |
short lecture_telemetre(); |
| 127 |
short lecture_capteur(); |
|
| 127 | 128 |
void commande_vitesse(short vitesse); |
| 128 |
void choix_circuit(int i);
|
|
| 129 |
void init_vitesse(short vitesse);
|
|
| 129 | 130 |
short consigne_angle = 450; |
| 130 |
short erreur; |
|
| 131 |
short vitesse = 120; |
|
| 132 |
short consigne_telemetre = 714; |
|
| 131 |
short consigne_telemetre = 705; |
|
| 133 | 132 |
FLGPTN flag; |
| 134 | 133 |
short dist; |
| 134 |
short angle_roues; |
|
| 135 |
int rampe = 0; |
|
| 136 |
short lecture; |
|
| 135 | 137 |
|
| 136 | 138 |
void main() |
| 137 | 139 |
{
|
| ... | ... | |
| 139 | 141 |
ports_mcu(); |
| 140 | 142 |
lcd_init(); |
| 141 | 143 |
periph_init(); |
| 142 |
periph_nom("#Test*");
|
|
| 144 |
periph_nom("#Seb*");
|
|
| 143 | 145 |
|
| 144 | 146 |
can_init(); |
| 145 | 147 |
clavier_init(1); |
| 146 | 148 |
capture_init(); |
| 147 | 149 |
|
| 148 | 150 |
periph[ADDR('R')].ev=0x01;
|
| 151 |
periph[ADDR('U')].ev=0x02;
|
|
| 152 |
periph[ADDR('C')].ev=0x03;
|
|
| 149 | 153 |
|
| 150 | 154 |
sta_cyc(ID_acqui); |
| 151 | 155 |
sta_tsk(ID_periph_rx); |
| 152 | 156 |
sta_tsk(ID_asserv0); |
| 153 | 157 |
sta_tsk(ID_asserv1); |
| 158 |
sta_tsk(ID_commande_capteur); |
|
| 154 | 159 |
|
| 155 |
commande_vitesse(35);
|
|
| 160 |
init_vitesse(45);
|
|
| 156 | 161 |
|
| 157 | 162 |
while(1) |
| 158 | 163 |
{
|
| ... | ... | |
| 180 | 185 |
|
| 181 | 186 |
short lecture_angle() |
| 182 | 187 |
{
|
| 183 |
CanFrame requete, reponse;
|
|
| 188 |
CanFrame requete; |
|
| 184 | 189 |
|
| 185 | 190 |
requete.data.id = 'R'; |
| 186 | 191 |
requete.data.rtr = 1; |
| ... | ... | |
| 198 | 203 |
{
|
| 199 | 204 |
erreur = K*(consigne_angle-lecture_angle()); |
| 200 | 205 |
commande_tourelle(erreur); |
| 201 |
dly_tsk(98);
|
|
| 206 |
dly_tsk(25);
|
|
| 202 | 207 |
} |
| 203 | 208 |
} |
| 204 | 209 |
|
| ... | ... | |
| 214 | 219 |
|
| 215 | 220 |
short lecture_telemetre() |
| 216 | 221 |
{
|
| 217 |
CanFrame requete, reponse;
|
|
| 222 |
CanFrame requete; |
|
| 218 | 223 |
|
| 219 | 224 |
requete.data.id = 'U'; |
| 220 | 225 |
requete.data.rtr = 1; |
| 221 | 226 |
snd_dtq(CanTx, requete.msg); |
| 222 |
wai_flg (ev_periph,0x01,TWF_ORW,&flag);
|
|
| 223 |
clr_flg(ev_periph,~0x01);
|
|
| 227 |
wai_flg (ev_periph,0x02,TWF_ORW,&flag);
|
|
| 228 |
clr_flg(ev_periph,~0x02);
|
|
| 224 | 229 |
return (periph[ADDR('U')].val);
|
| 225 | 230 |
} |
| 226 | 231 |
|
| 227 | 232 |
void asserv1() |
| 228 | 233 |
{
|
| 229 |
int K = 1;
|
|
| 234 |
int K = 2;
|
|
| 230 | 235 |
short erreur; |
| 231 | 236 |
short ancien_dist; |
| 232 | 237 |
while(1) |
| 233 | 238 |
{
|
| 234 |
ancien_dist = dist; |
|
| 235 |
dist = lecture_telemetre(); |
|
| 236 |
if(dist >= 1000) |
|
| 239 |
if (rampe == 0) |
|
| 237 | 240 |
{
|
| 238 |
dist = ancien_dist; |
|
| 241 |
ancien_dist = dist; |
|
| 242 |
dist = lecture_telemetre(); |
|
| 243 |
if((dist >= 2000)||(dist <= -2000)) |
|
| 244 |
{
|
|
| 245 |
dist = ancien_dist; |
|
| 246 |
} |
|
| 247 |
erreur = K*(consigne_telemetre-dist); |
|
| 248 |
angle_roues = -erreur; |
|
| 249 |
commande_angle_roues(-erreur); |
|
| 239 | 250 |
} |
| 240 |
erreur = K*(consigne_telemetre-dist); |
|
| 241 |
commande_angle_roues(-erreur); |
|
| 242 |
dly_tsk(98); |
|
| 251 |
dly_tsk(5); |
|
| 243 | 252 |
} |
| 244 | 253 |
} |
| 245 | 254 |
|
| 246 | 255 |
void commande_vitesse(short vitesse) |
| 256 |
{
|
|
| 257 |
CanFrame comm; |
|
| 258 |
|
|
| 259 |
comm.data.id = 'V'; |
|
| 260 |
comm.data.rtr = 0; |
|
| 261 |
comm.data.val=vitesse; |
|
| 262 |
snd_dtq(CanTx, comm.msg); |
|
| 263 |
} |
|
| 264 |
|
|
| 265 |
void init_vitesse(short vitesse) |
|
| 247 | 266 |
{
|
| 267 |
dly_tsk(3000); |
|
| 268 |
commande_vitesse(vitesse); |
|
| 269 |
} |
|
| 270 |
|
|
| 271 |
short lecture_capteur() |
|
| 272 |
{
|
|
| 248 | 273 |
CanFrame requete; |
| 249 | 274 |
|
| 250 |
dly_tsk(3000); |
|
| 251 |
requete.data.id = 'V'; |
|
| 252 |
requete.data.rtr = 0; |
|
| 253 |
requete.data.val=vitesse; |
|
| 275 |
requete.data.id = 'C'; |
|
| 276 |
requete.data.rtr = 1; |
|
| 254 | 277 |
snd_dtq(CanTx, requete.msg); |
| 278 |
wai_flg (ev_periph,0x03,TWF_ORW,&flag); |
|
| 279 |
clr_flg(ev_periph,~0x03); |
|
| 280 |
return (periph[ADDR('C')].val);
|
|
| 281 |
} |
|
| 282 |
|
|
| 283 |
void commande_capteur() |
|
| 284 |
{
|
|
| 285 |
while(1) |
|
| 286 |
{
|
|
| 287 |
lecture = lecture_capteur(); |
|
| 288 |
if((lecture & 0xFF00) == 0x6200) //bleu |
|
| 289 |
{
|
|
| 290 |
rampe = 1; |
|
| 291 |
} |
|
| 292 |
if ((lecture & 0xFF00) == 0x7600) //vert |
|
| 293 |
{
|
|
| 294 |
rampe = 0; |
|
| 295 |
commande_vitesse(20); |
|
| 296 |
} |
|
| 297 |
if ((lecture & 0xFF00) == 0x6a00) //jaune |
|
| 298 |
{
|
|
| 299 |
rampe = 0; |
|
| 300 |
} |
|
| 301 |
if ((lecture & 0xFF00) == 0x5604) |
|
| 302 |
{
|
|
| 303 |
commande_vitesse(45); |
|
| 304 |
} |
|
| 305 |
if ((lecture & 0xFF00) == 0x7200) //rouge |
|
| 306 |
{
|
|
| 307 |
consigne_telemetre = 600; |
|
| 308 |
} |
|
| 309 |
if ((lecture & 0xFF00) == 0x6300) //cyan |
|
| 310 |
{
|
|
| 311 |
consigne_telemetre = 705; |
|
| 312 |
} |
|
| 313 |
dly_tsk(100); |
|
| 314 |
} |
|
| 255 | 315 |
} |
Also available in: Unified diff