Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of command_AX12_gros_robot 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 | } |