Revision 10:e4e1228604fb, committed 2018-05-23

Comitter:

protongamer

Date:

Wed May 23 12:33:04 2018 +0000

Parent:

9:0f63d4cb5613

Commit message:

42

Changed in this revision

diff -r 0f63d4cb5613 -r e4e1228604fb main.cpp
--- 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)
          

diff -r 0f63d4cb5613 -r e4e1228604fb parameters.h
--- a/parameters.h	Fri May 18 09:03:25 2018 +0000
+++ b/parameters.h	Wed May 23 12:33:04 2018 +0000
@@ -1,49 +1,14 @@
-/*
-Here is the list of the CAN Codes to sent to the motor driver
-There identifiers and word code
-These parameters can be changed
-
-Here is the default values
+//Parameters of the robot
+//includes CANopen codes and robot parameters
 
 
-//Identifier Codes
-
-#define INITOP    0x00 
-#define RPDO1_R   0x215
-#define RPDO2_R   0x315
-#define RPDO3_R   0x415
-#define RPDO4_R   0x515
-#define TPDO1_R   0x195
-#define TPDO2_R   0x295
-#define TPDO3_R   0x395
-#define TPDO4_R   0x495
-#define RPDO1_L   0x220
-#define RPDO2_L   0x320
-#define RPDO3_L   0x420
-#define RPDO4_L   0x520
-#define TPDO1_L   0x1A0
-#define TPDO2_L   0x2A0
-#define TPDO3_L   0x3A0
-#define TPDO4_L   0x4A0
-
-
-
-//Word codes
-
-//Driver adresses
-#define DRIVER11 0x01
-#define DRIVER12 0x15
-#define DRIVER21 0x01
-#define DRIVER22 0x20
-
-
-
+/*
+Here is the list of the CANopen Codes to sent to the motor driver
+There identifiers and word code
+These parameters can be changed
 */
 
 
-
-
-
 //Identifier Codes
 
 /*
@@ -79,9 +44,11 @@
 
 
 
-//Word codes
+
 
-//Driver adresses
+//Driver adresses for operational mode
+//Example : DRIVER_R1 = 0x01  DRIVER_R2= 0x15  ---->  Code must be sent = DRIVER_R1 + DRIVER_R2 = 0x0115
+
 #define DRIVER_R1 0x01
 #define DRIVER_R2 0x15
 #define DRIVER_L1 0x01
@@ -92,14 +59,97 @@
 
 
 //Robot setup
-/*
-Here is the configuration of the robot
-*/
+
+//Here is the configuration of the robot
+//These value can be changed
+
 
 //Length from motors to cursor in millimeters(mm) when the origin is taken
+
 //Left motor to cursor
 #define LENGTH_L1_PO 781
+
 //Right motor to cursor
 #define LENGTH_L2_PO 781
+
 //Left motor to Right motor
 #define LENGTH_MOTOR 1000
+
+//Max velocity for normal mode
+#define ALPHA   425000 
+
+//Number of counters for 1 millimeters
+#define REFERENCE 2545
+
+
+
+
+/*Here is the default values
+
+
+//Identifier Codes
+
+#define INITOP    0x00 
+#define RPDO1_R   0x215
+#define RPDO2_R   0x315
+#define RPDO3_R   0x415
+#define RPDO4_R   0x515
+#define TPDO1_R   0x195
+#define TPDO2_R   0x295
+#define TPDO3_R   0x395
+#define TPDO4_R   0x495
+#define RPDO1_L   0x220
+#define RPDO2_L   0x320
+#define RPDO3_L   0x420
+#define RPDO4_L   0x520
+#define TPDO1_L   0x1A0
+#define TPDO2_L   0x2A0
+#define TPDO3_L   0x3A0
+#define TPDO4_L   0x4A0
+
+
+//Driver adresses for operational mode
+//Example : DRIVER_R1 = 0x01  DRIVER_R2= 0x15  ---->  Code must be sent = DRIVER_R1 + DRIVER_R2 = 0x0115
+
+#define DRIVER11 0x01
+#define DRIVER12 0x15
+#define DRIVER21 0x01
+#define DRIVER22 0x20
+
+
+
+//Robot setup
+
+//Here is the configuration of the robot
+//These value can be changed
+
+
+//Length from motors to cursor in millimeters(mm) when the origin is taken
+
+//Left motor to cursor
+#define LENGTH_L1_PO 781
+
+//Right motor to cursor
+#define LENGTH_L2_PO 781
+
+//Left motor to Right motor
+#define LENGTH_MOTOR 1000
+
+//Max velocity for normal mode
+#define ALPHA   425000 
+
+//Number of counters for 1 millimeters
+#define REFERENCE 2545
+
+
+
+
+*/
+
+
+
+
+
+
+
+