CRAC Team / command_AX12_petit_robot_V4tempo

code ax12 petit robot 12/05/2017

Fork of command_AX12_petit_robot_V3 by CRAC Team

main.cpp@2:99b1cb0d9f5e, 2017-04-18 (annotated)

Committer:
SquirrelGod
Date:
Tue Apr 18 16:02:40 2017 +0000
Revision:
2:99b1cb0d9f5e
Parent:
1:b3ff77670606
Child:
3:6cbd7d2c78f4
AX12_control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mathieulegros 0:a2a44c043932 1 #include "mbed.h"
mathieulegros 0:a2a44c043932 2 #include "AX12.h"
mathieulegros 0:a2a44c043932 3 #include "cmsis.h"
SquirrelGod 2:99b1cb0d9f5e 4 #include "ident_crac.h"
mathieulegros 0:a2a44c043932 5
SquirrelGod 2:99b1cb0d9f5e 6 #define SIZE_FIFO 14
SquirrelGod 2:99b1cb0d9f5e 7 #define TIME 0.8
SquirrelGod 2:99b1cb0d9f5e 8
mathieulegros 0:a2a44c043932 9 /* DECLARATION VARIABLES */
SquirrelGod 2:99b1cb0d9f5e 10 CAN can1(p30,p29);
SquirrelGod 2:99b1cb0d9f5e 11 CANMessage msgRxBuffer[SIZE_FIFO];
SquirrelGod 2:99b1cb0d9f5e 12 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
SquirrelGod 2:99b1cb0d9f5e 13 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
SquirrelGod 2:99b1cb0d9f5e 14
SquirrelGod 2:99b1cb0d9f5e 15 extern "C" void mbed_reset();//Pour pouvoir reset la carte
SquirrelGod 2:99b1cb0d9f5e 16
SquirrelGod 2:99b1cb0d9f5e 17 unsigned char action = 0, choix = 0;
SquirrelGod 2:99b1cb0d9f5e 18 short vitesse=700;
SquirrelGod 2:99b1cb0d9f5e 19 float angle=0.0;
SquirrelGod 2:99b1cb0d9f5e 20 float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
SquirrelGod 2:99b1cb0d9f5e 21 AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *six_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
SquirrelGod 2:99b1cb0d9f5e 22
SquirrelGod 2:99b1cb0d9f5e 23
SquirrelGod 2:99b1cb0d9f5e 24 /* PROTOTYPES DE FONCTIONS ET POINTEURS */
SquirrelGod 2:99b1cb0d9f5e 25
SquirrelGod 2:99b1cb0d9f5e 26
SquirrelGod 2:99b1cb0d9f5e 27
SquirrelGod 2:99b1cb0d9f5e 28 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 29 /* FUNCTION NAME: canProcessRx */
SquirrelGod 2:99b1cb0d9f5e 30 /* DESCRIPTION : Fonction de traitement des messages CAN */
SquirrelGod 2:99b1cb0d9f5e 31 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 32 void canProcessRx(void);
SquirrelGod 2:99b1cb0d9f5e 33
SquirrelGod 2:99b1cb0d9f5e 34 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 35 /* FUNCTION NAME: canRx_ISR */
SquirrelGod 2:99b1cb0d9f5e 36 /* DESCRIPTION : Interruption en réception sur le CAN */
SquirrelGod 2:99b1cb0d9f5e 37 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 38 void canRx_ISR (void);
SquirrelGod 2:99b1cb0d9f5e 39
SquirrelGod 2:99b1cb0d9f5e 40 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 41 /* FUNCTION NAME: SendRawId */
SquirrelGod 2:99b1cb0d9f5e 42 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
SquirrelGod 2:99b1cb0d9f5e 43 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 44 void SendRawId (unsigned short id);
SquirrelGod 2:99b1cb0d9f5e 45
SquirrelGod 2:99b1cb0d9f5e 46 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 47 /* FUNCTION NAME: prendre_modules */
SquirrelGod 2:99b1cb0d9f5e 48 /* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */
SquirrelGod 2:99b1cb0d9f5e 49 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 50 void Prendre_modules(unsigned char choix);
SquirrelGod 2:99b1cb0d9f5e 51
SquirrelGod 2:99b1cb0d9f5e 52 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 53 /* FUNCTION NAME: Poser_modules */
SquirrelGod 2:99b1cb0d9f5e 54 /* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */
SquirrelGod 2:99b1cb0d9f5e 55 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 56 void Poser_modules(unsigned char choix);
SquirrelGod 2:99b1cb0d9f5e 57
SquirrelGod 2:99b1cb0d9f5e 58 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 59 /* FUNCTION NAME: Funny_action */
SquirrelGod 2:99b1cb0d9f5e 60 /* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
SquirrelGod 2:99b1cb0d9f5e 61 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 62 void Funny_Action(void);
SquirrelGod 2:99b1cb0d9f5e 63
SquirrelGod 2:99b1cb0d9f5e 64 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 65 /* FUNCTION NAME: Tourner_modules */
SquirrelGod 2:99b1cb0d9f5e 66 /* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
SquirrelGod 2:99b1cb0d9f5e 67 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 68 void Tourner_modules(void);
SquirrelGod 2:99b1cb0d9f5e 69
mathieulegros 0:a2a44c043932 70 Timer t;
mathieulegros 0:a2a44c043932 71 Ticker flipper;
SquirrelGod 2:99b1cb0d9f5e 72
SquirrelGod 2:99b1cb0d9f5e 73 ///////////TABLEAU PINCE DROITE ///////////////////////////
SquirrelGod 2:99b1cb0d9f5e 74 static char TAB1[25]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte
SquirrelGod 2:99b1cb0d9f5e 75 0x04,0x00, 0x02, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 76 0x07,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 77 0x0F,0x90, 0x01, 0xFF, 0x03};
ConstantMBED 1:b3ff77670606 78
SquirrelGod 2:99b1cb0d9f5e 79 static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
SquirrelGod 2:99b1cb0d9f5e 80 0x04,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 81 0x07,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 82 0x0F,0x90, 0x01, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 83
SquirrelGod 2:99b1cb0d9f5e 84 static char TAB3[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee
SquirrelGod 2:99b1cb0d9f5e 85 0x04,0x15, 0x02, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 86 0x07,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 87 0x0F,0x05, 0x02, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 88
SquirrelGod 2:99b1cb0d9f5e 89 static char TAB4[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement
SquirrelGod 2:99b1cb0d9f5e 90 0x04,0x30, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 91 0x07,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 92 0x0F,0x05, 0x02, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 93
SquirrelGod 2:99b1cb0d9f5e 94 static char TAB5[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
SquirrelGod 2:99b1cb0d9f5e 95 0x04,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 96 0x07,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 97 0x0F,0x05, 0x02, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 98
SquirrelGod 2:99b1cb0d9f5e 99 static char TAB6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin
SquirrelGod 2:99b1cb0d9f5e 100 0x04,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 101 0x07,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 102 0x0F,0x90, 0x01, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 103
ConstantMBED 1:b3ff77670606 104
SquirrelGod 2:99b1cb0d9f5e 105 //////////////////TABLEAU PINCE GAUCHE /////////////////////////////
SquirrelGod 2:99b1cb0d9f5e 106 static char TAB1125]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte
SquirrelGod 2:99b1cb0d9f5e 107 0x04,0x00, 0x02, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 108 0x07,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 109 0x0F,0x90, 0x01, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 110
SquirrelGod 2:99b1cb0d9f5e 111 static char TAB12[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
SquirrelGod 2:99b1cb0d9f5e 112 0x04,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 113 0x07,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 114 0x0F,0x90, 0x01, 0xFF, 0x03};
ConstantMBED 1:b3ff77670606 115
SquirrelGod 2:99b1cb0d9f5e 116 static char TAB13[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee
SquirrelGod 2:99b1cb0d9f5e 117 0x04,0x15, 0x02, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 118 0x07,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 119 0x0F,0x05, 0x02, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 120
SquirrelGod 2:99b1cb0d9f5e 121 static char TAB14[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement
SquirrelGod 2:99b1cb0d9f5e 122 0x04,0x30, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 123 0x07,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 124 0x0F,0x05, 0x02, 0xFF, 0x03};
ConstantMBED 1:b3ff77670606 125
SquirrelGod 2:99b1cb0d9f5e 126 static char TAB15[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
SquirrelGod 2:99b1cb0d9f5e 127 0x04,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 128 0x07,0xC0, 0x00, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 129 0x0F,0x05, 0x02, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 130
SquirrelGod 2:99b1cb0d9f5e 131 static char TAB1 6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin
SquirrelGod 2:99b1cb0d9f5e 132 0x04,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 133 0x07,0x4D, 0x01, 0xFF, 0x03,
SquirrelGod 2:99b1cb0d9f5e 134 0x0F,0x90, 0x01, 0xFF, 0x03};
ConstantMBED 1:b3ff77670606 135
SquirrelGod 2:99b1cb0d9f5e 136
mathieulegros 0:a2a44c043932 137 /* ANGLE */
mathieulegros 0:a2a44c043932 138
mathieulegros 0:a2a44c043932 139 /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
mathieulegros 0:a2a44c043932 140 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02
mathieulegros 0:a2a44c043932 141 30° = 0x64, 0x00 | 130°= 0xB1, 0x01
mathieulegros 0:a2a44c043932 142 40° = 0x85, 0x00 | 140°= 0xD2, 0x01
mathieulegros 0:a2a44c043932 143 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01
mathieulegros 0:a2a44c043932 144 60° = 0xC8, 0x00 | 160°= 0x15, 0x02
mathieulegros 0:a2a44c043932 145 70° = 0xE9, 0x00 | 170°= 0x36, 0x02
mathieulegros 0:a2a44c043932 146 80° = 0x0A, 0x01 | 180°= 0x58, 0x02
mathieulegros 0:a2a44c043932 147 90° = 0x2C, 0x01 | 190°= 0x79, 0x02
mathieulegros 0:a2a44c043932 148 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
mathieulegros 0:a2a44c043932 149
ConstantMBED 1:b3ff77670606 150 /* NUMERO AX12 */
ConstantMBED 1:b3ff77670606 151
ConstantMBED 1:b3ff77670606 152 /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
ConstantMBED 1:b3ff77670606 153 1 = 0x01 | 10 = 0x0A
ConstantMBED 1:b3ff77670606 154 2 = 0x02 | 11 = 0x0B
ConstantMBED 1:b3ff77670606 155 3 = 0x03 | 12 = 0x0C
ConstantMBED 1:b3ff77670606 156 4 = 0x04 | 13 = 0x0D
ConstantMBED 1:b3ff77670606 157 5 = 0x05 | 14 = 0x0E
ConstantMBED 1:b3ff77670606 158 6 = 0x06 | 15 = 0x0F
ConstantMBED 1:b3ff77670606 159 7 = 0x07 | 16 = 0x10
ConstantMBED 1:b3ff77670606 160 8 = 0x08 | 17 = 0x11 */
mathieulegros 0:a2a44c043932 161
mathieulegros 0:a2a44c043932 162
SquirrelGod 2:99b1cb0d9f5e 163
SquirrelGod 2:99b1cb0d9f5e 164 /* MAIN */
mathieulegros 0:a2a44c043932 165
SquirrelGod 2:99b1cb0d9f5e 166 int main()
mathieulegros 0:a2a44c043932 167 {
SquirrelGod 2:99b1cb0d9f5e 168 can1.frequency(1000000); // fréquence de travail 1Mbit/s
SquirrelGod 2:99b1cb0d9f5e 169 can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
SquirrelGod 2:99b1cb0d9f5e 170
SquirrelGod 2:99b1cb0d9f5e 171 CANMessage TrameTx = CANMessage();
SquirrelGod 2:99b1cb0d9f5e 172 TrameTx.format=CANStandard;
SquirrelGod 2:99b1cb0d9f5e 173 TrameTx.type=CANData;
SquirrelGod 2:99b1cb0d9f5e 174
SquirrelGod 2:99b1cb0d9f5e 175 /*
SquirrelGod 2:99b1cb0d9f5e 176 deux_myAX12 = new AX12(p9, p10, 2, 1000000); //main tourneuse
SquirrelGod 2:99b1cb0d9f5e 177 quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); // main tourneuse
SquirrelGod 2:99b1cb0d9f5e 178 */
SquirrelGod 2:99b1cb0d9f5e 179
SquirrelGod 2:99b1cb0d9f5e 180 trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince droite
SquirrelGod 2:99b1cb0d9f5e 181 treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince droite
SquirrelGod 2:99b1cb0d9f5e 182 quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince droite
SquirrelGod 2:99b1cb0d9f5e 183 huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince droite
SquirrelGod 2:99b1cb0d9f5e 184
SquirrelGod 2:99b1cb0d9f5e 185 quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince gauche
SquirrelGod 2:99b1cb0d9f5e 186 sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince gauche
SquirrelGod 2:99b1cb0d9f5e 187 quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince gauche
SquirrelGod 2:99b1cb0d9f5e 188 dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince gauche
SquirrelGod 2:99b1cb0d9f5e 189
SquirrelGod 2:99b1cb0d9f5e 190 six_myAX12 = new AX12(p28, p27, 6, 1000000); //funny action
SquirrelGod 2:99b1cb0d9f5e 191
SquirrelGod 2:99b1cb0d9f5e 192 multiple_myAX12 = new AX12(p13,p14,0xFE,1000000);
SquirrelGod 2:99b1cb0d9f5e 193 multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000);
SquirrelGod 2:99b1cb0d9f5e 194
SquirrelGod 2:99b1cb0d9f5e 195
SquirrelGod 2:99b1cb0d9f5e 196 while(true) {
SquirrelGod 2:99b1cb0d9f5e 197 canProcessRx();//Traitement des trames CAN en attente
SquirrelGod 2:99b1cb0d9f5e 198
SquirrelGod 2:99b1cb0d9f5e 199 if (action == 0){
SquirrelGod 2:99b1cb0d9f5e 200 choix = 1;
SquirrelGod 2:99b1cb0d9f5e 201 Prendre_modules(choix);
SquirrelGod 2:99b1cb0d9f5e 202 choix = 2;
SquirrelGod 2:99b1cb0d9f5e 203 Prendre_modules(choix);
SquirrelGod 2:99b1cb0d9f5e 204 wait(2);
SquirrelGod 2:99b1cb0d9f5e 205 /*
SquirrelGod 2:99b1cb0d9f5e 206 choix = 1;
SquirrelGod 2:99b1cb0d9f5e 207 Poser_modules(choix);
SquirrelGod 2:99b1cb0d9f5e 208 choix = 2;
SquirrelGod 2:99b1cb0d9f5e 209 Poser_modules(choix);
SquirrelGod 2:99b1cb0d9f5e 210 wait(2);
SquirrelGod 2:99b1cb0d9f5e 211
SquirrelGod 2:99b1cb0d9f5e 212 TrameTx.id = 0x092;
SquirrelGod 2:99b1cb0d9f5e 213 TrameTx.len = 1;
SquirrelGod 2:99b1cb0d9f5e 214 TrameTx.data[0] = 0;
SquirrelGod 2:99b1cb0d9f5e 215 can1.write(TrameTx);
SquirrelGod 2:99b1cb0d9f5e 216 */
SquirrelGod 2:99b1cb0d9f5e 217
SquirrelGod 2:99b1cb0d9f5e 218 action = 2;
SquirrelGod 2:99b1cb0d9f5e 219 }
SquirrelGod 2:99b1cb0d9f5e 220 }
SquirrelGod 2:99b1cb0d9f5e 221 }
SquirrelGod 2:99b1cb0d9f5e 222
SquirrelGod 2:99b1cb0d9f5e 223 /* FONCTIONS */
SquirrelGod 2:99b1cb0d9f5e 224
SquirrelGod 2:99b1cb0d9f5e 225 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 226 /* FUNCTION NAME: canProcessRx */
SquirrelGod 2:99b1cb0d9f5e 227 /* DESCRIPTION : Fonction de traitement des messages CAN */
SquirrelGod 2:99b1cb0d9f5e 228 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 229 void canProcessRx(void)
SquirrelGod 2:99b1cb0d9f5e 230 {
SquirrelGod 2:99b1cb0d9f5e 231 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
SquirrelGod 2:99b1cb0d9f5e 232 CANMessage msgTx=CANMessage();
SquirrelGod 2:99b1cb0d9f5e 233 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
SquirrelGod 2:99b1cb0d9f5e 234 if(FIFO_occupation<0)
SquirrelGod 2:99b1cb0d9f5e 235 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 236 if(FIFO_max_occupation<FIFO_occupation)
SquirrelGod 2:99b1cb0d9f5e 237 FIFO_max_occupation=FIFO_occupation;
SquirrelGod 2:99b1cb0d9f5e 238 if(FIFO_occupation!=0) {
SquirrelGod 2:99b1cb0d9f5e 239
SquirrelGod 2:99b1cb0d9f5e 240 switch(msgRxBuffer[FIFO_lecture].id) {
SquirrelGod 2:99b1cb0d9f5e 241 case SERVO_AX12_PROCESS :
SquirrelGod 2:99b1cb0d9f5e 242 action = 1;
SquirrelGod 2:99b1cb0d9f5e 243 break;
SquirrelGod 2:99b1cb0d9f5e 244
SquirrelGod 2:99b1cb0d9f5e 245 case CHECK_ACTIONNEURS:
SquirrelGod 2:99b1cb0d9f5e 246 SendRawId (ALIVE_ACTIONNEURS);
SquirrelGod 2:99b1cb0d9f5e 247 break;
SquirrelGod 2:99b1cb0d9f5e 248
SquirrelGod 2:99b1cb0d9f5e 249 }
SquirrelGod 2:99b1cb0d9f5e 250
SquirrelGod 2:99b1cb0d9f5e 251 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 252 }
SquirrelGod 2:99b1cb0d9f5e 253 }
SquirrelGod 2:99b1cb0d9f5e 254
SquirrelGod 2:99b1cb0d9f5e 255 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 256 /* FUNCTION NAME: canRx_ISR */
SquirrelGod 2:99b1cb0d9f5e 257 /* DESCRIPTION : Interruption en réception sur le CAN */
SquirrelGod 2:99b1cb0d9f5e 258 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 259 void canRx_ISR (void)
SquirrelGod 2:99b1cb0d9f5e 260 {
SquirrelGod 2:99b1cb0d9f5e 261 if (can1.read(msgRxBuffer[FIFO_ecriture])) {
SquirrelGod 2:99b1cb0d9f5e 262 if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
SquirrelGod 2:99b1cb0d9f5e 263 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 264 }
SquirrelGod 2:99b1cb0d9f5e 265 }
SquirrelGod 2:99b1cb0d9f5e 266
SquirrelGod 2:99b1cb0d9f5e 267 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 268 /* FUNCTION NAME: SendRawId */
SquirrelGod 2:99b1cb0d9f5e 269 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
SquirrelGod 2:99b1cb0d9f5e 270 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 271 void SendRawId (unsigned short id)
SquirrelGod 2:99b1cb0d9f5e 272 {
SquirrelGod 2:99b1cb0d9f5e 273 CANMessage msgTx=CANMessage();
SquirrelGod 2:99b1cb0d9f5e 274 msgTx.id=id;
SquirrelGod 2:99b1cb0d9f5e 275 msgTx.len=0;
SquirrelGod 2:99b1cb0d9f5e 276 can1.write(msgTx);
SquirrelGod 2:99b1cb0d9f5e 277 }
SquirrelGod 2:99b1cb0d9f5e 278 void Init_AX12() // Initialisation des différents paramétres
SquirrelGod 2:99b1cb0d9f5e 279 {
SquirrelGod 2:99b1cb0d9f5e 280 deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
SquirrelGod 2:99b1cb0d9f5e 281 quatre_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 282 six_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 283 sept_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 284 quatorze_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 285 quinze_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 286 dixhuit_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 287 huit_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 288 trois_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 289 quatorze_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 290 treize_myAX12-> Set_Goal_speed(vitesse);
mathieulegros 0:a2a44c043932 291
SquirrelGod 2:99b1cb0d9f5e 292 deux_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 293 quatre_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 294 six_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 295 sept_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 296 quatorze_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 297 quinze_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 298 dixhuit_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 299 huit_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 300 trois_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 301 quatorze_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 302 treize_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 303 }
SquirrelGod 2:99b1cb0d9f5e 304
SquirrelGod 2:99b1cb0d9f5e 305 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 306 /* FUNCTION NAME: prendre_modules */
SquirrelGod 2:99b1cb0d9f5e 307 /* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */
SquirrelGod 2:99b1cb0d9f5e 308 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 309 void Prendre_modules(unsigned char choix){
SquirrelGod 2:99b1cb0d9f5e 310 if (choix == 1){
SquirrelGod 2:99b1cb0d9f5e 311 //multiple_myAX12->multiple_goal_and_speed(4,TAB1);
SquirrelGod 2:99b1cb0d9f5e 312 //wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 313 multiple_myAX12->multiple_goal_and_speed(4,TAB2);
SquirrelGod 2:99b1cb0d9f5e 314 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 315 multiple_myAX12->multiple_goal_and_speed(4,TAB5);
SquirrelGod 2:99b1cb0d9f5e 316 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 317 multiple_myAX12->multiple_goal_and_speed(4,TAB3);
SquirrelGod 2:99b1cb0d9f5e 318 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 319 multiple_myAX12->multiple_goal_and_speed(4,TAB4);
SquirrelGod 2:99b1cb0d9f5e 320 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 321 multiple_myAX12->multiple_goal_and_speed(5,TAB6);
SquirrelGod 2:99b1cb0d9f5e 322 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 323 }
mathieulegros 0:a2a44c043932 324
SquirrelGod 2:99b1cb0d9f5e 325 else if (choix == 2){
SquirrelGod 2:99b1cb0d9f5e 326 //multiple2_myAX12->multiple_goal_and_speed(4,TAB11);
SquirrelGod 2:99b1cb0d9f5e 327 //wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 328 multiple2_myAX12->multiple_goal_and_speed(4,TAB12);
SquirrelGod 2:99b1cb0d9f5e 329 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 330 multiple2_myAX12->multiple_goal_and_speed(4,TAB15);
SquirrelGod 2:99b1cb0d9f5e 331 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 332 multiple2_myAX12->multiple_goal_and_speed(4,TAB13);
SquirrelGod 2:99b1cb0d9f5e 333 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 334 multiple2_myAX12->multiple_goal_and_speed(4,TAB14);
SquirrelGod 2:99b1cb0d9f5e 335 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 336 multiple2_myAX12->multiple_goal_and_speed(5,TAB16);
SquirrelGod 2:99b1cb0d9f5e 337 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 338 }
SquirrelGod 2:99b1cb0d9f5e 339
SquirrelGod 2:99b1cb0d9f5e 340 SendRawId(0x123);
SquirrelGod 2:99b1cb0d9f5e 341 }
mathieulegros 0:a2a44c043932 342
SquirrelGod 2:99b1cb0d9f5e 343 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 344 /* FUNCTION NAME: Poser_modules */
SquirrelGod 2:99b1cb0d9f5e 345 /* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */
SquirrelGod 2:99b1cb0d9f5e 346 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 347 void Poser_modules(unsigned char choix){
SquirrelGod 2:99b1cb0d9f5e 348 multiple_myAX12->multiple_goal_and_speed(4,TAB4);
SquirrelGod 2:99b1cb0d9f5e 349 wait(2);
SquirrelGod 2:99b1cb0d9f5e 350 multiple_myAX12->multiple_goal_and_speed(4,TAB3);
SquirrelGod 2:99b1cb0d9f5e 351 wait(2);
SquirrelGod 2:99b1cb0d9f5e 352 multiple_myAX12->multiple_goal_and_speed(4,TAB2);
SquirrelGod 2:99b1cb0d9f5e 353 wait(2);
SquirrelGod 2:99b1cb0d9f5e 354 multiple_myAX12->multiple_goal_and_speed(4,TAB1);
SquirrelGod 2:99b1cb0d9f5e 355 }
SquirrelGod 2:99b1cb0d9f5e 356
SquirrelGod 2:99b1cb0d9f5e 357 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 358 /* FUNCTION NAME: Funny_action */
SquirrelGod 2:99b1cb0d9f5e 359 /* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
SquirrelGod 2:99b1cb0d9f5e 360 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 361 void Funny_Action(void){
SquirrelGod 2:99b1cb0d9f5e 362 six_myAX12 -> Set_Secure_Goal(150);
SquirrelGod 2:99b1cb0d9f5e 363 wait(0.5);
SquirrelGod 2:99b1cb0d9f5e 364 six_myAX12 -> Set_Secure_Goal(60);
SquirrelGod 2:99b1cb0d9f5e 365 wait(0.5);
SquirrelGod 2:99b1cb0d9f5e 366 six_myAX12 -> Set_Secure_Goal(150);
SquirrelGod 2:99b1cb0d9f5e 367 }
SquirrelGod 2:99b1cb0d9f5e 368
SquirrelGod 2:99b1cb0d9f5e 369 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 370 /* FUNCTION NAME: Tourner_modules */
SquirrelGod 2:99b1cb0d9f5e 371 /* DESCRIPTION : Fonction qui permet de deployer la main tourneuse */
SquirrelGod 2:99b1cb0d9f5e 372 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 373 void Tourner_modules(void){
SquirrelGod 2:99b1cb0d9f5e 374 multiple2_myAX12 -> multiple_goal_and_speed(2,TAB11);
SquirrelGod 2:99b1cb0d9f5e 375 wait(1.5);
SquirrelGod 2:99b1cb0d9f5e 376 multiple2_myAX12 -> multiple_goal_and_speed(2,TAB12);
SquirrelGod 2:99b1cb0d9f5e 377 }
mathieulegros 0:a2a44c043932 378
mathieulegros 0:a2a44c043932 379
SquirrelGod 2:99b1cb0d9f5e 380