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@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?

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;
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 }