version 2.5(pour le grand FEARHEAVEN) [spoil] oui [/spoil]
Dependencies: mbed
Fork of Robot_a_cables_v2_5 by
Diff: main.cpp
- Revision:
- 6:ffffa9dfadfc
- Parent:
- 5:d5a021bbe81b
- Child:
- 7:e87feff62bfd
--- a/main.cpp Mon Apr 30 12:27:56 2018 +0000 +++ b/main.cpp Mon May 07 12:12:34 2018 +0000 @@ -1,5 +1,5 @@ #include "mbed.h" -#include "can_parameters.h" +#include "parameters.h" /* Projet BTS SN - Robot à câbles - Lycée Georges Cabanis Enzo Niro - Erwin Sazio @@ -32,10 +32,20 @@ uint8_t done = 0; char command[8]; //word to send command uint64_t timer_process_read; //counter to make mbed read actual position -uint64_t timer_process_ping; //counter to make mbed read actual position -float dy = 525000;//valeur à la POM -int64_t d_old, d_new; -bool first_time = 0; +float dy = 1262, dx = 492.5;//coordinates values when origin is done +//these positions are in counters +int32_t p1, p2; //motors positions +int32_t p_actual, p_pom; //p_actual position read by can, p_pom position done when origin is made + +//longueur câbles +//lx = longueur entre les point des moteurs +//l1 = longueur moteur gauche à l'effecteur +//l2 = longueur moteur droite à l'effecteur +int64_t lx, l1, l2; + + + + void display_mode() { @@ -96,7 +106,7 @@ MOD_NOR = 0; wait(0.1); - command[0] = DRIVER_R1; + command[0] = DRIVER_R1; command[1] = DRIVER_R2; while(ping == 0) // INIT MOTEUR DROIT @@ -140,6 +150,34 @@ } } // fin initialisation + + +void read_position(){ + + + + + + /* send(TPDO1_R, command, 'R', 0); //Ask position + timer_process_read = 0; + p2 = msg.data[3]; + p2 = p2 << 8; + p2 = p2 | msg.data[2]; + p2 = p2 << 8; + p2 = p2 | msg.data[1]; + p2 = p2 << 8; + p2 = p2 | msg.data[0];*/ + + + + lx = 985; + + //l2 = LENGTH_L2_PO + (p2*1000)/2500; + + }//fin de fonction + + + void control_Moteur() { ///////////// CONTROLE MOTEUR ////////////////////////////////////////////////////////////////////////////// @@ -186,32 +224,30 @@ initialisation(); // Mise en mode opérationnel des moteurs - for(int working = 0; working < 50; working++) - { - send(TPDO1_R, command, 'R', 0); //Ask position - timer_process_read = 0; - d_new = msg.data[3]; - d_new = d_new << 8; - d_new = d_new | msg.data[2]; - d_new = d_new << 8; - d_new = d_new | msg.data[1]; - d_new = d_new << 8; - d_new = d_new | msg.data[0]; - pc.printf("%d \r\n",d_new); - } - d_old = d_new; + + while(1) { //timer_process_ping++; timer_process_read++; - pc.printf("%d \t",d_new); - pc.printf("%d \t",d_old); - //pc.printf("%d \t",d_new); - pc.printf("%f \r\n",dy); + l1 = (LENGTH_L1_PO*1000) - (p1*1000)/2500; //Longueur 1 + + if(BPO == 0){ + send(TPDO1_R, command, 'R', 0); //Ask position + + p_pom = msg.data[3]; + p_pom = p_pom << 8; + p_pom = p_pom | msg.data[2]; + p_pom = p_pom << 8; + p_pom = p_pom | msg.data[1]; + p_pom = p_pom << 8; + p_pom = p_pom | msg.data[0]; + wait(1.0); + } - dy = 525000 + (d_new*1000)/2500; - + //dy = 525000 + (d_new*1000)/2500; + //wait_ms(1); @@ -247,32 +283,34 @@ } }*/ - + pc.printf("%d", l1); + pc.printf("\t %d", p_pom); + pc.printf("\t %d", p_actual); + pc.printf("\t %d \r\n", p1); if(timer_process_read >= 10){ - send(TPDO1_R, command, 'R', 0); //Ask position + send(TPDO1_R, command, 'R', 0); //Ask position timer_process_read = 0; - d_new = msg.data[3]; - d_new = d_new << 8; - d_new = d_new | msg.data[2]; - d_new = d_new << 8; - d_new = d_new | msg.data[1]; - d_new = d_new << 8; - d_new = d_new | msg.data[0]; - - /* if(first_time == 0){ - d_old = d_new; - first_time = 1; - }*/ + p_actual = msg.data[3]; + p_actual = p_actual << 8; + p_actual = p_actual | msg.data[2]; + p_actual = p_actual << 8; + p_actual = p_actual | msg.data[1]; + p_actual = p_actual << 8; + p_actual = p_actual | msg.data[0]; + } + + p1 = p_pom - p_actual; if(can2.read(msg)) { for(int i = 0; i < msg.len; i++){ - pc.printf("0x%x ",msg.data[i]); + //pc.printf("0x%x ",msg.data[i]); }//end of for - pc.printf("\r \n"); + //pc.printf("\r \n"); }//can.read(msg) + //read_position(); control_Moteur(); // Controle du meteur mode degradé et normal display_mode(); // Controle des led affichant le mode