HomologationCachan
Dependencies: Moteur CACHAN_HOMOG mbed
main.cpp
- Committer:
- lenriquez389
- Date:
- 2018-06-09
- Revision:
- 0:105ca010735c
File content as of revision 0:105ca010735c:
#include "mbed.h" #include "Control.h" #define Vm 15 int32_t panError,tiltError; uint16_t blocks; void Suiveur_Pixy(void); void Suiveur_Moteurs(void); Timer temps_moteur,fin_jeux; int main() { temps_moteur.start(); temps_moteur.reset(); fin_jeux.start(); fin_jeux.reset(); while(1) { blocks=pixy.getBlocks(); if(blocks) { Suiveur_Pixy(); Suiveur_Moteurs(); } if(temps_moteur.read()>20) { MotD.setVit(0); MotD.setVit(0); break; } } } void Suiveur_Pixy(void) { //panError = X_CENTER - pixy.blocks[0].x; tiltError = pixy.blocks[0].y - Y_CENTER; //panLoop.update(panError); tiltLoop.update(tiltError); pixy.setServos(500L,tiltLoop.m_pos); } int16_t eant=0,e; float kp_=0.0007,kd_=0; //float kp_=2,kd_=0; void Suiveur_Moteurs(void) { float dt=temps_moteur.read(); temps_moteur.reset(); uint16_t posy=tiltLoop.m_pos; int vit=((Vm-4)*(PIXY_RCS_MAX_POS-posy))/(350L)+4; uint16_t posx=pixy.blocks[0].x; e=posx-PIXY_CENTER_X; float PID=kp_*(e)+kd_*(e-eant)/dt; float Vd=vit-PID; if(Vd<0)Vd=0; if(Vd>50)Vd=50; float Vi=Vm+PID; if(Vi<0)Vi=0; if(Vi>50)Vi=50; // printf("VitD: %d\t VitG: %d\n\r",(int)Vd,(int)Vi); MotD.setVit((int)(Vd)); MotG.setVit((int)(Vi)); eant=e; }