Jeanne Le Soudeer
/
lecture_position
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.
Revision 14:5374abce265d, committed 2020-06-04
- Comitter:
- jlesoudeer
- Date:
- Thu Jun 04 09:31:38 2020 +0000
- Parent:
- 13:a2c52e2a8104
- Commit message:
- test lecture des positions
Changed in this revision
| MX12_ENS.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r a2c52e2a8104 -r 5374abce265d MX12_ENS.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MX12_ENS.lib Thu Jun 04 09:31:38 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ajuton/code/MX12_ENS/#6bd931ef8d58
diff -r a2c52e2a8104 -r 5374abce265d main.cpp
--- a/main.cpp Fri May 15 09:52:46 2020 +0000
+++ b/main.cpp Thu Jun 04 09:31:38 2020 +0000
@@ -1,14 +1,15 @@
#include "mbed.h"
//AX12 myax12 (PC_4, PC_5, 1,115200);
-Serial myserial_ax12(PC_4,PC_5,1000000); // Modifier le baud rate en fonction de votre utilisation des moteurs
+Serial myserial_ax12(PC_4,PC_5,115200); // Modifier le baud rate en fonction de votre utilisation des moteurs
Serial pc(USBTX,USBRX,115200);
-//Serial zigBee(PC_1,PC_0,115200);
+//Serial zigBee(PC_1,PC_0,115200);
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);
@@ -23,179 +24,242 @@
int num_moteur = 1;
int vitesse;
char sens;
-
-Ticker ticker_10ms;
+int temps_ms = 0;
+//pour connaitre le nombre de tours
+int tour_1 = 0;
+int tour_2 = 0;
+int tour_3 = 0;
-//Définition des trames de requête pour le moteur 1
-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};
-char trame_demande_vitesse[8]={0xFF,0xFF,0x01,0x4,0x2,0x28,2,0xCE};
+Ticker ticker_1ms;
+
+//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};
-#define acq_vitesse 0
-#define acq_position 1
-
-//Fonction d'interruption sur réception de charactère sur liaison série
+//Fonction d'interruption sur réception de caractère sur liaison série
void Rx_Irq_fonction(void)
{
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)
- {
+ 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
+
+ if(tata[4] == 0) { //si erreur est nulle
//On transforme les données en fonction de ce qu'on lit depuis les moteurs
- if (acq_position == 1) position_1 = tata[5]+((uint16_t)tata[6]<<8);
- /*if (acq_vitesse == 1){
- sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
- vitesse = tata[5]+((uint16_t)tata[6]<<8);
- }*/
+ position_1 = tata[5]+((uint16_t)tata[6]<<8);
}
- tata[0]=0;
+ for(int i=0;i<10;i++)
+ {tata[i]=0;}
}
-
- if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7)
- {
+
+ 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
+
+ if(tata[4]==00) { //si erreur nulle
//On transforme les données en fonction de ce qu'on lit depuis les moteurs
- if (acq_position == 1) position_2 = tata[5]+((uint16_t)tata[6]<<8);
- /*if (acq_vitesse == 1){
- sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
- vitesse = tata[5]+((uint16_t)tata[6]<<8);
- }*/
+ position_2 = tata[5]+((uint16_t)tata[6]<<8);
+
}
- tata[0]=0;
+ for(int i=0;i<10;i++)
+ {tata[i]=0;}
}
-
- if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7)
- {
+
+ 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
+
+ if(tata[4]==00) { //si erreur nulle
//On transforme les données en fonction de ce qu'on lit depuis les moteurs
- if (acq_position == 1) position_3 = tata[5]+((uint16_t)tata[6]<<8);
- /*if (acq_vitesse == 1){
- sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
- vitesse = tata[5]+((uint16_t)tata[6]<<8);
- }*/
+ position_3 = tata[5]+((uint16_t)tata[6]<<8);
+
}
- tata[0]=0;
+ for(int i=0;i<10;i++)
+ {tata[i]=0;}
}
-
- if (i_rx>9)
- {
- /*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_10ms(void)
+void fonction_1ms(void)
{
- /*for(int i=0;i<=19;i++)
- {
- pc.printf("%X ",tata[i]);
- }
- pc.printf("\n\r");*/
-
- for(int i=0;i<=7;i++)
- {
- //On envoi la trame que l'on souhaite
- if(num_moteur == 1)myserial_ax12.putc(trame_demande_pos_1[i]);
- if(num_moteur == 2)myserial_ax12.putc(trame_demande_pos_2[i]);
- if(num_moteur == 3)myserial_ax12.putc(trame_demande_pos_3[i]);
- /*if(acq_vitesse){
- myserial_ax12.putc(trame_demande_vitesse[i]);
- //pc.printf("\n\rvitesse = %d\n\rsens = %d",vitesse,sens);
- }*/
- }
- num_moteur++;
- if(num_moteur > 3)num_moteur = 1;
+ temps_ms++;
}
-int main(){
+
+int main()
+{
//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};
- float Angle;
- short speed;
- char data[10];
- char s=0;
- float temp;
- int position_old_1=0,i,a;
- int position_old_2=0;
- int position_old_3=0;
+
+ //pour qu'il s'arrete
+ uint8_t car_recu=0;
+
+ uint8_t bp_lu;
+ //pour regarder les 3 moteurs les uns à la suite des autres
+ int etat=1;
+ int fm_etat=1;
+ int position_1_old;
+ int position_2_old;
+ int position_3_old;
pc.printf("test lecture AX12\n\r");
-
+
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);
-
+
myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
- ticker_10ms.attach(&fonction_10ms,1);
-
+ ticker_1ms.attach(&fonction_1ms,0.001);
+ etat=2;
while(1) {
-
-
- if (position_1 != position_old_1)
- {
- pc.printf("\n\rposition moteur 1 = %d :", position_1);
- //pc.printf("\n\rposition moteur 1 = %d\n\r mot recu :",position_1);
+
+ //commande des moteurs (pour les tests)
+ time_t seconds = time(NULL);
+ bp_lu = bp.read();
+ if(pc.readable()) {
+ car_recu=pc.getc();
+ switch(car_recu) {
+ case 's' :
+ SetCRSpeed(1,0);
+ SetCRSpeed(2,0);
+ SetCRSpeed(3,0);
+ break;
+ case 'a' :
+ SetCRSpeed(1,0.05);
+ wait(0.1);
+ SetCRSpeed(2,0.05);
+ wait(0.1);
+ SetCRSpeed(3,0.05);
- /*for(i=0;i<=7;i++)
- {
- pc.printf("%X ",tata[i]);
- }*/
- position_old_1 = position_1;
+ }
}
- if (position_2 != position_old_2)
- {
- pc.printf("\n\rposition moteur 2 = %d :", position_2);
- position_old_2 = position_2;
- }
- if (position_3 != position_old_3)
- {
- pc.printf("\n\rposition moteur 3 = %d :", position_3);
-
- position_old_3 = position_3;
- }
- }
-
+
+
+ //on regarde la position des moteurs
+ switch(etat) {
+ case 2 :
+ if(fm_etat) {
+ position_1_old = position_1;
+ for (int i=0; i<=9; i++){
+ pc.printf("%x ", tata[i]);
+ }
+ 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
+ }
+ if(position_1_old> position_1) {
+ tour_1++;
+ }
+
+ fm_etat=0;
+ }
+
+ if(temps_ms>=1000) {
+ etat=3;
+ fm_etat=1;
+ }
+ break;
+
+ case 3 :
+
+ if(fm_etat) {
+ 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
+
+ }
+ if(position_2_old > position_2) {
+ tour_2++;
+ }
+ fm_etat=0;
+ }
+
+ if(temps_ms>=1000) {
+ etat=4;
+ fm_etat=1;
+ }
+ break;
+
+ case 4 :
+ if(fm_etat) {
+ 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]);
+
+ }
+ if(position_3_old > position_3) {
+ tour_3++;
+ }
+ fm_etat=0;
+ }
+
+ if(temps_ms>=1000) {
+ etat=5;
+ fm_etat=1;
+ }
+ break;
+
+ case 5 :
+
+ pc.printf("\n\rposition moteur 1 = %d :", position_1);
+ pc.printf("\n\rtour moteur 1 = %d :", tour_1);
+ pc.printf("\n\rposition moteur 2 = %d :", position_2);
+ pc.printf("\n\rtour moteur 2 = %d :", tour_2);
+ pc.printf("\n\rposition moteur 3 = %d :", position_3);
+ pc.printf("\n\rtour moteur 3 = %d :", tour_3);
+ etat = 2;
+ fm_etat =1;
+ temps_ms = 0;
+ break;
+ }
+
+
+
+
+ }
+
}
-void SetCRSpeed(int ID, float speed) {
+void SetCRSpeed(int ID, float speed)
+{
// bit 10 = direction, 0 = CCW, 1=CW
// bits 9-0 = Speed
@@ -215,7 +279,8 @@
Write(ID, 0x20, 2, data);
}
-void SetMode(int ID, int mode) {
+void SetMode(int ID, int mode)
+{
if (mode == 1) { // set CR
SetCWLimit(ID,0);
@@ -228,7 +293,8 @@
}
}
-void SetCWLimit (int ID, int degrees) {
+void SetCWLimit (int ID, int degrees)
+{
char data[2];
short limit = (4095 * degrees) / 360;
@@ -239,18 +305,20 @@
Write(ID, 0x06, 2, data);
}
-void SetCCWLimit (int ID, int degrees) {
-
+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){
+void Write(int ID, int start, int bytes, char* data)
+{
char TxBuf[16];
char sum = 0;
@@ -288,88 +356,8 @@
for (int i = 0; i < (7 + bytes) ; i++) {
myserial_ax12.putc(TxBuf[i]);
}
-
+
wait(0.00002);
}
-//void set_speed(ID,speed)
-//void demande_lecture_pos(ID)
-
-
-/*
-#include "mbed.h"
-
-//AX12 myax12 (PC_4, PC_5, 1,115200);
-Serial myserial_ax12(PC_4,PC_5,115200);
-char tata[10];
-int position;
-
-void Rx_Irq_fonction(void)
-{
- static int i_rx;
- tata[i_rx]=myserial_ax12.getc();
- if (tata[0]!=0xFF)
- i_rx=0;
- else i_rx++;
- if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7)
- {
- i_rx=0;
- if(tata[4]==00) //si erreur est nulle
- position = tata[5]+((uint16_t)tata[6]<<8);
- tata[0]=0;
- }
- if (i_rx>9)
- {
- for(int i=0;i<10;i++)
- tata[i]=0;
- i_rx=0;
- }
-}
-
-int main(){
- //myax12.SetMode(1); //passage en Continuous rotation
- char trame_demande_pos[8]={0xFF,0xFF,0x01,0x4,0x2,0x24,2,0xD2};
- char trame_cmd_speed[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
- float Angle;
- short speed;
- char data[10];
- char s=0;
- float temp;
- int position_old=0,i,a;
- //myax12.SetCRSpeed(0.05);
-
- for(i=0;i<=8;i++)
- {
- myserial_ax12.putc(trame_cmd_speed[i]);
- }
-
- for(i=0;i<=7;i++)
- {
- myserial_ax12.putc(trame_demande_pos[i]);
- }
- myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
-
-
- while(1) {
- if (position != position_old)
- {
- printf("\n\rposition = %d\n\r mot recu :",position);
- for(i=0;i<=7;i++)
- {
- printf("%X ",tata[i]);
- }
- position_old=position;
- }
- wait(0.01);
- a++;
- if(!(a % 20))
- for(i=0;i<=7;i++)
- {
- myserial_ax12.putc(trame_demande_pos[i]);
- }
- }
-
-}
-
-void set_speed(ID,speed)*/
\ No newline at end of file