Luis Enriquez / Mbed 2 deprecated Cachan_Homolog2018

HomologationCachan

Dependencies:   Moteur CACHAN_HOMOG mbed

Fork of Cachan_Homolog by Luis Enriquez

main.cpp@0:105ca010735c, 2018-06-09 (annotated)

Committer:
lenriquez389
Date:
Sat Jun 09 03:59:07 2018 +0000
Revision:
0:105ca010735c
Homologation Cachan

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lenriquez389 0:105ca010735c 1 #include "mbed.h"
lenriquez389 0:105ca010735c 2 #include "Control.h"
lenriquez389 0:105ca010735c 3 #define Vm 15
lenriquez389 0:105ca010735c 4 int32_t panError,tiltError;
lenriquez389 0:105ca010735c 5 uint16_t blocks;
lenriquez389 0:105ca010735c 6 void Suiveur_Pixy(void);
lenriquez389 0:105ca010735c 7 void Suiveur_Moteurs(void);
lenriquez389 0:105ca010735c 8 Timer temps_moteur,fin_jeux;
lenriquez389 0:105ca010735c 9 int main()
lenriquez389 0:105ca010735c 10 {
lenriquez389 0:105ca010735c 11 temps_moteur.start();
lenriquez389 0:105ca010735c 12 temps_moteur.reset();
lenriquez389 0:105ca010735c 13 fin_jeux.start();
lenriquez389 0:105ca010735c 14 fin_jeux.reset();
lenriquez389 0:105ca010735c 15 while(1) {
lenriquez389 0:105ca010735c 16 blocks=pixy.getBlocks();
lenriquez389 0:105ca010735c 17 if(blocks) {
lenriquez389 0:105ca010735c 18 Suiveur_Pixy();
lenriquez389 0:105ca010735c 19 Suiveur_Moteurs();
lenriquez389 0:105ca010735c 20 }
lenriquez389 0:105ca010735c 21 if(temps_moteur.read()>20) {
lenriquez389 0:105ca010735c 22 MotD.setVit(0);
lenriquez389 0:105ca010735c 23 MotD.setVit(0);
lenriquez389 0:105ca010735c 24 break;
lenriquez389 0:105ca010735c 25 }
lenriquez389 0:105ca010735c 26 }
lenriquez389 0:105ca010735c 27 }
lenriquez389 0:105ca010735c 28
lenriquez389 0:105ca010735c 29 void Suiveur_Pixy(void)
lenriquez389 0:105ca010735c 30 {
lenriquez389 0:105ca010735c 31
lenriquez389 0:105ca010735c 32 //panError = X_CENTER - pixy.blocks[0].x;
lenriquez389 0:105ca010735c 33 tiltError = pixy.blocks[0].y - Y_CENTER;
lenriquez389 0:105ca010735c 34 //panLoop.update(panError);
lenriquez389 0:105ca010735c 35 tiltLoop.update(tiltError);
lenriquez389 0:105ca010735c 36 pixy.setServos(500L,tiltLoop.m_pos);
lenriquez389 0:105ca010735c 37 }
lenriquez389 0:105ca010735c 38
lenriquez389 0:105ca010735c 39 int16_t eant=0,e;
lenriquez389 0:105ca010735c 40 float kp_=0.0007,kd_=0;
lenriquez389 0:105ca010735c 41 //float kp_=2,kd_=0;
lenriquez389 0:105ca010735c 42 void Suiveur_Moteurs(void)
lenriquez389 0:105ca010735c 43 {
lenriquez389 0:105ca010735c 44 float dt=temps_moteur.read();
lenriquez389 0:105ca010735c 45 temps_moteur.reset();
lenriquez389 0:105ca010735c 46
lenriquez389 0:105ca010735c 47 uint16_t posy=tiltLoop.m_pos;
lenriquez389 0:105ca010735c 48 int vit=((Vm-4)*(PIXY_RCS_MAX_POS-posy))/(350L)+4;
lenriquez389 0:105ca010735c 49
lenriquez389 0:105ca010735c 50 uint16_t posx=pixy.blocks[0].x;
lenriquez389 0:105ca010735c 51 e=posx-PIXY_CENTER_X;
lenriquez389 0:105ca010735c 52
lenriquez389 0:105ca010735c 53 float PID=kp_*(e)+kd_*(e-eant)/dt;
lenriquez389 0:105ca010735c 54 float Vd=vit-PID;
lenriquez389 0:105ca010735c 55 if(Vd<0)Vd=0;
lenriquez389 0:105ca010735c 56 if(Vd>50)Vd=50;
lenriquez389 0:105ca010735c 57 float Vi=Vm+PID;
lenriquez389 0:105ca010735c 58 if(Vi<0)Vi=0;
lenriquez389 0:105ca010735c 59 if(Vi>50)Vi=50;
lenriquez389 0:105ca010735c 60 // printf("VitD: %d\t VitG: %d\n\r",(int)Vd,(int)Vi);
lenriquez389 0:105ca010735c 61 MotD.setVit((int)(Vd));
lenriquez389 0:105ca010735c 62 MotG.setVit((int)(Vi));
lenriquez389 0:105ca010735c 63
lenriquez389 0:105ca010735c 64 eant=e;
lenriquez389 0:105ca010735c 65 }