Diff: main.cpp
- Revision:
- 6:b32a24905cd6
- Parent:
- 5:86f9ec683ad5
- Child:
- 7:49ef78d60837
- Child:
- 8:922178e5d286
--- a/main.cpp Fri May 22 07:33:50 2020 +0000
+++ b/main.cpp Fri Jun 05 08:47:26 2020 +0000
@@ -1,21 +1,21 @@
/* #include <mbed_events.h> */
#include "mbed.h"
-#include "AX12.h"
DigitalOut alivenessLED(LED1);
DigitalOut testLed(LED2);
Serial coach(D1, D0);
/* Serial coach(USBTX, USBRX); */
+Serial myserial_ax12(PC_4,PC_5,115200); // Modifier le baud rate en fonction de votre utilisation des moteurs
const float rapport_vitesse = 0.019383809*0.026315;
const float root3 = 1.7321;
-/*a=moteur de queue*/
+/*a=moteur de queue
AX12 myax12a (PC_4, PC_5, 2,115200);
-/*b : moteur de gauche*/
+b : moteur de gauche
AX12 myax12b (PC_4, PC_5, 1,115200);
-/*c : moteur de droite*/
-AX12 myax12c (PC_4, PC_5, 3,115200);
+c : moteur de droite
+AX12 myax12c (PC_4, PC_5, 3,115200);*/
int i = 0;
char last_read;
@@ -27,14 +27,136 @@
int flag_msg = 0;
int flag_a_traite = 0;
+
+DigitalOut my_led1(LED1);
+DigitalOut my_led2(LED2);
+DigitalOut my_led3(LED3);
+DigitalIn bp(USER_BUTTON);
+
+void SetCRSpeed(int ID, float speed);
+void SetMode(int ID, int mode);
+void SetCWLimit (int ID, int degrees);
+void SetCCWLimit (int ID, int degrees);
+void Write(int ID, int start, int bytes, char* data);
+
+char tata[10] = {};
+uint16_t position_1;
+uint16_t position_2;
+uint16_t position_3;
+int num_moteur = 1;
+int vitesse;
+char sens;
+int temps_ms = 0;
+//pour connaitre le nombre de tours
+int tour_1 = 0;
+int tour_2 = 0;
+int tour_3 = 0;
+
+Ticker ticker_1ms;
+
+uint16_t position_1_old;
+uint16_t position_2_old;
+uint16_t position_3_old;
+int16_t delta_position_1;
+int16_t delta_position_2;
+int16_t delta_position_3;
+
+//Définition des trames de requête
+char trame_demande_pos_1[8]= {0xFF,0xFF,0x01,0x4,0x2,0x24,2,0xD2};
+char trame_demande_pos_2[8]= {0xFF,0xFF,0x02,0x4,0x2,0x24,2,0xD1};
+char trame_demande_pos_3[8]= {0xFF,0xFF,0x03,0x4,0x2,0x24,2,0xD0};
+
+
+//Fonction d'interruption sur réception de caractère sur liaison série
+void Rx_Irq_fonction(void)
+{
+ uint16_t position_lu = 0;
+ my_led1 = !my_led1;
+
+ static int i_rx=0;
+ tata[i_rx]=myserial_ax12.getc();
+
+ if (tata[0]!=0xFF) // On vérifie que le premier et le deuxième octet sont biens 0xFF
+ i_rx=0;
+ else i_rx++;
+ if((i_rx==2)&&(tata[1] != 0xFF)) i_rx=0;
+
+//Une fois qu'on sait que la trame est "juste" on récupère le reste
+ if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7) {
+ my_led3 = !my_led3;
+ i_rx=0;
+
+ if(tata[4] == 0) { //si erreur est nulle
+ //On transforme les données en fonction de ce qu'on lit depuis les moteurs
+ position_lu = tata[5]+((uint16_t)tata[6]<<8);
+ if((0<= position_lu)&&(position_lu<=4095)) {
+ position_1 = position_lu;
+ }
+ }
+ for(int i=0; i<10; i++) {
+ tata[i]=0;
+ }
+ }
+
+ if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7) {
+ my_led3 = !my_led3;
+ i_rx=0;
+
+ if(tata[4]==00) { //si erreur nulle
+ //On transforme les données en fonction de ce qu'on lit depuis les moteurs
+ position_lu = tata[5]+((uint16_t)tata[6]<<8);
+ if((0<= position_lu)&&(position_lu<=4095)) {
+ position_2 = position_lu;
+ }
+
+ }
+ for(int i=0; i<10; i++) {
+ tata[i]=0;
+ }
+ }
+
+ if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7) {
+ my_led3 = !my_led3;
+ i_rx=0;
+
+ if(tata[4]==00) { //si erreur nulle
+ //On transforme les données en fonction de ce qu'on lit depuis les moteurs
+ position_lu = tata[5]+((uint16_t)tata[6]<<8);
+ if((0<= position_lu)&&(position_lu<=4095)) {
+ position_3 = position_lu;
+ }
+
+ }
+ for(int i=0; i<10; i++) {
+ tata[i]=0;
+ }
+ }
+
+ if (i_rx>9) {
+ for(int i=0; i<10; i++)
+ tata[i]=0;
+ i_rx=0;
+ }
+}
+
+void fonction_1ms(void)
+{
+ temps_ms++;
+}
+
+
+
void asser(float Vx, float Vy, float gammaz)
{
float wa = rapport_vitesse*(Vx + 100 * gammaz);
float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz);
float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz);
- myax12a.SetCRSpeed(wa);
- myax12b.SetCRSpeed(wb);
- myax12c.SetCRSpeed(wc);
+ /*moteur a = ID 2 */
+ SetCRSpeed(2,wa);
+ /*moteur b = ID 1*/
+ SetCRSpeed(1,wb);
+ /* moteur c = ID 3*/
+ SetCRSpeed(3,wc);
}
void send_stat()
@@ -42,6 +164,11 @@
coach.printf("S%f,%f,%f,%f,%f \n", front, lat, rot, T_front, T_lat );
}
+void send_roues()
+{
+ coach.printf("\n\r %5d ; %5d ; %5d ; %5d ; %5d ; %5d", position_1, delta_position_1, position_2, delta_position_2, position_3, delta_position_3);
+}
+
void mise_a_zero_msg_0()
{
for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
@@ -62,6 +189,8 @@
last_read = coach.getc();
if (last_read == 'S') {
send_stat();
+ } else if (last_read == 'R') {
+ send_roues();
} else if (last_read == ';') {
coach.puts("EOL\n");
/*coach.puts(message_0);
@@ -100,22 +229,256 @@
}
}
-void setup()
-{
- coach.baud(115200);
- myax12a.SetMode(1);
- myax12b.SetMode(1);
- myax12c.SetMode(1);
- coach.puts("stl\n\r");
- coach.attach(&callback_serial, Serial::RxIrq);
-}
int main()
{
- setup();
+ //myax12.SetMode(1); //passage en Continuous rotation
+
+ //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
+ //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5};
+ //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4};
+
+ //pour regarder les 3 moteurs les uns à la suite des autres
+ int etat=1;
+ int fm_etat=1;
+
+ SetMode(1, 1);
+ wait(0.1);
+ SetMode(2, 1);
+ wait(0.1);
+ SetMode(3, 1);
+
+ wait(0.1);
+
+ /* SetCRSpeed(1,0.05);
+ wait(0.1);
+ SetCRSpeed(2,0.05);
+ wait(0.1);
+ SetCRSpeed(3,0.05); */
+
+ char a;
+
+ //On passe le mode retour en 1 : retour seulement sur demande de lecture
+ a=1;
+ Write(2,16,1,&a);
+ Write(1,16,1,&a);
+ Write(3,16,1,&a);
+
+
+ //On passe le délai de retour à 250µs. C'est bien de le diminuer ensuite.
+ a=250;
+ Write(2,5,1,&a);
+ Write(1,5,1,&a);
+ Write(3,5,1,&a);
+
+ /* On coupe de le couple du moteur pour pouvoir le faire tourner à la main
+ a=0;
+ Write(2,24,1,&a);
+ Write(1,24,1,&a);
+ Write(3,24,1,&a); */
+
+ myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
+ ticker_1ms.attach(&fonction_1ms,0.001);
+ etat=2;
+
+ coach.baud(115200);
+ coach.puts("stl\n\r");
+ coach.attach(&callback_serial, Serial::RxIrq);
+
while (true) {
reception_com();
asser(front, lat, rot);
+
+ //on regarde la position des moteurs
+ switch(etat) {
+ case 2 :
+ if(fm_etat) {
+ position_1_old = position_1;
+ for(int i=0; i<=7; i++) {
+ //On envoi la trame que l'on souhaite
+
+ myserial_ax12.putc(trame_demande_pos_1[i]);
+ //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures
+ }
+ fm_etat=0;
+ }
+
+ if(temps_ms>=3) {
+ etat=3;
+ fm_etat=1;
+ }
+ break;
+
+ case 3 :
+
+ if(fm_etat) {
+ delta_position_1 = (int16_t)position_1 - (int16_t)position_1_old;
+ if(delta_position_1 > 2048) {
+ delta_position_1 = delta_position_1 - 4096;
+ }
+
+ if(delta_position_1 < -2048) {
+ delta_position_1 = delta_position_1 + 4096;
+ }
+ position_2_old = position_2;
+ for(int i=0; i<=7; i++) {
+ //On envoi la trame que l'on souhaite
+ myserial_ax12.putc(trame_demande_pos_2[i]);
+ //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures
+
+ }
+ fm_etat=0;
+ }
+
+ if(temps_ms>=6) {
+ etat=4;
+ fm_etat=1;
+ }
+ break;
+
+ case 4 :
+ if(fm_etat) {
+ delta_position_2 = (int16_t)position_2 - (int16_t)position_2_old;
+ if(delta_position_2 > 2048) {
+ delta_position_2 = delta_position_2 - 4096;
+ }
+
+ if(delta_position_2 < -2048) {
+ delta_position_2 = delta_position_2 + 4096;
+ }
+ position_3_old = position_3;
+ for(int i=0; i<=7; i++) {
+ //On envoi la trame que l'on souhaite
+ myserial_ax12.putc(trame_demande_pos_3[i]);
+
+ }
+ fm_etat=0;
+ }
+
+ if(temps_ms>=9) {
+ etat=5;
+ fm_etat=1;
+ }
+ break;
+
+ case 5 :
+
+ delta_position_3 = (int16_t)position_3 - (int16_t)position_3_old;
+ if(delta_position_3 > 2048) {
+ delta_position_3 = delta_position_3 - 4096;
+ }
+
+ if(delta_position_3 < -2048) {
+ delta_position_3 = delta_position_3 + 4096;
+ }
+ etat = 2;
+ fm_etat =1;
+ temps_ms = 0;
+ break;
+ }
}
}
+
+void SetCRSpeed(int ID, float speed)
+{
+
+ // bit 10 = direction, 0 = CCW, 1=CW
+ // bits 9-0 = Speed
+ char data[2];
+
+ int goal = (0x3ff * abs(speed));
+
+ // Set direction CW if we have a negative speed
+ if (speed < 0) {
+ goal |= (0x1 << 10);
+ }
+
+ data[0] = goal & 0xff; // bottom 8 bits
+ data[1] = goal >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ Write(ID, 0x20, 2, data);
+}
+
+void SetMode(int ID, int mode)
+{
+
+ if (mode == 1) { // set CR
+ SetCWLimit(ID,0);
+ SetCCWLimit(ID,0);
+ SetCRSpeed(ID,0.0);
+ } else {
+ SetCWLimit(ID,0);
+ SetCCWLimit(ID,360);
+ SetCRSpeed(ID,0.0);
+ }
+}
+
+void SetCWLimit (int ID, int degrees)
+{
+
+ char data[2];
+ short limit = (4095 * degrees) / 360;
+
+ data[0] = limit & 0xff; // bottom 8 bits
+ data[1] = limit >> 8; // top 8 bits
+
+ Write(ID, 0x06, 2, data);
+}
+
+void SetCCWLimit (int ID, int degrees)
+{
+
+ char data[2];
+ short limit = (4095 * degrees) / 360;
+
+ data[0] = limit & 0xff; // bottom 8 bits
+ data[1] = limit >> 8; // top 8 bits
+
+ Write(ID, 0x08, 2, data);
+}
+
+void Write(int ID, int start, int bytes, char* data)
+{
+ char TxBuf[16];
+ char sum = 0;
+
+ TxBuf[0] = 0xff;
+ TxBuf[1] = 0xff;
+
+ // ID
+ TxBuf[2] = ID;
+ sum += TxBuf[2];
+
+
+ // packet Length
+ TxBuf[3] = 3+bytes;
+ sum += TxBuf[3];
+
+ // Instruction
+ TxBuf[4]=0x03;
+ sum += TxBuf[4];
+
+ // Start Address
+ TxBuf[5] = start;
+ sum += TxBuf[5];
+
+ // data
+ for (char i=0; i<bytes ; i++) {
+ TxBuf[6+i] = data[i];
+ sum += TxBuf[6+i];
+
+ }
+
+ // checksum
+ TxBuf[6+bytes] = 0xFF - sum;
+
+ // Transmit the packet in one burst with no pausing
+ for (int i = 0; i < (7 + bytes) ; i++) {
+ myserial_ax12.putc(TxBuf[i]);
+ }
+
+ wait(0.00002);
+
+}
\ No newline at end of file
Leopold Clement
