CRAC Team / MYCRAC

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?

UserRevisionLine numberNew 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