AX12 gros robot
Fork of command_AX12_gros_robot by
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?
User | Revision | Line number | New 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 |