CRAC Team / Utilisatio_MX12_V2

Programme d'utilisation des AX12 avec rajout de l'MX12

Fork of test_carteAToutFaire_PR by CRAC Team

main.cpp@1:f3f702086a30, 2017-05-11 (annotated)

Committer:
ClementBreteau
Date:
Thu May 11 11:49:50 2017 +0000
Revision:
1:f3f702086a30
Parent:
0:7737d7573e3b
Child:
2:9d280856a536
test de la carte ? tout faire du petit robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ClementBreteau 0:7737d7573e3b 1 #include "mbed.h"
ClementBreteau 1:f3f702086a30 2 #include "AX12.h"
ClementBreteau 1:f3f702086a30 3 #include "cmsis.h"
ClementBreteau 0:7737d7573e3b 4
ClementBreteau 0:7737d7573e3b 5 #define SIZE_FIFO 20
ClementBreteau 1:f3f702086a30 6 #define TIME 1
ClementBreteau 1:f3f702086a30 7 #define T_MOT 0.00005
ClementBreteau 0:7737d7573e3b 8
ClementBreteau 0:7737d7573e3b 9 /*
ClementBreteau 0:7737d7573e3b 10 DigitalIn IO1(p23);
ClementBreteau 0:7737d7573e3b 11 DigitalIn IO2(p24);
ClementBreteau 0:7737d7573e3b 12 DigitalIn IO3(p25);
ClementBreteau 0:7737d7573e3b 13 DigitalIn IO4(p26);
ClementBreteau 0:7737d7573e3b 14
ClementBreteau 0:7737d7573e3b 15 AnalogIn A_in1(p15);
ClementBreteau 0:7737d7573e3b 16 AnalogIn A_in2(p16);
ClementBreteau 0:7737d7573e3b 17 AnalogIn A_in3(p17);
ClementBreteau 0:7737d7573e3b 18 AnalogIn A_in4(p18);
ClementBreteau 0:7737d7573e3b 19 AnalogIn A_in5(p19);
ClementBreteau 0:7737d7573e3b 20 AnalogIn A_in6(p20);
ClementBreteau 0:7737d7573e3b 21
ClementBreteau 0:7737d7573e3b 22 PwmOut IRL_1(p21);
ClementBreteau 0:7737d7573e3b 23 PwmOut IRL_2(p22);
ClementBreteau 0:7737d7573e3b 24 */
ClementBreteau 0:7737d7573e3b 25
ClementBreteau 0:7737d7573e3b 26 AnalogIn cptGauche(p20);
ClementBreteau 0:7737d7573e3b 27 AnalogIn cptDroit(p19);
ClementBreteau 0:7737d7573e3b 28
ClementBreteau 0:7737d7573e3b 29 DigitalIn pressionGauche(p23);
ClementBreteau 0:7737d7573e3b 30 DigitalIn pressionDroit(p24);
ClementBreteau 0:7737d7573e3b 31
ClementBreteau 0:7737d7573e3b 32 AnalogIn telemetre(p15);
ClementBreteau 0:7737d7573e3b 33
ClementBreteau 0:7737d7573e3b 34 DigitalIn jack(p25);
ClementBreteau 0:7737d7573e3b 35
ClementBreteau 1:f3f702086a30 36 PwmOut motGauche(p21);
ClementBreteau 1:f3f702086a30 37 PwmOut motDroit(p22);
ClementBreteau 0:7737d7573e3b 38
ClementBreteau 0:7737d7573e3b 39 DigitalOut led(LED1);
ClementBreteau 0:7737d7573e3b 40
ClementBreteau 0:7737d7573e3b 41 CAN can1(p30,p29); // Rx&Tx pour le CAN
ClementBreteau 0:7737d7573e3b 42 CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN
ClementBreteau 0:7737d7573e3b 43 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
ClementBreteau 1:f3f702086a30 44 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
ClementBreteau 0:7737d7573e3b 45
ClementBreteau 1:f3f702086a30 46 AX12 *AX12_17, *AX12_14, *multiple_AX12;
ClementBreteau 1:f3f702086a30 47 AX12 *AX12_14_2, *AX12_11, *multiple_AX12_2;
ClementBreteau 1:f3f702086a30 48 AX12 *AX12_4, *AX12_18, *multiple_AX12_3;
ClementBreteau 1:f3f702086a30 49
ClementBreteau 1:f3f702086a30 50 int printCapteurs = 1;
ClementBreteau 1:f3f702086a30 51
ClementBreteau 1:f3f702086a30 52 static char TAB1[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03,
ClementBreteau 1:f3f702086a30 53 0x11, 0x50, 0x02, 0xFF, 0x03};
ClementBreteau 1:f3f702086a30 54
ClementBreteau 1:f3f702086a30 55 static char TAB2[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03,
ClementBreteau 1:f3f702086a30 56 0x11, 0x00, 0x00, 0xFF, 0x03};
ClementBreteau 1:f3f702086a30 57
ClementBreteau 1:f3f702086a30 58 static char TAB3[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03,
ClementBreteau 1:f3f702086a30 59 0x0B, 0x50, 0x02, 0xFF, 0x03};
ClementBreteau 1:f3f702086a30 60
ClementBreteau 1:f3f702086a30 61 static char TAB4[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03,
ClementBreteau 1:f3f702086a30 62 0x0B, 0x00, 0x00, 0xFF, 0x03};
ClementBreteau 1:f3f702086a30 63
ClementBreteau 1:f3f702086a30 64 static char TAB5[10]= {0x04, 0x00, 0x00, 0xFF, 0x03,
ClementBreteau 1:f3f702086a30 65 0x12, 0x50, 0x02, 0xFF, 0x03};
ClementBreteau 1:f3f702086a30 66
ClementBreteau 1:f3f702086a30 67 static char TAB6[10]= {0x04, 0x50, 0x02, 0xFF, 0x03,
ClementBreteau 1:f3f702086a30 68 0x12, 0x00, 0x00, 0xFF, 0x03};
ClementBreteau 1:f3f702086a30 69
ClementBreteau 1:f3f702086a30 70 Timer t;
ClementBreteau 1:f3f702086a30 71 Ticker flipper;
ClementBreteau 1:f3f702086a30 72
ClementBreteau 0:7737d7573e3b 73
ClementBreteau 0:7737d7573e3b 74 void canRx_ISR (void)
ClementBreteau 0:7737d7573e3b 75 {
ClementBreteau 0:7737d7573e3b 76 if (can1.read(msgRxBuffer[FIFO_ecriture])) {
ClementBreteau 0:7737d7573e3b 77 /*if(msgRxBuffer[FIFO_ecriture].id==RESET_ACTIONNEURS) mbed_reset();
ClementBreteau 0:7737d7573e3b 78 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;*/
ClementBreteau 0:7737d7573e3b 79 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
ClementBreteau 0:7737d7573e3b 80 }
ClementBreteau 0:7737d7573e3b 81 }
ClementBreteau 0:7737d7573e3b 82
ClementBreteau 0:7737d7573e3b 83
ClementBreteau 0:7737d7573e3b 84
ClementBreteau 0:7737d7573e3b 85 /*********************************************************************************************************/
ClementBreteau 0:7737d7573e3b 86 /* FUNCTION NAME: SendRawId */
ClementBreteau 0:7737d7573e3b 87 /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
ClementBreteau 0:7737d7573e3b 88 /*********************************************************************************************************/
ClementBreteau 0:7737d7573e3b 89 void SendRawId (unsigned short id)
ClementBreteau 0:7737d7573e3b 90 {
ClementBreteau 0:7737d7573e3b 91 CANMessage msgTx=CANMessage();
ClementBreteau 0:7737d7573e3b 92 msgTx.id=id;
ClementBreteau 0:7737d7573e3b 93 msgTx.len=0;
ClementBreteau 0:7737d7573e3b 94 can1.write(msgTx);
ClementBreteau 0:7737d7573e3b 95 wait_us(200);
ClementBreteau 0:7737d7573e3b 96 }
ClementBreteau 0:7737d7573e3b 97
ClementBreteau 1:f3f702086a30 98 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 99 /* FUNCTION NAME: AX1 */
ClementBreteau 1:f3f702086a30 100 /* DESCRIPTION : bouge les AX12 */
ClementBreteau 1:f3f702086a30 101 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 102 void AX1(void){
ClementBreteau 1:f3f702086a30 103 multiple_AX12->multiple_goal_and_speed(2,TAB1);
ClementBreteau 1:f3f702086a30 104 wait(TIME);
ClementBreteau 1:f3f702086a30 105 multiple_AX12->multiple_goal_and_speed(2,TAB2);
ClementBreteau 1:f3f702086a30 106 wait(TIME);
ClementBreteau 1:f3f702086a30 107 }
ClementBreteau 1:f3f702086a30 108
ClementBreteau 1:f3f702086a30 109 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 110 /* FUNCTION NAME: AX2 */
ClementBreteau 1:f3f702086a30 111 /* DESCRIPTION : bouge les AX12 */
ClementBreteau 1:f3f702086a30 112 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 113 void AX2(void){
ClementBreteau 1:f3f702086a30 114 multiple_AX12_2->multiple_goal_and_speed(2,TAB3);
ClementBreteau 1:f3f702086a30 115 wait(TIME);
ClementBreteau 1:f3f702086a30 116 multiple_AX12_2->multiple_goal_and_speed(2,TAB4);
ClementBreteau 1:f3f702086a30 117 wait(TIME);
ClementBreteau 1:f3f702086a30 118 }
ClementBreteau 1:f3f702086a30 119
ClementBreteau 1:f3f702086a30 120 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 121 /* FUNCTION NAME: AX3 */
ClementBreteau 1:f3f702086a30 122 /* DESCRIPTION : bouge les AX12 */
ClementBreteau 1:f3f702086a30 123 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 124 void AX3(void){
ClementBreteau 1:f3f702086a30 125 multiple_AX12_3->multiple_goal_and_speed(2,TAB5);
ClementBreteau 1:f3f702086a30 126 wait(TIME);
ClementBreteau 1:f3f702086a30 127 multiple_AX12_3->multiple_goal_and_speed(2,TAB6);
ClementBreteau 1:f3f702086a30 128 wait(TIME);
ClementBreteau 1:f3f702086a30 129 }
ClementBreteau 1:f3f702086a30 130
ClementBreteau 1:f3f702086a30 131 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 132 /* FUNCTION NAME: moteur */
ClementBreteau 1:f3f702086a30 133 /* DESCRIPTION : bouge les moteurs */
ClementBreteau 1:f3f702086a30 134 /*********************************************************************************************************/
ClementBreteau 1:f3f702086a30 135 void moteur(void){
ClementBreteau 1:f3f702086a30 136 motGauche.write(0.5f);
ClementBreteau 1:f3f702086a30 137 motDroit.write(0.5f);
ClementBreteau 1:f3f702086a30 138 }
ClementBreteau 1:f3f702086a30 139
ClementBreteau 1:f3f702086a30 140
ClementBreteau 0:7737d7573e3b 141 /****************************************************************************************/
ClementBreteau 0:7737d7573e3b 142 /* FUNCTION NAME: canProcessRx */
ClementBreteau 0:7737d7573e3b 143 /* DESCRIPTION : Fonction de traitement des messages CAN */
ClementBreteau 0:7737d7573e3b 144 /****************************************************************************************/
ClementBreteau 0:7737d7573e3b 145 void canProcessRx(void){
ClementBreteau 0:7737d7573e3b 146 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
ClementBreteau 0:7737d7573e3b 147 CANMessage msgTx=CANMessage();
ClementBreteau 0:7737d7573e3b 148 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
ClementBreteau 0:7737d7573e3b 149 if(FIFO_occupation<0)
ClementBreteau 0:7737d7573e3b 150 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
ClementBreteau 0:7737d7573e3b 151 if(FIFO_max_occupation<FIFO_occupation)
ClementBreteau 0:7737d7573e3b 152 FIFO_max_occupation=FIFO_occupation;
ClementBreteau 0:7737d7573e3b 153 if(FIFO_occupation!=0) {
ClementBreteau 0:7737d7573e3b 154
ClementBreteau 0:7737d7573e3b 155 switch(msgRxBuffer[FIFO_lecture].id) {
ClementBreteau 0:7737d7573e3b 156 case 0x63:
ClementBreteau 0:7737d7573e3b 157 SendRawId(0x73);
ClementBreteau 0:7737d7573e3b 158 break;
ClementBreteau 0:7737d7573e3b 159 case 0x10:
ClementBreteau 0:7737d7573e3b 160 printCapteurs = 0;
ClementBreteau 0:7737d7573e3b 161 break;
ClementBreteau 0:7737d7573e3b 162 case 0x11:
ClementBreteau 0:7737d7573e3b 163 printCapteurs = 1;
ClementBreteau 0:7737d7573e3b 164 break;
ClementBreteau 0:7737d7573e3b 165
ClementBreteau 0:7737d7573e3b 166
ClementBreteau 0:7737d7573e3b 167 }
ClementBreteau 0:7737d7573e3b 168 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
ClementBreteau 0:7737d7573e3b 169 }
ClementBreteau 0:7737d7573e3b 170 }
ClementBreteau 0:7737d7573e3b 171
ClementBreteau 0:7737d7573e3b 172 int main() {
ClementBreteau 0:7737d7573e3b 173 can1.frequency(1000000); // fréquence de travail 1Mbit/s
ClementBreteau 0:7737d7573e3b 174 can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
ClementBreteau 0:7737d7573e3b 175
ClementBreteau 0:7737d7573e3b 176 CANMessage msgTx=CANMessage();
ClementBreteau 1:f3f702086a30 177
ClementBreteau 1:f3f702086a30 178
ClementBreteau 1:f3f702086a30 179 AX12_14 = new AX12(p9, p10, 14, 1000000); // tx, rx
ClementBreteau 1:f3f702086a30 180 AX12_17 = new AX12(p9, p10, 17, 1000000);
ClementBreteau 1:f3f702086a30 181 multiple_AX12 = new AX12(p9,p10,0xFE,1000000);
ClementBreteau 1:f3f702086a30 182
ClementBreteau 1:f3f702086a30 183
ClementBreteau 1:f3f702086a30 184 AX12_14_2 = new AX12(p13, p14, 14, 1000000);
ClementBreteau 1:f3f702086a30 185 AX12_11 = new AX12(p13, p14, 11, 1000000);
ClementBreteau 1:f3f702086a30 186 multiple_AX12_2 = new AX12(p13,p14,0xFE,1000000);
ClementBreteau 1:f3f702086a30 187
ClementBreteau 1:f3f702086a30 188 AX12_4= new AX12(p28, p27, 14, 1000000);
ClementBreteau 1:f3f702086a30 189 AX12_18= new AX12(p28, p27, 11, 1000000);
ClementBreteau 1:f3f702086a30 190 multiple_AX12_3= new AX12(p28,p27,0xFE,1000000);
ClementBreteau 1:f3f702086a30 191
ClementBreteau 1:f3f702086a30 192 motGauche.period(T_MOT);
ClementBreteau 1:f3f702086a30 193 motDroit.period(T_MOT);
ClementBreteau 1:f3f702086a30 194 motGauche.write(0.0);
ClementBreteau 1:f3f702086a30 195 motDroit.write(0.0);
ClementBreteau 1:f3f702086a30 196
ClementBreteau 0:7737d7573e3b 197 while(1) {
ClementBreteau 0:7737d7573e3b 198 led = !led;
ClementBreteau 0:7737d7573e3b 199
ClementBreteau 0:7737d7573e3b 200 canProcessRx();
ClementBreteau 1:f3f702086a30 201
ClementBreteau 0:7737d7573e3b 202 if (printCapteurs){
ClementBreteau 1:f3f702086a30 203
ClementBreteau 1:f3f702086a30 204 motGauche.write(0);
ClementBreteau 1:f3f702086a30 205 motDroit.write(0);
ClementBreteau 1:f3f702086a30 206
ClementBreteau 0:7737d7573e3b 207 msgTx.id=0x21;
ClementBreteau 0:7737d7573e3b 208 msgTx.format=CANStandard;
ClementBreteau 0:7737d7573e3b 209 msgTx.type=CANData;
ClementBreteau 0:7737d7573e3b 210 msgTx.len=7;
ClementBreteau 0:7737d7573e3b 211
ClementBreteau 0:7737d7573e3b 212 msgTx.data[0]=(unsigned char)cptGauche.read();
ClementBreteau 1:f3f702086a30 213 msgTx.data[1]=(unsigned char)cptDroit.read();
ClementBreteau 0:7737d7573e3b 214 msgTx.data[2]=(unsigned char)pressionGauche.read();
ClementBreteau 0:7737d7573e3b 215 msgTx.data[3]=(unsigned char)pressionDroit.read();
ClementBreteau 0:7737d7573e3b 216 msgTx.data[4]=(unsigned char)jack.read();
ClementBreteau 0:7737d7573e3b 217 msgTx.data[5]=(unsigned char)(telemetre.read_u16()>>8);
ClementBreteau 0:7737d7573e3b 218 msgTx.data[6]=(unsigned char)telemetre.read_u16();
ClementBreteau 0:7737d7573e3b 219
ClementBreteau 0:7737d7573e3b 220 can1.write(msgTx);
ClementBreteau 1:f3f702086a30 221 wait(0.2);
ClementBreteau 1:f3f702086a30 222 }else{
ClementBreteau 1:f3f702086a30 223 moteur();
ClementBreteau 0:7737d7573e3b 224
ClementBreteau 1:f3f702086a30 225 AX1();
ClementBreteau 1:f3f702086a30 226 AX2();
ClementBreteau 1:f3f702086a30 227 AX3();
ClementBreteau 1:f3f702086a30 228
ClementBreteau 0:7737d7573e3b 229 }
ClementBreteau 1:f3f702086a30 230
ClementBreteau 1:f3f702086a30 231
ClementBreteau 0:7737d7573e3b 232 }
ClementBreteau 0:7737d7573e3b 233 }
ClementBreteau 0:7737d7573e3b 234