mouvement AX12 petit robot
Fork of command_AX12 by
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?
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; |
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 | } |