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@7:ad4a19e26b84, 2017-05-12 (annotated)

Committer:
ClementBreteau
Date:
Fri May 12 14:35:09 2017 +0000
Revision:
7:ad4a19e26b84
Parent:
6:c239c7abd7fe 
position ax12

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"
ClementBreteau 7:ad4a19e26b84 3 #include "actionneurs.h"
ClementBreteau 7:ad4a19e26b84 4 #include "global.h"
SquirrelGod 6:c239c7abd7fe 5
SquirrelGod 2:99b1cb0d9f5e 6 #include "ident_crac.h"
mathieulegros 0:a2a44c043932 7
SquirrelGod 4:888b32bc5b63 8 #define AX12_INITIALISATION 0
SquirrelGod 4:888b32bc5b63 9 #define AX12_PREPARATION_PRISE 1
SquirrelGod 4:888b32bc5b63 10 #define AX12_STOCKAGE_HAUT 2
SquirrelGod 4:888b32bc5b63 11 #define AX12_STOCKAGE_BAS 3
SquirrelGod 4:888b32bc5b63 12 #define AX12_DEPOSER 4
SquirrelGod 4:888b32bc5b63 13 #define AX12_PREPARATION_DEPOT_BAS 5
SquirrelGod 4:888b32bc5b63 14 #define AX12_PREPARATION_DEPOT_HAUT 6
SquirrelGod 4:888b32bc5b63 15 #define AX12_POUSSER_MODULE 7
SquirrelGod 4:888b32bc5b63 16 #define AX12_DEFAUT 20
SquirrelGod 4:888b32bc5b63 17
ClementBreteau 7:ad4a19e26b84 18
ClementBreteau 7:ad4a19e26b84 19 #define SIZE_FIFO 25
ClementBreteau 7:ad4a19e26b84 20
SquirrelGod 2:99b1cb0d9f5e 21
SquirrelGod 2:99b1cb0d9f5e 22 extern "C" void mbed_reset();//Pour pouvoir reset la carte
SquirrelGod 2:99b1cb0d9f5e 23
SquirrelGod 2:99b1cb0d9f5e 24 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 25 /* FUNCTION NAME: canProcessRx */
SquirrelGod 2:99b1cb0d9f5e 26 /* DESCRIPTION : Fonction de traitement des messages CAN */
SquirrelGod 2:99b1cb0d9f5e 27 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 28 void canProcessRx(void);
SquirrelGod 2:99b1cb0d9f5e 29
SquirrelGod 2:99b1cb0d9f5e 30 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 31 /* FUNCTION NAME: canRx_ISR */
SquirrelGod 2:99b1cb0d9f5e 32 /* DESCRIPTION : Interruption en réception sur le CAN */
SquirrelGod 2:99b1cb0d9f5e 33 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 34 void canRx_ISR (void);
SquirrelGod 2:99b1cb0d9f5e 35
SquirrelGod 2:99b1cb0d9f5e 36 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 37 /* FUNCTION NAME: SendRawId */
SquirrelGod 2:99b1cb0d9f5e 38 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
SquirrelGod 2:99b1cb0d9f5e 39 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 40 void SendRawId (unsigned short id);
SquirrelGod 2:99b1cb0d9f5e 41
SquirrelGod 2:99b1cb0d9f5e 42 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 43 /* FUNCTION NAME: Fin_action */
SquirrelGod 4:888b32bc5b63 44 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
SquirrelGod 4:888b32bc5b63 45 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 46 void Fin_action(void);
SquirrelGod 4:888b32bc5b63 47
SquirrelGod 4:888b32bc5b63 48 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 49 /* FUNCTION NAME: Automate_ax12 */
SquirrelGod 4:888b32bc5b63 50 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
SquirrelGod 4:888b32bc5b63 51 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 52 void AX12_automate(void);
ClementBreteau 7:ad4a19e26b84 53
ClementBreteau 7:ad4a19e26b84 54
ClementBreteau 7:ad4a19e26b84 55 /* DECLARATION VARIABLES */
ClementBreteau 7:ad4a19e26b84 56 CAN can1(p30,p29);
ClementBreteau 7:ad4a19e26b84 57 CANMessage msgRxBuffer[SIZE_FIFO];
ClementBreteau 7:ad4a19e26b84 58 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
ClementBreteau 7:ad4a19e26b84 59 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
ConstantMBED 1:b3ff77670606 60
ClementBreteau 7:ad4a19e26b84 61 extern "C" void mbed_reset();//Pour pouvoir reset la carte
SquirrelGod 4:888b32bc5b63 62
ClementBreteau 7:ad4a19e26b84 63 unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0;
ClementBreteau 7:ad4a19e26b84 64 short vitesse=700;
ClementBreteau 7:ad4a19e26b84 65 float angle=0.0;
ClementBreteau 7:ad4a19e26b84 66 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;
ClementBreteau 7:ad4a19e26b84 67
SquirrelGod 2:99b1cb0d9f5e 68
SquirrelGod 2:99b1cb0d9f5e 69 /* MAIN */
mathieulegros 0:a2a44c043932 70
SquirrelGod 2:99b1cb0d9f5e 71 int main()
mathieulegros 0:a2a44c043932 72 {
SquirrelGod 2:99b1cb0d9f5e 73 can1.frequency(1000000); // fréquence de travail 1Mbit/s
SquirrelGod 2:99b1cb0d9f5e 74 can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
SquirrelGod 2:99b1cb0d9f5e 75
ClementBreteau 7:ad4a19e26b84 76 declarationAX12();
SquirrelGod 4:888b32bc5b63 77
SquirrelGod 4:888b32bc5b63 78 //Demonstration bras gauche
ClementBreteau 7:ad4a19e26b84 79 /*Initialisation_gauche();
SquirrelGod 4:888b32bc5b63 80 wait(1);
SquirrelGod 4:888b32bc5b63 81 Preparation_prise_gauche();
SquirrelGod 4:888b32bc5b63 82 wait(1);
SquirrelGod 4:888b32bc5b63 83 Prendre_module_gauche();
SquirrelGod 4:888b32bc5b63 84 wait(1);
SquirrelGod 4:888b32bc5b63 85 Preparation_module_gauche();
SquirrelGod 4:888b32bc5b63 86 wait(1);
SquirrelGod 4:888b32bc5b63 87 Tourner_module_gauche();
SquirrelGod 4:888b32bc5b63 88 wait(1);
SquirrelGod 4:888b32bc5b63 89 Preparation_module_gauche();
SquirrelGod 4:888b32bc5b63 90 wait(1);
SquirrelGod 4:888b32bc5b63 91 Tourner_module_gauche();
SquirrelGod 4:888b32bc5b63 92 wait(1);
SquirrelGod 4:888b32bc5b63 93 Preparation_module_gauche();
SquirrelGod 4:888b32bc5b63 94 wait(1);
ClementBreteau 7:ad4a19e26b84 95 Tourner_module_gauche();*/
ClementBreteau 7:ad4a19e26b84 96 float position =0;
SquirrelGod 2:99b1cb0d9f5e 97 while(true) {
SquirrelGod 4:888b32bc5b63 98 //AX12_automate();
ClementBreteau 7:ad4a19e26b84 99 canProcessRx();
ClementBreteau 7:ad4a19e26b84 100
SquirrelGod 2:99b1cb0d9f5e 101 }
SquirrelGod 2:99b1cb0d9f5e 102 }
SquirrelGod 2:99b1cb0d9f5e 103
SquirrelGod 2:99b1cb0d9f5e 104 /* FONCTIONS */
SquirrelGod 2:99b1cb0d9f5e 105
SquirrelGod 2:99b1cb0d9f5e 106 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 107 /* FUNCTION NAME: canProcessRx */
SquirrelGod 2:99b1cb0d9f5e 108 /* DESCRIPTION : Fonction de traitement des messages CAN */
SquirrelGod 2:99b1cb0d9f5e 109 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 110 void canProcessRx(void)
SquirrelGod 2:99b1cb0d9f5e 111 {
SquirrelGod 2:99b1cb0d9f5e 112 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
SquirrelGod 4:888b32bc5b63 113
SquirrelGod 2:99b1cb0d9f5e 114 CANMessage msgTx=CANMessage();
SquirrelGod 4:888b32bc5b63 115 msgTx.format=CANStandard;
SquirrelGod 4:888b32bc5b63 116 msgTx.type=CANData;
SquirrelGod 2:99b1cb0d9f5e 117 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
SquirrelGod 4:888b32bc5b63 118
SquirrelGod 2:99b1cb0d9f5e 119 if(FIFO_occupation<0)
SquirrelGod 2:99b1cb0d9f5e 120 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
SquirrelGod 4:888b32bc5b63 121
SquirrelGod 2:99b1cb0d9f5e 122 if(FIFO_max_occupation<FIFO_occupation)
SquirrelGod 2:99b1cb0d9f5e 123 FIFO_max_occupation=FIFO_occupation;
SquirrelGod 4:888b32bc5b63 124
SquirrelGod 2:99b1cb0d9f5e 125 if(FIFO_occupation!=0) {
SquirrelGod 2:99b1cb0d9f5e 126
SquirrelGod 2:99b1cb0d9f5e 127 switch(msgRxBuffer[FIFO_lecture].id) {
SquirrelGod 4:888b32bc5b63 128 case CHECK_AX12:
SquirrelGod 4:888b32bc5b63 129 SendRawId(ALIVE_AX12);
SquirrelGod 4:888b32bc5b63 130 flag = 1;
ClementBreteau 7:ad4a19e26b84 131 mbed_reset();
SquirrelGod 2:99b1cb0d9f5e 132 break;
SquirrelGod 4:888b32bc5b63 133
SquirrelGod 4:888b32bc5b63 134 case SERVO_AX12_ACTION :
SquirrelGod 4:888b32bc5b63 135 etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
SquirrelGod 2:99b1cb0d9f5e 136
SquirrelGod 4:888b32bc5b63 137 //ACK de reception des actions a effectuer
SquirrelGod 4:888b32bc5b63 138 msgTx.id = SERVO_AX12_ACK;
SquirrelGod 4:888b32bc5b63 139 msgTx.len = 1;
SquirrelGod 4:888b32bc5b63 140 msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
SquirrelGod 4:888b32bc5b63 141 can1.write(msgTx);
SquirrelGod 2:99b1cb0d9f5e 142 break;
ClementBreteau 7:ad4a19e26b84 143 case 0x123:
ClementBreteau 7:ad4a19e26b84 144 SendRawId(ALIVE_AX12);
ClementBreteau 7:ad4a19e26b84 145 if(msgRxBuffer[FIFO_lecture].data[0] == 0){
ClementBreteau 7:ad4a19e26b84 146 getPosiotionCentrale();
ClementBreteau 7:ad4a19e26b84 147 }else if(msgRxBuffer[FIFO_lecture].data[0] == 1){
ClementBreteau 7:ad4a19e26b84 148 getPosiotionGauche();
ClementBreteau 7:ad4a19e26b84 149 }else{
ClementBreteau 7:ad4a19e26b84 150 getPosiotionDroite();
ClementBreteau 7:ad4a19e26b84 151 }
ClementBreteau 7:ad4a19e26b84 152 break;
ClementBreteau 7:ad4a19e26b84 153 case 0x456:
ClementBreteau 7:ad4a19e26b84 154 //Demonstration pince centrale
ClementBreteau 7:ad4a19e26b84 155 switch(msgRxBuffer[FIFO_lecture].data[0]){
ClementBreteau 7:ad4a19e26b84 156 case 0:
ClementBreteau 7:ad4a19e26b84 157 Initialisation_position();
ClementBreteau 7:ad4a19e26b84 158 break;
ClementBreteau 7:ad4a19e26b84 159 case 1:
ClementBreteau 7:ad4a19e26b84 160 Preparation_prise();
ClementBreteau 7:ad4a19e26b84 161 break;
ClementBreteau 7:ad4a19e26b84 162 case 2:
ClementBreteau 7:ad4a19e26b84 163 Stockage_haut();
ClementBreteau 7:ad4a19e26b84 164 break;
ClementBreteau 7:ad4a19e26b84 165 case 3:
ClementBreteau 7:ad4a19e26b84 166 Stockage_bas();
ClementBreteau 7:ad4a19e26b84 167 break;
ClementBreteau 7:ad4a19e26b84 168 case 4:
ClementBreteau 7:ad4a19e26b84 169 Pousser_module();
ClementBreteau 7:ad4a19e26b84 170 break;
ClementBreteau 7:ad4a19e26b84 171 case 5:
ClementBreteau 7:ad4a19e26b84 172 Preparation_depot_bas();
ClementBreteau 7:ad4a19e26b84 173 break;
ClementBreteau 7:ad4a19e26b84 174 case 6:
ClementBreteau 7:ad4a19e26b84 175 Deposer();
ClementBreteau 7:ad4a19e26b84 176 break;
ClementBreteau 7:ad4a19e26b84 177 case 7:
ClementBreteau 7:ad4a19e26b84 178 Preparation_depot_haut();
ClementBreteau 7:ad4a19e26b84 179 break;
ClementBreteau 7:ad4a19e26b84 180
ClementBreteau 7:ad4a19e26b84 181 }
ClementBreteau 7:ad4a19e26b84 182 wait(1);
ClementBreteau 7:ad4a19e26b84 183 break;
SquirrelGod 2:99b1cb0d9f5e 184 }
SquirrelGod 2:99b1cb0d9f5e 185
SquirrelGod 2:99b1cb0d9f5e 186 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 187 }
SquirrelGod 2:99b1cb0d9f5e 188 }
SquirrelGod 2:99b1cb0d9f5e 189
SquirrelGod 2:99b1cb0d9f5e 190 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 191 /* FUNCTION NAME: canRx_ISR */
SquirrelGod 2:99b1cb0d9f5e 192 /* DESCRIPTION : Interruption en réception sur le CAN */
SquirrelGod 2:99b1cb0d9f5e 193 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 194 void canRx_ISR (void)
SquirrelGod 2:99b1cb0d9f5e 195 {
SquirrelGod 2:99b1cb0d9f5e 196 if (can1.read(msgRxBuffer[FIFO_ecriture])) {
ClementBreteau 7:ad4a19e26b84 197 if(msgRxBuffer[FIFO_ecriture].id==0x65) mbed_reset();
SquirrelGod 2:99b1cb0d9f5e 198 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 199 }
SquirrelGod 2:99b1cb0d9f5e 200 }
SquirrelGod 2:99b1cb0d9f5e 201
SquirrelGod 2:99b1cb0d9f5e 202 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 203 /* FUNCTION NAME: SendRawId */
SquirrelGod 2:99b1cb0d9f5e 204 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
SquirrelGod 2:99b1cb0d9f5e 205 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 206 void SendRawId (unsigned short id)
SquirrelGod 2:99b1cb0d9f5e 207 {
SquirrelGod 2:99b1cb0d9f5e 208 CANMessage msgTx=CANMessage();
SquirrelGod 2:99b1cb0d9f5e 209 msgTx.id=id;
SquirrelGod 2:99b1cb0d9f5e 210 msgTx.len=0;
SquirrelGod 2:99b1cb0d9f5e 211 can1.write(msgTx);
SquirrelGod 2:99b1cb0d9f5e 212 }
SquirrelGod 2:99b1cb0d9f5e 213
SquirrelGod 3:6cbd7d2c78f4 214
SquirrelGod 3:6cbd7d2c78f4 215
SquirrelGod 4:888b32bc5b63 216 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 217 /* FUNCTION NAME: Fin_action */
SquirrelGod 4:888b32bc5b63 218 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
SquirrelGod 4:888b32bc5b63 219 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 220 void Fin_action(void){
SquirrelGod 4:888b32bc5b63 221 CANMessage msgTx=CANMessage();
SquirrelGod 4:888b32bc5b63 222 msgTx.format=CANStandard;
SquirrelGod 4:888b32bc5b63 223 msgTx.type=CANData;
SquirrelGod 4:888b32bc5b63 224
SquirrelGod 4:888b32bc5b63 225 msgTx.id = SERVO_AX12_END;
SquirrelGod 4:888b32bc5b63 226 msgTx.len = 1;
SquirrelGod 4:888b32bc5b63 227 msgTx.data[0] = AX12_PREPARATION_PRISE;
SquirrelGod 4:888b32bc5b63 228 can1.write(msgTx);
SquirrelGod 4:888b32bc5b63 229 }
SquirrelGod 3:6cbd7d2c78f4 230
SquirrelGod 4:888b32bc5b63 231 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 232 /* FUNCTION NAME: Automate_ax12 */
SquirrelGod 4:888b32bc5b63 233 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
SquirrelGod 4:888b32bc5b63 234 /****************************************************************************************/
SquirrelGod 4:888b32bc5b63 235 void AX12_automate(void){
SquirrelGod 4:888b32bc5b63 236 switch(etat_ax12){
SquirrelGod 4:888b32bc5b63 237 case AX12_INITIALISATION :
SquirrelGod 4:888b32bc5b63 238 if (flag == 1){
SquirrelGod 4:888b32bc5b63 239 Initialisation_position();
SquirrelGod 4:888b32bc5b63 240 flag = 2;
SquirrelGod 4:888b32bc5b63 241 }
SquirrelGod 4:888b32bc5b63 242
SquirrelGod 4:888b32bc5b63 243 break;
SquirrelGod 4:888b32bc5b63 244
SquirrelGod 4:888b32bc5b63 245 case AX12_PREPARATION_PRISE :
SquirrelGod 4:888b32bc5b63 246 Preparation_prise();
SquirrelGod 4:888b32bc5b63 247 if (action == 0){
SquirrelGod 4:888b32bc5b63 248 Fin_action();
SquirrelGod 4:888b32bc5b63 249 action ++;
SquirrelGod 4:888b32bc5b63 250 }
SquirrelGod 4:888b32bc5b63 251 break;
SquirrelGod 4:888b32bc5b63 252
SquirrelGod 4:888b32bc5b63 253 case AX12_STOCKAGE_HAUT :
SquirrelGod 4:888b32bc5b63 254 Stockage_haut();
SquirrelGod 4:888b32bc5b63 255 etat_ax12 = AX12_DEFAUT;
SquirrelGod 4:888b32bc5b63 256 Fin_action();
SquirrelGod 4:888b32bc5b63 257 break;
SquirrelGod 4:888b32bc5b63 258
SquirrelGod 4:888b32bc5b63 259 case AX12_STOCKAGE_BAS :
SquirrelGod 4:888b32bc5b63 260 Stockage_bas();
SquirrelGod 4:888b32bc5b63 261 etat_ax12 = AX12_DEFAUT;
SquirrelGod 4:888b32bc5b63 262 Fin_action();
SquirrelGod 4:888b32bc5b63 263 break;
SquirrelGod 4:888b32bc5b63 264
SquirrelGod 4:888b32bc5b63 265 case AX12_DEPOSER :
SquirrelGod 4:888b32bc5b63 266 Deposer();
SquirrelGod 4:888b32bc5b63 267 etat_ax12 = AX12_DEFAUT;
SquirrelGod 4:888b32bc5b63 268 Fin_action();
SquirrelGod 4:888b32bc5b63 269 break;
SquirrelGod 4:888b32bc5b63 270
SquirrelGod 4:888b32bc5b63 271 case AX12_PREPARATION_DEPOT_BAS :
SquirrelGod 4:888b32bc5b63 272 Preparation_depot_bas();
SquirrelGod 4:888b32bc5b63 273 etat_ax12 = AX12_DEFAUT;
SquirrelGod 4:888b32bc5b63 274 Fin_action();
SquirrelGod 4:888b32bc5b63 275 break;
SquirrelGod 4:888b32bc5b63 276
SquirrelGod 4:888b32bc5b63 277 case AX12_PREPARATION_DEPOT_HAUT :
SquirrelGod 4:888b32bc5b63 278 Preparation_depot_haut();
SquirrelGod 4:888b32bc5b63 279 etat_ax12 = AX12_DEFAUT;
SquirrelGod 4:888b32bc5b63 280 Fin_action();
SquirrelGod 4:888b32bc5b63 281 break;
SquirrelGod 4:888b32bc5b63 282
SquirrelGod 4:888b32bc5b63 283 case AX12_POUSSER_MODULE :
SquirrelGod 4:888b32bc5b63 284 Pousser_module();
SquirrelGod 4:888b32bc5b63 285 etat_ax12 = AX12_DEFAUT;
SquirrelGod 4:888b32bc5b63 286 Fin_action();
SquirrelGod 4:888b32bc5b63 287 break;
SquirrelGod 4:888b32bc5b63 288
SquirrelGod 4:888b32bc5b63 289 case AX12_DEFAUT :
SquirrelGod 4:888b32bc5b63 290 action = 0;
SquirrelGod 4:888b32bc5b63 291 break;
SquirrelGod 4:888b32bc5b63 292 }
SquirrelGod 4:888b32bc5b63 293 }