Diff: main.cpp

Revision:

5:d5a021bbe81b

Parent:

4:3fd7c53d31c1

Child:

6:ffffa9dfadfc

--- a/main.cpp	Mon Apr 23 08:44:10 2018 +0000
+++ b/main.cpp	Mon Apr 30 12:27:56 2018 +0000
@@ -9,9 +9,6 @@
  
  
 
-
-
-
 DigitalOut MOTOR1_OP(LED1);
 DigitalOut MOTOR2_OP(LED2);
 DigitalOut DEB_MOD_DEG(LED3);
@@ -21,14 +18,38 @@
 DigitalIn SWH(p5);
 DigitalIn SWV(p6);
 DigitalIn BPO(p7);
+AnalogIn joystick_ctr(p15);   // Pin 2 (fil vert) Center Tap Reference
+AnalogIn joystick_x(p16);     // Pin 4 (fil jaune)
+AnalogIn joystick_y(p17);     // Pin 5 (fil bleu)
 
-
+CAN can2(p30, p29, 1000000);    // on definit pin et debit
+CANMessage msg;
+Serial pc(USBTX, USBRX);
 
-CAN can1(p9, p10,1000000);// on definit pin et debit
-CAN can2(p30, p29, 1000000);
-Serial pc(USBTX, USBRX);
-//AnalogIn   pot_1(p19); // potard pour selection
+uint8_t mode = 0;
+int x,y,ctr_x,ctr_y; // Variables de joystick
+uint8_t ping = 0; //variable pour etat de processus
+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;
 
+void display_mode()
+{
+    if(mode == 0)   // MODE DEGRADE
+    {
+        MOD_DEG = 1;
+        MOD_NOR = 0;
+    }
+    else if(mode == 1) // MODE NORMAL
+    {
+        MOD_DEG = 0;
+        MOD_NOR = 1;
+    }
+}
 
 void send(int id, char octet_emis[], char RouD, char longueur )
 {
@@ -53,49 +74,59 @@
     } */
 }
 
-
-
-
-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;
- 
-int main() {
-    
-    
-    CANMessage msg;
-    uint8_t ping = 0; //variable pour etat de processus
-    uint8_t done = 0;
-    char command[8]; //word to send command
-    
-    
-        //define Pullup switch
-        SWH.mode(PullUp);
-        SWV.mode(PullUp);
-        BPO.mode(PullUp);
-        pc.printf("starting...\r\n");
+void initialisation()
+{
+    pc.printf("starting...\r\n");
+    MOD_DEG = 1;
+    MOD_NOR = 0;
+    wait(0.1);
+    MOD_DEG = 0;
+    MOD_NOR = 1;
+    wait(0.1);
+    MOD_DEG = 1;
+    MOD_NOR = 0;
+    wait(0.1);
+    MOD_DEG = 0;
+    MOD_NOR = 1;
+    wait(0.1);
+    MOD_DEG = 1;
+    MOD_NOR = 0;
+    wait(0.1);
+    MOD_DEG = 0;
+    MOD_NOR = 0;
+    wait(0.1);
+        
+    command[0] = DRIVER_R1;
+    command[1] = DRIVER_R2;
+        
+    while(ping == 0)    // INIT MOTEUR DROIT
+    {
+        pc.printf("ping droite ...\r\n");
+        send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
+        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);
+        }
+        
+        ping =0;
+        command[0] = DRIVER_L1;
+        command[1] = DRIVER_L2;
+        
+        while(ping == 0)    // INIT MOTEUR GAUCHE
+        {
+            pc.printf("ping gauche ...\r\n");
+            send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
             MOD_DEG = 1;
-            wait(0.1);
-            MOD_DEG = 0;
-            wait(0.1);
-            MOD_DEG = 1;
-            wait(0.1);
-            MOD_DEG = 0;
-            wait(0.1);
-            MOD_DEG = 1;
-            wait(0.1);
-            MOD_DEG = 0;
-            wait(0.1);
-        
-        command[0] = DRIVER_R1;
-        command[1] = DRIVER_R2;
-        
-        while(ping == 0){
-            pc.printf("ping...\r\n");
-            send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
-            MOD_NOR = 1;
             if(can2.read(msg)) {
             for(int i = 0; i < msg.len; i++){
             pc.printf("0x%x ",msg.data[i]);
@@ -104,41 +135,87 @@
             pc.printf("\r \n");
             }//can.read(msg)
             wait(0.1);
-            MOD_NOR = 0;
+            MOD_DEG = 0;
             wait(0.9);
+        }
+}   // fin initialisation
+
+void control_Moteur()
+{
+    ///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
+    // LECTURE Joystick + stockage valeurs dans x, y
+    ctr_x = int(-((joystick_ctr.read()*100)-50));
+    ctr_y = int((joystick_ctr.read()*100)-50);
+    if(SWH){x = int(-((joystick_x.read()*100)-50))+ctr_x;}else{x=0;}    //  Detection x avec correction ctr de -42 à +42
+    if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;}       //  Detection y avec correction ctr
+        
+    if(mode ==0)
+    {
+    ///////////// MODE DEGRADE /////////////////////////////////////
+    float VX = (float(x)/42)*425000;    // Calcule brut vélocité x, y
+    float VY = (float(y)/42)*425000;    
+    long int VM_L = VX - VY;            // Calcule des vélocités exactes de chaques moteurs
+    long int VM_R = -VX - VY;
+        
+    command[0]= (VM_L & 0x000000FF);        // Masque + décalage pour translation big vers little endian
+    command[1]= (VM_L & 0x0000FF00)>>8;
+    command[2]= (VM_L & 0x00FF0000)>>16;
+    command[3]= (VM_L & 0xFF000000)>>24;
+    send(RPDO1_L, command, 'D', 4);         // Controle moteur gauche
+        
+    command[0]= (VM_R & 0x000000FF);
+    command[1]= (VM_R & 0x0000FF00)>>8;
+    command[2]= (VM_R & 0x00FF0000)>>16;
+    command[3]= (VM_R & 0xFF000000)>>24;
+    send(RPDO1_R, command, 'D', 4);         // Controle moteur droit
+    ////////////////////////////////////////////////////////////////
+    }
+    else if(mode ==1)
+    {
+        // MODE NORMAL
+    }
+    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+}
+ 
+int main() {
+    
+    //define Pullup switch
+    SWH.mode(PullUp);
+    SWV.mode(PullUp);
+    BPO.mode(PullUp);
+        
+    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);
             
-            }
-            MOD_NOR = 1;
-            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);
-            
-            dy = 525000 + (d_new*1000)/2500;
+        dy = 525000 + (d_new*1000)/2500;
             
                 
-            //wait_ms(1);
+        //wait_ms(1);
             
-          /*  if(timer_process_ping >= 5000){
+        /*  if(timer_process_ping >= 5000){
                 send(TPDO2_R, command, 'R', 0); //Ask mod of motor
                 timer_process_ping = 0;
                 if(can2.read(msg)) {
@@ -195,52 +272,10 @@
             }//end of for
             pc.printf("\r \n");
             }//can.read(msg)
-
             
-        switch(BPO){
-            case 0:
-            if(done == 0){
-            timer_process_read = 50;
-            //timer_process_ping = 0;
-            switch(SWV){
-                case 0:
-                command[0] = 0x00;
-                command[1] = 0x8F;
-                command[2] = 0x00;
-                command[3] = 0x00;
-                send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
-                break;
-                
-                case 1:
-                command[0] = 0xFF;
-                command[1] = 0x70;
-                command[2] = 0xFF;
-                command[3] = 0xFF;
-                send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
-       
-
-                break;
-                }//end of switch SWV
-                done = 1;
-                }
-            
-            break;
-            
-            case 1:
-                if(done == 1){
-                command[0] = 0x00;
-                command[1] = 0x00;
-                command[2] = 0x00;
-                command[3] = 0x00;
-                send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
-                done = 0;
-                //send(TPDO1_R, command, 'R', 0); //send word to put pluto driver in operational mode
-                }
-            break;
-                 
-                }
+        control_Moteur();   // Controle du meteur mode degradé et normal
+        display_mode();     // Controle des led affichant le mode
         
-            
     }// fin while(1)
          
 } // fin main