Diff: main.cpp

Revision:

10:e4e1228604fb

Parent:

9:0f63d4cb5613

--- a/main.cpp	Fri May 18 09:03:25 2018 +0000
+++ b/main.cpp	Wed May 23 12:33:04 2018 +0000
@@ -10,15 +10,14 @@
  #define DEGRAD   0
  #define NORMAL   1
  
- //#define ALPHA   425000
- #define ALPHA   425000
+
+
 
-DigitalOut MOTOR1_OP(LED1);
-DigitalOut MOTOR2_OP(LED2);
-DigitalOut DEB_MOD_DEG(LED3);
-DigitalOut DEB_MOD_NOR(LED4);
+DigitalOut MOTOR_L_COM(LED1);
+DigitalOut MOTOR_R_COM(LED3);
 DigitalOut MOD_DEG(p25);
 DigitalOut MOD_NOR(p26);
+DigitalOut CYCLE_TIME(p20);
 DigitalIn SWH(p5);
 DigitalIn SWV(p6);
 DigitalIn BPO(p7);
@@ -28,7 +27,7 @@
 
 CAN can2(p30, p29, 1000000);    // on definit pin et debit
 CANMessage msg;
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX, 115200);
 
 uint8_t mode = 0;
 int x,y,ctr_x,ctr_y; // Variables de joystick
@@ -67,17 +66,15 @@
 
 void send(int id, char octet_emis[], char RouD, char longueur )
 {
-    int emis_ok = 0 ;
+ 
     
-    MOTOR1_OP = 1;
-    MOTOR1_OP = 0;
     
     if (RouD == 'D')
-    { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ;
+    {  can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ;
       //pc.printf("id: %x, %c L = %d,  \nData : %x:%x:%x:%x ...   \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] );
     }// c'ets la valeur retournée par la fonction write
    else
-    { emis_ok = can2.write(CANMessage(id, octet_emis, longueur,  CANRemote, CANStandard ));
+    {  can2.write(CANMessage(id, octet_emis, longueur,  CANRemote, CANStandard ));
         //pc.printf("id: %x, %c L = %d,  \nData : %x:%x:%x:%x ...   \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] );
          }
     //lcd.locate(0,10);
@@ -110,7 +107,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
@@ -120,19 +117,19 @@
         MOD_NOR = 1;
         if(can2.read(msg))
         {
-            for(int i = 0; i < msg.len; i++)
-            {
-                pc.printf("0x%x ",msg.data[i]);
-                ping = 1;
-            }//end of for
-            pc.printf("\r \n");
-            }//can.read(msg)
-            wait(0.1);
-            MOD_NOR = 0;
-            wait(0.9);
-        }
+        for(int i = 0; i < msg.len; i++){
+        pc.printf("0x%x ",msg.data[i]);
+        ping = 1;
+        MOTOR_R_COM = 1;
+        }//end of for
+        pc.printf("\r \n");
+        }//can.read(msg)
+        wait(0.1);
+        MOD_NOR = 0;
+        wait(0.9);
+    }
         
-        ping =0;
+        ping = 0;
         command[0] = DRIVER_L1;
         command[1] = DRIVER_L2;
         
@@ -145,6 +142,7 @@
             for(int i = 0; i < msg.len; i++){
             pc.printf("0x%x ",msg.data[i]);
             ping = 1;
+            MOTOR_L_COM = 1;
             }//end of for
             pc.printf("\r \n");
             }//can.read(msg)
@@ -157,24 +155,6 @@
 
 
 void read_position(){
-    
-        //timer_process_read++;//var used  to set time for reading value
-        l1 = LENGTH_L1_PO - (p1/2545); //Length 1
-        l2 = LENGTH_L2_PO - (p2/2545); //Length 2
-        lx = LENGTH_MOTOR;
-        dx = (((double) l1*(double) l1) - ((double) l2*(double) l2) + ((double) lx*(double) lx))/(2*(double) lx);
-        dy = sqrt(((double) l1*(double) l1) - (dx*dx));
-        
-        if(BPO == 0){                       
-
-         p_pom = p_actual;
-         p_pom2 = p_actual2;
-         mode = !mode; //Change mode
-         wait(1.0);
-        }  
-    
-        p1 = p_pom - p_actual;
-        p2 = p_pom2 - p_actual2;
             
 //Left motor      
         send(TPDO1_L, command, 'R', 0); //Ask position for second length
@@ -186,9 +166,10 @@
         p_actual = p_actual | msg.data[1];
         p_actual = p_actual << 8;
         p_actual = p_actual | msg.data[0];
+        MOTOR_L_COM = 1;
         }
         //timer_process_read = 1;
-        wait(0.005);
+        wait(0.01);
         send(TPDO1_R, command, 'R', 0); //Ask position for first length
         if(can2.read(msg)){
         p_actual2 = msg.data[3];
@@ -198,8 +179,28 @@
         p_actual2 = p_actual2 | msg.data[1];
         p_actual2 = p_actual2 << 8;
         p_actual2 = p_actual2 | msg.data[0];
+        MOTOR_R_COM = 1;
         }
-
+        
+        
+        if(BPO == 0){                      
+         p_pom = p_actual; //Refresh first origin value
+         p_pom2 = p_actual2; //Refresh second origin value
+         mode = !mode; //Change mode 
+         wait(1.0);
+        }  
+        
+        //Calculate positions values in counters
+        p1 = p_pom - p_actual; 
+        p2 = p_pom2 - p_actual2;
+        
+        
+        //Calculate Coordinates
+        l1 = LENGTH_L1_PO - (p1/REFERENCE); //Length 1
+        l2 = LENGTH_L2_PO - (p2/REFERENCE); //Length 2
+        lx = LENGTH_MOTOR;
+        dx = (((double) l1*(double) l1) - ((double) l2*(double) l2) + ((double) lx*(double) lx))/(2*(double) lx);
+        dy = sqrt(((double) l1*(double) l1) - (dx*dx));
     
     }//fin de fonction
 
@@ -215,7 +216,7 @@
     if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;}       //  Detection y avec correction ctr
     float VX;    // Calcule brut vélocité x, y
     float VY;       
-    long int VM_L;            // Calcule des vélocités exactes de chaques moteurs
+    long int VM_L;           
     long int VM_R;
     
     if(mode ==0)
@@ -244,9 +245,11 @@
     ///////////// MODE NORMAL //////////////////////////////////////
     VX = x;
     VY = y;
+    //Left control
     VM_L = ((dx*VX)-(dy*VY));
     VM_L = (VM_L/l1);
     VM_L = VM_L*(ALPHA/42);
+    //Right control
     VM_R = ((-(lx-dx)*VX)-(dy*VY));    
     VM_R = (VM_R/l2); 
     VM_R = VM_R*(ALPHA/42);  
@@ -276,9 +279,6 @@
     BPO.mode(PullUp);
         
     initialisation();   // Mise en mode opérationnel des moteurs
-
-
-
                 
     while(1)
     {
@@ -291,10 +291,14 @@
             pc.printf("\t VM_L = %f", pVx);
             pc.printf("\t VM_R = %f \r\n", pVy);
 
-            
+        CYCLE_TIME = 0;
+        MOTOR_L_COM = 0;
+        MOTOR_R_COM = 0;
         read_position();
         control_Moteur();   // Controle du meteur mode degradé et normal
         display_mode();     // Controle des led affichant le mode
+            
+        CYCLE_TIME = 1;
         
     }// fin while(1)