Diff: main.cpp

Revision:

6:ffffa9dfadfc

Parent:

5:d5a021bbe81b

Child:

7:e87feff62bfd

diff -r d5a021bbe81b -r ffffa9dfadfc main.cpp
--- 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