mouvement AX12 petit robot
Fork of command_AX12 by
main.cpp@0:a2a44c043932, 2017-03-15 (annotated)
- Committer:
- mathieulegros
- Date:
- Wed Mar 15 16:36:50 2017 +0000
- Revision:
- 0:a2a44c043932
- Child:
- 1:b3ff77670606
command AX12 same time
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" |
mathieulegros | 0:a2a44c043932 | 4 | |
mathieulegros | 0:a2a44c043932 | 5 | /* DECLARATION VARIABLES */ |
mathieulegros | 0:a2a44c043932 | 6 | Timer t; |
mathieulegros | 0:a2a44c043932 | 7 | Ticker flipper; |
mathieulegros | 0:a2a44c043932 | 8 | static char TAB[25]= {0x01,0x9A, 0x02, 0xFF, 0x00, // socle |
mathieulegros | 0:a2a44c043932 | 9 | 0x02,0x0A, 0x01, 0xFF, 0x00, // bas |
mathieulegros | 0:a2a44c043932 | 10 | 0x05,0xC8, 0x00, 0xFF, 0x00, // milieu |
mathieulegros | 0:a2a44c043932 | 11 | 0x0D,0x90, 0x01, 0xFF, 0x00, // haut |
mathieulegros | 0:a2a44c043932 | 12 | 0x09,0xD2, 0x01, 0xFF, 0x00}; //ventouse |
mathieulegros | 0:a2a44c043932 | 13 | |
mathieulegros | 0:a2a44c043932 | 14 | static char TAB2[25]= {0x01,0xE9, 0x00,0xFF, 0x00, // socle |
mathieulegros | 0:a2a44c043932 | 15 | 0x02,0x2C, 0x01, 0xFF, 0x00, // bas |
mathieulegros | 0:a2a44c043932 | 16 | 0x05,0xA6, 0x00, 0xFF, 0x00, // milieu |
mathieulegros | 0:a2a44c043932 | 17 | 0x0D,0xD2, 0x01, 0xFF, 0x00, // haut |
mathieulegros | 0:a2a44c043932 | 18 | 0x09,0xFF, 0x03, 0xFF, 0x00}; //ventouse |
mathieulegros | 0:a2a44c043932 | 19 | |
mathieulegros | 0:a2a44c043932 | 20 | static char TAB3[25]= {0x01,0xA6, 0x00,0xFF, 0x00, // socle |
mathieulegros | 0:a2a44c043932 | 21 | 0x02,0x6E, 0x01, 0xFF, 0x00, // bas |
mathieulegros | 0:a2a44c043932 | 22 | 0x05,0xE9, 0x00, 0xFF, 0x00, // milieu |
mathieulegros | 0:a2a44c043932 | 23 | 0x0D,0x90, 0x01, 0xFF, 0x00, // haut |
mathieulegros | 0:a2a44c043932 | 24 | 0x09,0xDD, 0x02, 0xFF, 0x00}; //ventouse |
mathieulegros | 0:a2a44c043932 | 25 | |
mathieulegros | 0:a2a44c043932 | 26 | /* ANGLE */ |
mathieulegros | 0:a2a44c043932 | 27 | |
mathieulegros | 0:a2a44c043932 | 28 | /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 |
mathieulegros | 0:a2a44c043932 | 29 | 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 |
mathieulegros | 0:a2a44c043932 | 30 | 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 |
mathieulegros | 0:a2a44c043932 | 31 | 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 |
mathieulegros | 0:a2a44c043932 | 32 | 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 |
mathieulegros | 0:a2a44c043932 | 33 | 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 |
mathieulegros | 0:a2a44c043932 | 34 | 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 |
mathieulegros | 0:a2a44c043932 | 35 | 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 |
mathieulegros | 0:a2a44c043932 | 36 | 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 |
mathieulegros | 0:a2a44c043932 | 37 | 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ |
mathieulegros | 0:a2a44c043932 | 38 | |
mathieulegros | 0:a2a44c043932 | 39 | |
mathieulegros | 0:a2a44c043932 | 40 | /*static char Tab1[5]={ 0x09, |
mathieulegros | 0:a2a44c043932 | 41 | 0x5E, |
mathieulegros | 0:a2a44c043932 | 42 | 0x01, |
mathieulegros | 0:a2a44c043932 | 43 | 0x70, |
mathieulegros | 0:a2a44c043932 | 44 | 0x01};*/ |
mathieulegros | 0:a2a44c043932 | 45 | |
mathieulegros | 0:a2a44c043932 | 46 | /*static char Tab2[5]={ 0x05, |
mathieulegros | 0:a2a44c043932 | 47 | 0x96, |
mathieulegros | 0:a2a44c043932 | 48 | 0x01, |
mathieulegros | 0:a2a44c043932 | 49 | 0x85, |
mathieulegros | 0:a2a44c043932 | 50 | 0x05};*/ |
mathieulegros | 0:a2a44c043932 | 51 | float angle=0.0; |
mathieulegros | 0:a2a44c043932 | 52 | 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; |
mathieulegros | 0:a2a44c043932 | 53 | int torque_socle=0, torque_bas=0, torque_milieu=0, torque_haut=0, torque_ventouse=0; |
mathieulegros | 0:a2a44c043932 | 54 | int error_torque_socle=0, error_torque_bas=0, error_torque_milieu=0, error_torque_haut=0, error_torque_ventouse=0; |
mathieulegros | 0:a2a44c043932 | 55 | |
mathieulegros | 0:a2a44c043932 | 56 | /* DECLARATION PROTOTYPES & POINTEURS */ |
mathieulegros | 0:a2a44c043932 | 57 | |
mathieulegros | 0:a2a44c043932 | 58 | short vitesse=700; // Vitesse angulaire (0-1023) |
mathieulegros | 0:a2a44c043932 | 59 | AX12 *ventouse_myAX12, *haut_myAX12, *milieu_myAX12, *bas_myAX12, *socle_myAX12, *multiple_AX12 ; |
mathieulegros | 0:a2a44c043932 | 60 | void Init_AX12(void); |
mathieulegros | 0:a2a44c043932 | 61 | void Lecture(void); |
mathieulegros | 0:a2a44c043932 | 62 | void Flip(void); |
mathieulegros | 0:a2a44c043932 | 63 | |
mathieulegros | 0:a2a44c043932 | 64 | |
mathieulegros | 0:a2a44c043932 | 65 | |
mathieulegros | 0:a2a44c043932 | 66 | |
mathieulegros | 0:a2a44c043932 | 67 | /* FONCTIONS */ |
mathieulegros | 0:a2a44c043932 | 68 | |
mathieulegros | 0:a2a44c043932 | 69 | void Init_AX12() // Initialisation des différents paramétres |
mathieulegros | 0:a2a44c043932 | 70 | { |
mathieulegros | 0:a2a44c043932 | 71 | |
mathieulegros | 0:a2a44c043932 | 72 | ventouse_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
mathieulegros | 0:a2a44c043932 | 73 | haut_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
mathieulegros | 0:a2a44c043932 | 74 | milieu_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
mathieulegros | 0:a2a44c043932 | 75 | bas_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
mathieulegros | 0:a2a44c043932 | 76 | socle_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
mathieulegros | 0:a2a44c043932 | 77 | |
mathieulegros | 0:a2a44c043932 | 78 | ventouse_myAX12->Set_Mode(0); // Mode Position(0) Mode Continu(1) |
mathieulegros | 0:a2a44c043932 | 79 | haut_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1) |
mathieulegros | 0:a2a44c043932 | 80 | milieu_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1) |
mathieulegros | 0:a2a44c043932 | 81 | bas_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1) |
mathieulegros | 0:a2a44c043932 | 82 | socle_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1) |
mathieulegros | 0:a2a44c043932 | 83 | } |
mathieulegros | 0:a2a44c043932 | 84 | |
mathieulegros | 0:a2a44c043932 | 85 | void Lecture() |
mathieulegros | 0:a2a44c043932 | 86 | { |
mathieulegros | 0:a2a44c043932 | 87 | angle= socle_myAX12-> Get_Position(); |
mathieulegros | 0:a2a44c043932 | 88 | test_socle = socle_myAX12 -> read_and_test(angle,TAB); |
mathieulegros | 0:a2a44c043932 | 89 | |
mathieulegros | 0:a2a44c043932 | 90 | angle= bas_myAX12-> Get_Position(); |
mathieulegros | 0:a2a44c043932 | 91 | test_bas = bas_myAX12-> read_and_test(angle,TAB); |
mathieulegros | 0:a2a44c043932 | 92 | |
mathieulegros | 0:a2a44c043932 | 93 | angle= milieu_myAX12-> Get_Position(); |
mathieulegros | 0:a2a44c043932 | 94 | test_milieu = milieu_myAX12-> read_and_test(angle,TAB); |
mathieulegros | 0:a2a44c043932 | 95 | |
mathieulegros | 0:a2a44c043932 | 96 | angle= haut_myAX12-> Get_Position(); |
mathieulegros | 0:a2a44c043932 | 97 | test_haut = haut_myAX12-> read_and_test(angle,TAB); |
mathieulegros | 0:a2a44c043932 | 98 | |
mathieulegros | 0:a2a44c043932 | 99 | angle= ventouse_myAX12-> Get_Position(); |
mathieulegros | 0:a2a44c043932 | 100 | test_ventouse = ventouse_myAX12-> read_and_test(angle,TAB); |
mathieulegros | 0:a2a44c043932 | 101 | |
mathieulegros | 0:a2a44c043932 | 102 | valeur_test=test_bas+test_milieu+test_haut+test_socle+test_ventouse; |
mathieulegros | 0:a2a44c043932 | 103 | |
mathieulegros | 0:a2a44c043932 | 104 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 105 | printf("\ntest_socle = %f",test_socle); |
mathieulegros | 0:a2a44c043932 | 106 | #endif |
mathieulegros | 0:a2a44c043932 | 107 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 108 | printf("\ntest_bas = %f",test_bas); |
mathieulegros | 0:a2a44c043932 | 109 | #endif |
mathieulegros | 0:a2a44c043932 | 110 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 111 | printf("\ntest_milieu = %f",test_milieu); |
mathieulegros | 0:a2a44c043932 | 112 | #endif |
mathieulegros | 0:a2a44c043932 | 113 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 114 | printf("\ntest_haut = %f",test_haut); |
mathieulegros | 0:a2a44c043932 | 115 | #endif |
mathieulegros | 0:a2a44c043932 | 116 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 117 | printf("\ntest_ventouse = %f",test_ventouse); |
mathieulegros | 0:a2a44c043932 | 118 | #endif |
mathieulegros | 0:a2a44c043932 | 119 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 120 | printf("\nvaleur_test = %f",valeur_test); |
mathieulegros | 0:a2a44c043932 | 121 | #endif |
mathieulegros | 0:a2a44c043932 | 122 | } |
mathieulegros | 0:a2a44c043932 | 123 | |
mathieulegros | 0:a2a44c043932 | 124 | /* PROGRAMME PRINCIPAL*/ |
mathieulegros | 0:a2a44c043932 | 125 | |
mathieulegros | 0:a2a44c043932 | 126 | int main() |
mathieulegros | 0:a2a44c043932 | 127 | { |
mathieulegros | 0:a2a44c043932 | 128 | flipper.attach(&Flip, 2.0); |
mathieulegros | 0:a2a44c043932 | 129 | |
mathieulegros | 0:a2a44c043932 | 130 | ventouse_myAX12 = new AX12(p9, p10, 9, 1000000); // Création objet : data pin 9&10, ID moteur 9, baud |
mathieulegros | 0:a2a44c043932 | 131 | haut_myAX12 = new AX12(p9, p10,13, 1000000); // Création objet : data pin 9&10, ID moteur 13, baud |
mathieulegros | 0:a2a44c043932 | 132 | milieu_myAX12 = new AX12(p9, p10, 5, 1000000); // Création objet : data pin 9&10, ID moteur 5, baud |
mathieulegros | 0:a2a44c043932 | 133 | bas_myAX12 = new AX12(p9, p10, 2, 1000000); // Création objet : data pin 9&10, ID moteur 2, baud |
mathieulegros | 0:a2a44c043932 | 134 | socle_myAX12 = new AX12(p9, p10, 1, 1000000); // Création objet : data pin 9&10, ID moteur 1, baud |
mathieulegros | 0:a2a44c043932 | 135 | multiple_AX12 = new AX12(p9,p10,0xFE,1000000); // Création objet : data pin 9&10, ID Broadcast, baud |
mathieulegros | 0:a2a44c043932 | 136 | |
mathieulegros | 0:a2a44c043932 | 137 | multiple_AX12->multiple_goal_and_speed(5,TAB); |
mathieulegros | 0:a2a44c043932 | 138 | |
mathieulegros | 0:a2a44c043932 | 139 | |
mathieulegros | 0:a2a44c043932 | 140 | while(valeur_test<4) |
mathieulegros | 0:a2a44c043932 | 141 | { |
mathieulegros | 0:a2a44c043932 | 142 | Lecture(); |
mathieulegros | 0:a2a44c043932 | 143 | } |
mathieulegros | 0:a2a44c043932 | 144 | |
mathieulegros | 0:a2a44c043932 | 145 | |
mathieulegros | 0:a2a44c043932 | 146 | multiple_AX12->multiple_goal_and_speed(5,TAB2); |
mathieulegros | 0:a2a44c043932 | 147 | |
mathieulegros | 0:a2a44c043932 | 148 | |
mathieulegros | 0:a2a44c043932 | 149 | |
mathieulegros | 0:a2a44c043932 | 150 | } |
mathieulegros | 0:a2a44c043932 | 151 | |
mathieulegros | 0:a2a44c043932 | 152 | void Flip () |
mathieulegros | 0:a2a44c043932 | 153 | { |
mathieulegros | 0:a2a44c043932 | 154 | |
mathieulegros | 0:a2a44c043932 | 155 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 156 | printf("\ntorque_ventouse avant = %d",torque_ventouse); |
mathieulegros | 0:a2a44c043932 | 157 | #endif |
mathieulegros | 0:a2a44c043932 | 158 | |
mathieulegros | 0:a2a44c043932 | 159 | torque_socle= socle_myAX12 -> Get_Torque_Enable(); |
mathieulegros | 0:a2a44c043932 | 160 | torque_bas= bas_myAX12 -> Get_Torque_Enable(); |
mathieulegros | 0:a2a44c043932 | 161 | torque_milieu= milieu_myAX12 -> Get_Torque_Enable(); |
mathieulegros | 0:a2a44c043932 | 162 | torque_haut= haut_myAX12 -> Get_Torque_Enable(); |
mathieulegros | 0:a2a44c043932 | 163 | torque_ventouse=ventouse_myAX12 -> Get_Torque_Enable(); |
mathieulegros | 0:a2a44c043932 | 164 | |
mathieulegros | 0:a2a44c043932 | 165 | |
mathieulegros | 0:a2a44c043932 | 166 | |
mathieulegros | 0:a2a44c043932 | 167 | /*if(torque_socle==1) |
mathieulegros | 0:a2a44c043932 | 168 | { |
mathieulegros | 0:a2a44c043932 | 169 | error_torque_socle=socle_myAX12 -> Set_Torque_Enable(0); |
mathieulegros | 0:a2a44c043932 | 170 | |
mathieulegros | 0:a2a44c043932 | 171 | } |
mathieulegros | 0:a2a44c043932 | 172 | if(torque_bas==1) |
mathieulegros | 0:a2a44c043932 | 173 | { |
mathieulegros | 0:a2a44c043932 | 174 | error_torque_bas=bas_myAX12 -> Set_Torque_Enable(0); |
mathieulegros | 0:a2a44c043932 | 175 | |
mathieulegros | 0:a2a44c043932 | 176 | } |
mathieulegros | 0:a2a44c043932 | 177 | if(torque_milieu==1) |
mathieulegros | 0:a2a44c043932 | 178 | { |
mathieulegros | 0:a2a44c043932 | 179 | error_torque_milieu=milieu_myAX12-> Set_Torque_Enable(0); |
mathieulegros | 0:a2a44c043932 | 180 | |
mathieulegros | 0:a2a44c043932 | 181 | } |
mathieulegros | 0:a2a44c043932 | 182 | if(torque_haut==1) |
mathieulegros | 0:a2a44c043932 | 183 | { |
mathieulegros | 0:a2a44c043932 | 184 | error_torque_haut=haut_myAX12 -> Set_Torque_Enable(0); |
mathieulegros | 0:a2a44c043932 | 185 | |
mathieulegros | 0:a2a44c043932 | 186 | } */ |
mathieulegros | 0:a2a44c043932 | 187 | |
mathieulegros | 0:a2a44c043932 | 188 | error_torque_ventouse=ventouse_myAX12 -> Set_Torque_Enable(0); |
mathieulegros | 0:a2a44c043932 | 189 | //error_torque_ventouse=ventouse_myAX12 -> Set_Torque_Enable(1); |
mathieulegros | 0:a2a44c043932 | 190 | #ifdef AX12_DEBUG |
mathieulegros | 0:a2a44c043932 | 191 | printf("\ntorque_ventouse apres = %d",torque_ventouse); |
mathieulegros | 0:a2a44c043932 | 192 | #endif |
mathieulegros | 0:a2a44c043932 | 193 | |
mathieulegros | 0:a2a44c043932 | 194 | } |