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@1:b3ff77670606, 2017-03-21 (annotated)

Committer:
ConstantMBED
Date:
Tue Mar 21 14:46:54 2017 +0000
Revision:
1:b3ff77670606
Parent:
0:a2a44c043932
Child:
2:99b1cb0d9f5e
Commande des 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"
mathieulegros 0:a2a44c043932 3 #include "cmsis.h"
mathieulegros 0:a2a44c043932 4
mathieulegros 0:a2a44c043932 5 /* DECLARATION VARIABLES */
mathieulegros 0:a2a44c043932 6 Timer t;
mathieulegros 0:a2a44c043932 7 Ticker flipper;
ConstantMBED 1:b3ff77670606 8 static char TAB1[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 9 0x12,0xBC, 0x02, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 10 0x04,0xE9, 0x00, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 11 0x07,0x4D, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 12 0x0F,0x90, 0x01, 0xFF, 0x00};
ConstantMBED 1:b3ff77670606 13
ConstantMBED 1:b3ff77670606 14 static char TAB2[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 15 0x12,0x25, 0x02, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 16 0x04,0x15, 0x02, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 17 0x07,0x4D, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 18 0x0F,0x90, 0x01, 0xFF, 0x00};
ConstantMBED 1:b3ff77670606 19
ConstantMBED 1:b3ff77670606 20 static char TAB3[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 21 0x12,0x2C, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 22 0x04,0x15, 0x02, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 23 0x07,0x4D, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 24 0x0F,0x90, 0x01, 0xFF, 0x00};
ConstantMBED 1:b3ff77670606 25
ConstantMBED 1:b3ff77670606 26 static char TAB4[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 27 0x12,0x2C, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 28 0x04,0x4D, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 29 0x07,0x4D, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 30 0x0F,0x90, 0x01, 0xFF, 0x00};
ConstantMBED 1:b3ff77670606 31
ConstantMBED 1:b3ff77670606 32 static char TAB5[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 33 0x12,0x2C, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 34 0x04,0x4D, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 35 0x07,0x6E, 0x01, 0xFF, 0x00,
ConstantMBED 1:b3ff77670606 36 0x0F,0x90, 0x01, 0xFF, 0x00};
ConstantMBED 1:b3ff77670606 37
ConstantMBED 1:b3ff77670606 38
mathieulegros 0:a2a44c043932 39
mathieulegros 0:a2a44c043932 40 /* ANGLE */
mathieulegros 0:a2a44c043932 41
mathieulegros 0:a2a44c043932 42 /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
mathieulegros 0:a2a44c043932 43 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02
mathieulegros 0:a2a44c043932 44 30° = 0x64, 0x00 | 130°= 0xB1, 0x01
mathieulegros 0:a2a44c043932 45 40° = 0x85, 0x00 | 140°= 0xD2, 0x01
mathieulegros 0:a2a44c043932 46 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01
mathieulegros 0:a2a44c043932 47 60° = 0xC8, 0x00 | 160°= 0x15, 0x02
mathieulegros 0:a2a44c043932 48 70° = 0xE9, 0x00 | 170°= 0x36, 0x02
mathieulegros 0:a2a44c043932 49 80° = 0x0A, 0x01 | 180°= 0x58, 0x02
mathieulegros 0:a2a44c043932 50 90° = 0x2C, 0x01 | 190°= 0x79, 0x02
mathieulegros 0:a2a44c043932 51 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
mathieulegros 0:a2a44c043932 52
ConstantMBED 1:b3ff77670606 53 /* NUMERO AX12 */
ConstantMBED 1:b3ff77670606 54
ConstantMBED 1:b3ff77670606 55 /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
ConstantMBED 1:b3ff77670606 56 1 = 0x01 | 10 = 0x0A
ConstantMBED 1:b3ff77670606 57 2 = 0x02 | 11 = 0x0B
ConstantMBED 1:b3ff77670606 58 3 = 0x03 | 12 = 0x0C
ConstantMBED 1:b3ff77670606 59 4 = 0x04 | 13 = 0x0D
ConstantMBED 1:b3ff77670606 60 5 = 0x05 | 14 = 0x0E
ConstantMBED 1:b3ff77670606 61 6 = 0x06 | 15 = 0x0F
ConstantMBED 1:b3ff77670606 62 7 = 0x07 | 16 = 0x10
ConstantMBED 1:b3ff77670606 63 8 = 0x08 | 17 = 0x11 */
ConstantMBED 1:b3ff77670606 64
mathieulegros 0:a2a44c043932 65 /*static char Tab1[5]={ 0x09,
mathieulegros 0:a2a44c043932 66 0x5E,
mathieulegros 0:a2a44c043932 67 0x01,
mathieulegros 0:a2a44c043932 68 0x70,
mathieulegros 0:a2a44c043932 69 0x01};*/
mathieulegros 0:a2a44c043932 70
mathieulegros 0:a2a44c043932 71 /*static char Tab2[5]={ 0x05,
mathieulegros 0:a2a44c043932 72 0x96,
mathieulegros 0:a2a44c043932 73 0x01,
mathieulegros 0:a2a44c043932 74 0x85,
mathieulegros 0:a2a44c043932 75 0x05};*/
mathieulegros 0:a2a44c043932 76 float angle=0.0;
mathieulegros 0:a2a44c043932 77 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;
ConstantMBED 1:b3ff77670606 78
mathieulegros 0:a2a44c043932 79
mathieulegros 0:a2a44c043932 80 /* DECLARATION PROTOTYPES & POINTEURS */
mathieulegros 0:a2a44c043932 81
mathieulegros 0:a2a44c043932 82 short vitesse=700; // Vitesse angulaire (0-1023)
ConstantMBED 1:b3ff77670606 83 AX12 *dixhuit_myAX12, *quatre_myAX12,*six_myAX12, *sept_myAX12, *quinze_myAX12, *multiple_AX12;
mathieulegros 0:a2a44c043932 84 void Init_AX12(void);
mathieulegros 0:a2a44c043932 85 void Lecture(void);
mathieulegros 0:a2a44c043932 86 void Flip(void);
mathieulegros 0:a2a44c043932 87
mathieulegros 0:a2a44c043932 88
mathieulegros 0:a2a44c043932 89
mathieulegros 0:a2a44c043932 90
mathieulegros 0:a2a44c043932 91 /* FONCTIONS */
mathieulegros 0:a2a44c043932 92
mathieulegros 0:a2a44c043932 93 void Init_AX12() // Initialisation des différents paramétres
mathieulegros 0:a2a44c043932 94 {
mathieulegros 0:a2a44c043932 95
ConstantMBED 1:b3ff77670606 96 dixhuit_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
ConstantMBED 1:b3ff77670606 97 quatre_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 98 six_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 99 sept_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 100 quinze_myAX12-> Set_Goal_speed(vitesse);
mathieulegros 0:a2a44c043932 101
ConstantMBED 1:b3ff77670606 102 dixhuit_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1)
ConstantMBED 1:b3ff77670606 103 quatre_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 104 six_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 105 sept_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 106 quinze_myAX12-> Set_Mode(0);
mathieulegros 0:a2a44c043932 107 }
mathieulegros 0:a2a44c043932 108
ConstantMBED 1:b3ff77670606 109 /*void Lecture()
mathieulegros 0:a2a44c043932 110 {
mathieulegros 0:a2a44c043932 111 angle= socle_myAX12-> Get_Position();
mathieulegros 0:a2a44c043932 112 test_socle = socle_myAX12 -> read_and_test(angle,TAB);
mathieulegros 0:a2a44c043932 113
mathieulegros 0:a2a44c043932 114 angle= bas_myAX12-> Get_Position();
mathieulegros 0:a2a44c043932 115 test_bas = bas_myAX12-> read_and_test(angle,TAB);
mathieulegros 0:a2a44c043932 116
mathieulegros 0:a2a44c043932 117 angle= milieu_myAX12-> Get_Position();
mathieulegros 0:a2a44c043932 118 test_milieu = milieu_myAX12-> read_and_test(angle,TAB);
mathieulegros 0:a2a44c043932 119
mathieulegros 0:a2a44c043932 120 angle= haut_myAX12-> Get_Position();
mathieulegros 0:a2a44c043932 121 test_haut = haut_myAX12-> read_and_test(angle,TAB);
mathieulegros 0:a2a44c043932 122
mathieulegros 0:a2a44c043932 123 angle= ventouse_myAX12-> Get_Position();
mathieulegros 0:a2a44c043932 124 test_ventouse = ventouse_myAX12-> read_and_test(angle,TAB);
mathieulegros 0:a2a44c043932 125
mathieulegros 0:a2a44c043932 126 valeur_test=test_bas+test_milieu+test_haut+test_socle+test_ventouse;
mathieulegros 0:a2a44c043932 127
mathieulegros 0:a2a44c043932 128 #ifdef AX12_DEBUG
mathieulegros 0:a2a44c043932 129 printf("\ntest_socle = %f",test_socle);
mathieulegros 0:a2a44c043932 130 #endif
mathieulegros 0:a2a44c043932 131 #ifdef AX12_DEBUG
mathieulegros 0:a2a44c043932 132 printf("\ntest_bas = %f",test_bas);
mathieulegros 0:a2a44c043932 133 #endif
mathieulegros 0:a2a44c043932 134 #ifdef AX12_DEBUG
mathieulegros 0:a2a44c043932 135 printf("\ntest_milieu = %f",test_milieu);
mathieulegros 0:a2a44c043932 136 #endif
mathieulegros 0:a2a44c043932 137 #ifdef AX12_DEBUG
mathieulegros 0:a2a44c043932 138 printf("\ntest_haut = %f",test_haut);
mathieulegros 0:a2a44c043932 139 #endif
mathieulegros 0:a2a44c043932 140 #ifdef AX12_DEBUG
mathieulegros 0:a2a44c043932 141 printf("\ntest_ventouse = %f",test_ventouse);
mathieulegros 0:a2a44c043932 142 #endif
mathieulegros 0:a2a44c043932 143 #ifdef AX12_DEBUG
mathieulegros 0:a2a44c043932 144 printf("\nvaleur_test = %f",valeur_test);
mathieulegros 0:a2a44c043932 145 #endif
ConstantMBED 1:b3ff77670606 146 }*/
mathieulegros 0:a2a44c043932 147
mathieulegros 0:a2a44c043932 148 /* PROGRAMME PRINCIPAL*/
mathieulegros 0:a2a44c043932 149
mathieulegros 0:a2a44c043932 150 int main()
mathieulegros 0:a2a44c043932 151 {
ConstantMBED 1:b3ff77670606 152 // flipper.attach(&Flip, 2.0);
ConstantMBED 1:b3ff77670606 153 dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000);
ConstantMBED 1:b3ff77670606 154 quatre_myAX12 = new AX12(p13, p14, 4, 1000000);
ConstantMBED 1:b3ff77670606 155 six_myAX12 = new AX12(p13, p14, 6, 1000000);
ConstantMBED 1:b3ff77670606 156 sept_myAX12 = new AX12(p13, p14, 7, 1000000);
ConstantMBED 1:b3ff77670606 157 quinze_myAX12 = new AX12(p13, p14, 15, 1000000);
ConstantMBED 1:b3ff77670606 158 multiple_AX12 = new AX12(p13,p14,0xFE,1000000); // Création objet : data pin 9&10, ID Broadcast, baud
mathieulegros 0:a2a44c043932 159
ConstantMBED 1:b3ff77670606 160 multiple_AX12->multiple_goal_and_speed(5,TAB1);
ConstantMBED 1:b3ff77670606 161 wait(2);
ConstantMBED 1:b3ff77670606 162 multiple_AX12->multiple_goal_and_speed(5,TAB2);
ConstantMBED 1:b3ff77670606 163 wait(2);
ConstantMBED 1:b3ff77670606 164 multiple_AX12->multiple_goal_and_speed(5,TAB3);
ConstantMBED 1:b3ff77670606 165 wait(2);
ConstantMBED 1:b3ff77670606 166 multiple_AX12->multiple_goal_and_speed(5,TAB4);
ConstantMBED 1:b3ff77670606 167 wait(2);
ConstantMBED 1:b3ff77670606 168 multiple_AX12->multiple_goal_and_speed(5,TAB5);
mathieulegros 0:a2a44c043932 169
mathieulegros 0:a2a44c043932 170
ConstantMBED 1:b3ff77670606 171 //six_myAX12 -> Set_Secure_Goal(200);
ConstantMBED 1:b3ff77670606 172 /*quatorze_myAX12 -> Set_Secure_Goal(260);
ConstantMBED 1:b3ff77670606 173 gauche_myAX12 -> Set_Secure_Goal(280);
ConstantMBED 1:b3ff77670606 174 quatorze_myAX12 -> Set_Secure_Goal(300); //avoir ax12 14 a 90°*/
ConstantMBED 1:b3ff77670606 175
ConstantMBED 1:b3ff77670606 176 /* while(valeur_test<4)
mathieulegros 0:a2a44c043932 177 {
mathieulegros 0:a2a44c043932 178 Lecture();
ConstantMBED 1:b3ff77670606 179 }*/
mathieulegros 0:a2a44c043932 180
mathieulegros 0:a2a44c043932 181
ConstantMBED 1:b3ff77670606 182
mathieulegros 0:a2a44c043932 183
mathieulegros 0:a2a44c043932 184
mathieulegros 0:a2a44c043932 185
mathieulegros 0:a2a44c043932 186 }