CRAC-LIB
Dependents: Asserv_mot Asserv_mot
MYCRAC_utility.h@0:cbe368f18aeb, 2017-04-19 (annotated)
- Committer:
- Brand101
- Date:
- Wed Apr 19 17:41:26 2017 +0000
- Revision:
- 0:cbe368f18aeb
B1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Brand101 | 0:cbe368f18aeb | 1 | #include "ident_crac.h" |
Brand101 | 0:cbe368f18aeb | 2 | #include "cyapicallbacks.h" |
Brand101 | 0:cbe368f18aeb | 3 | |
Brand101 | 0:cbe368f18aeb | 4 | //----------------------------- constantes ----------------------------------------- |
Brand101 | 0:cbe368f18aeb | 5 | |
Brand101 | 0:cbe368f18aeb | 6 | #define LARGEUR_ROBOT 324.25 |
Brand101 | 0:cbe368f18aeb | 7 | #define PERIMETRE_ROUE_CODEUSE 160.01 |
Brand101 | 0:cbe368f18aeb | 8 | #define COEF_ROUE_GAUCHE 1 |
Brand101 | 0:cbe368f18aeb | 9 | |
Brand101 | 0:cbe368f18aeb | 10 | #define GROS_ROB 1 |
Brand101 | 0:cbe368f18aeb | 11 | |
Brand101 | 0:cbe368f18aeb | 12 | #define VMAX_DANGER 20.0 |
Brand101 | 0:cbe368f18aeb | 13 | #define AMAX_DANGER 1.0 |
Brand101 | 0:cbe368f18aeb | 14 | |
Brand101 | 0:cbe368f18aeb | 15 | #define PI 3.14159265359 |
Brand101 | 0:cbe368f18aeb | 16 | #define RESOLUTION_ROUE_CODEUSE 1024.0 |
Brand101 | 0:cbe368f18aeb | 17 | #define RECALAGE_TH 300 |
Brand101 | 0:cbe368f18aeb | 18 | |
Brand101 | 0:cbe368f18aeb | 19 | #define TYPE_DEPLACEMENT_STOP 0 |
Brand101 | 0:cbe368f18aeb | 20 | #define TYPE_DEPLACEMENT_IMMOBILE 1 |
Brand101 | 0:cbe368f18aeb | 21 | #define TYPE_DEPLACEMENT_LIGNE_DROITE 2 |
Brand101 | 0:cbe368f18aeb | 22 | #define TYPE_DEPLACEMENT_ROTATION 3 |
Brand101 | 0:cbe368f18aeb | 23 | #define TYPE_DEPLACEMENT_X_Y_THETA 4 |
Brand101 | 0:cbe368f18aeb | 24 | #define TYPE_DEPLACEMENT_RAYON_COURBURE 5 |
Brand101 | 0:cbe368f18aeb | 25 | #define TYPE_DEPLACEMENT_RECALAGE 6 |
Brand101 | 0:cbe368f18aeb | 26 | |
Brand101 | 0:cbe368f18aeb | 27 | #define INITIALISATION 0 |
Brand101 | 0:cbe368f18aeb | 28 | #define ACCELERATION_TRAPEZE 1 |
Brand101 | 0:cbe368f18aeb | 29 | #define VITESSE_CONSTANTE_TRAPEZE 2 |
Brand101 | 0:cbe368f18aeb | 30 | #define DECELERATION_TRAPEZE 3 |
Brand101 | 0:cbe368f18aeb | 31 | #define ARRET 4 |
Brand101 | 0:cbe368f18aeb | 32 | #define ACCELERATION_TRIANGLE 5 |
Brand101 | 0:cbe368f18aeb | 33 | #define DECELERATION_TRIANGLE 6 |
Brand101 | 0:cbe368f18aeb | 34 | |
Brand101 | 0:cbe368f18aeb | 35 | #define INIT_RECALAGE 0 |
Brand101 | 0:cbe368f18aeb | 36 | #define ACCELERATION_RECALAGE 1 |
Brand101 | 0:cbe368f18aeb | 37 | #define VITESSE_CONSTANTE_RECALAGE 2 |
Brand101 | 0:cbe368f18aeb | 38 | #define FIN_RECALAGE 3 |
Brand101 | 0:cbe368f18aeb | 39 | |
Brand101 | 0:cbe368f18aeb | 40 | #define INIT_X_Y_THETA 0 |
Brand101 | 0:cbe368f18aeb | 41 | #define ROTATION_X_Y_THETA_1 1 |
Brand101 | 0:cbe368f18aeb | 42 | #define LIGNE_DROITE_X_Y_THETA 2 |
Brand101 | 0:cbe368f18aeb | 43 | #define ROTATION_X_Y_THETA_2 3 |
Brand101 | 0:cbe368f18aeb | 44 | |
Brand101 | 0:cbe368f18aeb | 45 | #define INIT_RAYON_COURBURE 0 |
Brand101 | 0:cbe368f18aeb | 46 | #define ACCELERATION_TRAPEZE_RAYON_COURBURE 1 |
Brand101 | 0:cbe368f18aeb | 47 | #define VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE 2 |
Brand101 | 0:cbe368f18aeb | 48 | #define DECELERATION_TRAPEZE_RAYON_COURBURE 3 |
Brand101 | 0:cbe368f18aeb | 49 | #define ARRET_RAYON_COURBURE 4 |
Brand101 | 0:cbe368f18aeb | 50 | #define ACCELERATION_TRIANGLE_RAYON_COURBURE 5 |
Brand101 | 0:cbe368f18aeb | 51 | #define DECELERATION_TRIANGLE_RAYON_COURBURE 6 |
Brand101 | 0:cbe368f18aeb | 52 | |
Brand101 | 0:cbe368f18aeb | 53 | //----------------------------- nouveaux types ----------------------------------------- |
Brand101 | 0:cbe368f18aeb | 54 | |
Brand101 | 0:cbe368f18aeb | 55 | struct Ordre_deplacement |
Brand101 | 0:cbe368f18aeb | 56 | { |
Brand101 | 0:cbe368f18aeb | 57 | char type, enchainement; |
Brand101 | 0:cbe368f18aeb | 58 | short vmax, amax; |
Brand101 | 0:cbe368f18aeb | 59 | short distance, recalage, val_recalage; |
Brand101 | 0:cbe368f18aeb | 60 | long angle; |
Brand101 | 0:cbe368f18aeb | 61 | short x, y, theta; |
Brand101 | 0:cbe368f18aeb | 62 | signed char sens; |
Brand101 | 0:cbe368f18aeb | 63 | short rayon, vit_ray, theta_ray; |
Brand101 | 0:cbe368f18aeb | 64 | }; |
Brand101 | 0:cbe368f18aeb | 65 | |
Brand101 | 0:cbe368f18aeb | 66 | //----------------------------- prototypes de fcts ----------------------------------------- |
Brand101 | 0:cbe368f18aeb | 67 | |
Brand101 | 0:cbe368f18aeb | 68 | void write_PWM1(double vit); |
Brand101 | 0:cbe368f18aeb | 69 | void write_PWM2(double vit); |
Brand101 | 0:cbe368f18aeb | 70 | |
Brand101 | 0:cbe368f18aeb | 71 | |
Brand101 | 0:cbe368f18aeb | 72 | //----------------- DECLARATIONS EXTERN de var globales (definies dans main.cpp) --------------------- |
Brand101 | 0:cbe368f18aeb | 73 | |
Brand101 | 0:cbe368f18aeb | 74 | extern PwmOut mot1; // attention pour la decl d'objet : pas de pin / d'initialisation |
Brand101 | 0:cbe368f18aeb | 75 | extern PwmOut mot2; |
Brand101 | 0:cbe368f18aeb | 76 | |
Brand101 | 0:cbe368f18aeb | 77 | extern DigitalOut INA_M2; |
Brand101 | 0:cbe368f18aeb | 78 | extern DigitalOut INB_M2; |
Brand101 | 0:cbe368f18aeb | 79 | extern DigitalOut INA_M1; |
Brand101 | 0:cbe368f18aeb | 80 | extern DigitalOut INB_M1 ; |
Brand101 | 0:cbe368f18aeb | 81 | |
Brand101 | 0:cbe368f18aeb | 82 | extern double test1; // attention pour la decl de var : pas d'initialisation |
Brand101 | 0:cbe368f18aeb | 83 | extern double test2; |
Brand101 | 0:cbe368f18aeb | 84 | |
Brand101 | 0:cbe368f18aeb | 85 | extern double consigne_pos, consigne_vit, // Consignes de position et de vitesse dans les mouvements |
Brand101 | 0:cbe368f18aeb | 86 | commande1, commande2, // Commande, aka duty cycle des PWM de chaque moteur |
Brand101 | 0:cbe368f18aeb | 87 | last_pos1, last_pos2, pos1, pos2, // Valeurs des compteurs incrémentaux aux instants t et t-1 |
Brand101 | 0:cbe368f18aeb | 88 | ErreurPos1, ErreurPos2, last_ErreurPos1, last_ErreurPos2, // Valeurs des erreurs de position de la boucle d'asservissement |
Brand101 | 0:cbe368f18aeb | 89 | Somme_ErreurPos1, Somme_ErreurPos2, // Valeurs des intégrales des erreurs |
Brand101 | 0:cbe368f18aeb | 90 | Delta_ErreurPos1, Delta_ErreurPos2, // Valeurs des dérivées des erreurs |
Brand101 | 0:cbe368f18aeb | 91 | Kpp1, Kip1, Kdp1, Kpp2, Kip2, Kdp2, // Valeurs des correcteurs d'asservissement pour les 2 moteurs |
Brand101 | 0:cbe368f18aeb | 92 | Kpp1a, Kip1a, Kdp1a, Kpp2a, Kip2a, Kdp2a, // Valeurs des correcteurs d'asservissement pour les 2 moteurs |
Brand101 | 0:cbe368f18aeb | 93 | VMAX, AMAX, DMAX, // Valeurs maximales d'accéleration, décélération et vitesse |
Brand101 | 0:cbe368f18aeb | 94 | Odo_x, Odo_y, Odo_theta, Odo_val_pos_1, Odo_val_pos_2, Odo_last_val_pos_1, Odo_last_val_pos_2; // Variables de positions utilisées pour l'odométrie |
Brand101 | 0:cbe368f18aeb | 95 | extern uint32_t roue_drt_init, roue_gch_init; // Valeur des compteurs (!= 0) quand on commence un nouveau mouvement |
Brand101 | 0:cbe368f18aeb | 96 | extern short etat_automate, etat_automate_depl, new_message, |
Brand101 | 0:cbe368f18aeb | 97 | xytheta_sens, next_move_xyt, next_move, i, cpt, stop, stop_receive, param_xytheta[3], |
Brand101 | 0:cbe368f18aeb | 98 | etat_automate_xytheta, ralentare, recalage_debut, couleur_debut, recalage_debut_receive; |
Brand101 | 0:cbe368f18aeb | 99 | extern char nb_ordres, vitesse_danger, Stop_Danger, cpt_ordre, asser_actif; |
Brand101 | 0:cbe368f18aeb | 100 | extern struct Ordre_deplacement liste[200]; |
Brand101 | 0:cbe368f18aeb | 101 |