Diff: main.cpp

Revision:

10:fb3542f3c5e6

Parent:

9:0f63d4cb5613

diff -r 0f63d4cb5613 -r fb3542f3c5e6 main.cpp
--- a/main.cpp	Fri May 18 09:03:25 2018 +0000
+++ b/main.cpp	Mon May 28 14:18:32 2018 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "parameters.h"
+#include "rac.h"
  /* 
   Projet BTS SN - Robot à câbles - Lycée Georges Cabanis
   Enzo Niro - Erwin Sazio
@@ -10,15 +11,13 @@
  #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 MOD_DEG(p25);
 DigitalOut MOD_NOR(p26);
+DigitalOut CYCLE_TIME(p20);
 DigitalIn SWH(p5);
 DigitalIn SWV(p6);
 DigitalIn BPO(p7);
@@ -26,16 +25,16 @@
 AnalogIn joystick_x(p16);     // Pin 4 (fil jaune)
 AnalogIn joystick_y(p17);     // Pin 5 (fil bleu)
 
+RAC robot;
 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;
+bool mode = 0;   // 0 = Degraded   1 = Normal
 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
-uint16_t timer_process_read; //counter to make mbed read actual position
 double dy = 1262, dx = 492.5;//coordinates values when origin is done
 //these positions are in counters
 int32_t p1, p2; //motors positions
@@ -46,7 +45,7 @@
 //l1 = longueur moteur gauche à l'effecteur
 //l2 = longueur moteur droite à l'effecteur
 int32_t lx, l1, l2;
-float pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur
+double pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur
 
 
 
@@ -65,28 +64,7 @@
     }
 }
 
-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 )) ;
-      //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 ));
-        //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);
-    //lcd.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] );
-    /*if(emis_ok) {
-        // ici octet emis n'a pas de sens car trame remote !
-     //pc.printf("send \r \n");
-    } */
-}
+
 
 void initialisation()
 {
@@ -110,36 +88,35 @@
     MOD_NOR = 0;
     wait(0.1);
         
-   command[0] = DRIVER_R1;
+    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
+        robot.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);
-        }
+        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;
+        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
+            robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
             MOD_DEG = 1;
             if(can2.read(msg)) {
             for(int i = 0; i < msg.len; i++){
@@ -155,118 +132,6 @@
 }   // fin initialisation
 
 
-
-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
-        if(can2.read(msg)){
-        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];
-        }
-        //timer_process_read = 1;
-        wait(0.005);
-        send(TPDO1_R, command, 'R', 0); //Ask position for first length
-        if(can2.read(msg)){
-        p_actual2 = msg.data[3];
-        p_actual2 = p_actual2 << 8;
-        p_actual2 = p_actual2 | msg.data[2];
-        p_actual2 = p_actual2 << 8;
-        p_actual2 = p_actual2 | msg.data[1];
-        p_actual2 = p_actual2 << 8;
-        p_actual2 = p_actual2 | msg.data[0];
-        }
-
-    
-    }//fin de fonction
-
-
-
-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
-    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_R;
-    
-    if(mode ==0)
-    {
-    ///////////// MODE DEGRADE /////////////////////////////////////
-    VX = (float(x)/42)*425000;    // Calcule brut vélocité x, y
-    VY = (float(y)/42)*425000; 
-    VM_L = VX - VY;            // Calcule des vélocités exactes de chaques moteurs
-    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 //////////////////////////////////////
-    VX = x;
-    VY = y;
-    VM_L = ((dx*VX)-(dy*VY));
-    VM_L = (VM_L/l1);
-    VM_L = VM_L*(ALPHA/42);
-    VM_R = ((-(lx-dx)*VX)-(dy*VY));    
-    VM_R = (VM_R/l2); 
-    VM_R = VM_R*(ALPHA/42);  
-    
-    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
-    }
-    pVx = VM_R;
-    pVy = VM_L;
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
-}
  
 int main() {
     
@@ -274,27 +139,37 @@
     SWH.mode(PullUp);
     SWV.mode(PullUp);
     BPO.mode(PullUp);
-        
+    
     initialisation();   // Mise en mode opérationnel des moteurs
-
-
-
                 
     while(1)
     {
   
-    
+    /*
             pc.printf("L1 = %d", l1);
-            pc.printf("\t L2 = %d", l2);
-            pc.printf("\t x = %f", dx);
-            pc.printf("\t y = %f", dy);
-            pc.printf("\t VM_L = %f", pVx);
-            pc.printf("\t VM_R = %f \r\n", pVy);
-
+            pc.printf("\t L2 = %d", l2);*/
+            if(mode == DEGRAD){
+            pc.printf("mode = D");    
+                }
+            else if(mode == NORMAL){
+            pc.printf("mode = N");    
+                }
+            pc.printf(" x = %f", dx);
+            pc.printf(" y = %f", dy);
+            pc.printf(" VM_L = %f", pVx);
+            pc.printf(" VM_R = %f \r\n", pVy);
             
-        read_position();
-        control_Moteur();   // Controle du meteur mode degradé et normal
+            
+            
+        robot.readButtons(SWH, SWV, BPO);   //read state buttons, important for others functions
+        
+        CYCLE_TIME = 0;  
+        robot.readPosition(&dx, &dy, &mode); //read motors encoders and calculate coordinates position (x,y)
+        robot.setVelocity(joystick_x.read(),joystick_y.read(),joystick_ctr.read(), mode);   //Motors control with the joystick and with the mode we use
+        robot.readVelocity(&pVx, &pVy);//read velocities applied on the robot
         display_mode();     // Controle des led affichant le mode
+            
+        CYCLE_TIME = 1;
         
     }// fin while(1)