Castel'Lab le Fablab MJC de Château-Renault

Outils pour utilisateurs

Outils du site

Piste : • robots_cmt • interruptions
start:arduino:robots:mecanum

Différences

Ci-dessous, les différences entre deux révisions de la page.

Lien vers cette vue comparative

Les deux révisions précédentesRévision précédente
Prochaine révision		Révision précédente
start:arduino:robots:mecanum [2022/07/30 18:19] – [Cablage SR04] gerardadminstart:arduino:robots:mecanum [2023/02/25 12:06] (Version actuelle) – [Robot à roues Mecanum] gerardadmin
Ligne 2: Ligne 2:
  
 [[https://fr.lambdageeks.com/mecanum-wheeled-robot-design-applications/http://example.com|Robot à roues Mecanum]] [[https://fr.lambdageeks.com/mecanum-wheeled-robot-design-applications/http://example.com|Robot à roues Mecanum]]
 +
 +[[https://perso.univ-lyon1.fr/marc.buffat/COURS/MECA_HTML/Cinematique_mecanum.html|Cinematique Mecanum]]
  
 ==== Qu'est-ce que Mecanum Wheel?==== ==== Qu'est-ce que Mecanum Wheel?====
Ligne 1259: Ligne 1261:
 <code c robot_Original_Mecanum.ino> <code c robot_Original_Mecanum.ino>
  
-// Tester par GL Fontionne mais ne permet pas tout les Mouvements+// Tester par GL; Fonctionne mais ne permet pas tout les Mouvements
 //2020.11.22 //2020.11.22
 //Configure THE PWM control pin //Configure THE PWM control pin
Ligne 1455: Ligne 1457:
 </code> </code>
  
-===== Robot Mecanum Version 2 modifié par GL ===== 
  
 +==== Robot Mecanum Version 3 modifié par GL ====
  
-<code c Robot_Mecanum002.ino> 
-// Avec Librairie MotorDriver.h pour L293D A tester 
  
 +<code c Robot_Mecanum003.ino>
 +
 +// Tester par GL le 19 Aout 2022
 +//Avec compensation vitesse moteur gauche ( M1 et M2 )
 + 
 #include <MotorDriver.h> #include <MotorDriver.h>
  
Ligne 1468: Ligne 1473:
  
 MotorDriver m; MotorDriver m;
 +
  
 int vit=180; int vit=180;
 +int comp = vit /3; // Compensation vitesse pour les moteurs M1 et M2
 +int vitcomp= (vit - comp);
 +;
 int tempo = 1000; int tempo = 1000;
 +int tempo10 = 1000;
 int dure= 2; int dure= 2;
  
Ligne 1487: Ligne 1497:
 void avant() void avant()
 { {
-  m.motor(1,FORWARD,vit); +  m.motor(1,FORWARD,vitcomp); 
-  m.motor(2,FORWARD,vit);+  m.motor(2,FORWARD,vitcomp);
   m.motor(3,FORWARD,vit);   m.motor(3,FORWARD,vit);
   m.motor(4,FORWARD,vit);   m.motor(4,FORWARD,vit);
Ligne 1495: Ligne 1505:
 void arriere() void arriere()
 { {
-  m.motor(1,BACKWARD,vit); +  m.motor(1,BACKWARD,vitcomp); 
-  m.motor(2,BACKWARD,vit);+  m.motor(2,BACKWARD,vitcomp);
   m.motor(3,BACKWARD,vit);   m.motor(3,BACKWARD,vit);
   m.motor(4,BACKWARD,vit);   m.motor(4,BACKWARD,vit);
Ligne 1503: Ligne 1513:
 void LatDroite() void LatDroite()
 { {
-  m.motor(1,FORWARD,vit); +  m.motor(1,FORWARD,vitcomp); 
-  m.motor(2,BACKWARD,vit);+  m.motor(2,BACKWARD,vitcomp);
   m.motor(3,FORWARD,vit);   m.motor(3,FORWARD,vit);
   m.motor(4,BACKWARD,vit);    m.motor(4,BACKWARD,vit); 
Ligne 1511: Ligne 1521:
 void LatGauche() void LatGauche()
 { {
-  m.motor(1,BACKWARD,vit); +  m.motor(1,BACKWARD,vitcomp); 
-  m.motor(2,FORWARD,vit);+  m.motor(2,FORWARD,vitcomp);
   m.motor(3,BACKWARD,vit);   m.motor(3,BACKWARD,vit);
   m.motor(4,FORWARD,vit);    m.motor(4,FORWARD,vit); 
Ligne 1518: Ligne 1528:
 void BiaisDroite() void BiaisDroite()
 { {
-  m.motor(1,FORWARD,vit);+  m.motor(1,FORWARD,vitcomp);
   m.motor(2,RELEASE,0);   m.motor(2,RELEASE,0);
   m.motor(3,FORWARD,vit);   m.motor(3,FORWARD,vit);
Ligne 1528: Ligne 1538:
 { {
   m.motor(1,RELEASE,0);   m.motor(1,RELEASE,0);
-  m.motor(2,FORWARD,vit);+  m.motor(2,FORWARD,vitcomp);
   m.motor(3,RELEASE,0);   m.motor(3,RELEASE,0);
   m.motor(4,FORWARD,vit);   m.motor(4,FORWARD,vit);
Ligne 1536: Ligne 1546:
 void TourneDroite() void TourneDroite()
 { {
-  m.motor(1,FORWARD,vit); +  m.motor(1,FORWARD,vitcomp); 
-  m.motor(2,FORWARD,vit);+  m.motor(2,FORWARD,vitcomp);
   m.motor(3,RELEASE,0);   m.motor(3,RELEASE,0);
   m.motor(4,RELEASE,0);   m.motor(4,RELEASE,0);
Ligne 1553: Ligne 1563:
 void DemiTourDroite() void DemiTourDroite()
 { {
-  m.motor(1,FORWARD,vit); +  m.motor(1,FORWARD,vitcomp); 
-  m.motor(2,FORWARD,vit);+  m.motor(2,FORWARD,vitcomp);
   m.motor(3,BACKWARD,vit);   m.motor(3,BACKWARD,vit);
   m.motor(4,BACKWARD,vit);    m.motor(4,BACKWARD,vit); 
Ligne 1561: Ligne 1571:
 void DemiTourGauche() void DemiTourGauche()
 { {
-  m.motor(1,BACKWARD,vit); +  m.motor(1,BACKWARD,vitcomp); 
-  m.motor(2,BACKWARD,vit);+  m.motor(2,BACKWARD,vitcomp);
   m.motor(3,FORWARD,vit);   m.motor(3,FORWARD,vit);
   m.motor(4,FORWARD,vit);   m.motor(4,FORWARD,vit);
Ligne 1579: Ligne 1589:
     cm = pulseIn(Echo,HIGH)/58.8; //Convert the ranging time to CM     cm = pulseIn(Echo,HIGH)/58.8; //Convert the ranging time to CM
     cm = (int(cm * 100.0))/100.0; //Leave 2 as a decimal     cm = (int(cm * 100.0))/100.0; //Leave 2 as a decimal
-    Serial.print("Distance:");    //Character Distance displayed in serial port monitor window: +    //Serial.print("Distance:");    //Character Distance displayed in serial port monitor window: 
-    Serial.print(cm);  +    //Serial.print(cm);  
-    Serial.println("cm"); +    //Serial.println("cm"); 
  
     return cm;      //Returns cm value ranging data     return cm;      //Returns cm value ranging data
Ligne 1608: Ligne 1618:
 void HC05() void HC05()
 { {
 +    if(Serial.available() == -1) {  
 +       arret();
 +       cmd='S';
 +          }
     if(Serial.available() > 0)      //Determine if the received data is greater than 0     if(Serial.available() > 0)      //Determine if the received data is greater than 0
     {     {
         serialData = Serial.read(); //Receiving function         serialData = Serial.read(); //Receiving function
 +              
 +        if     ('F' == serialData )  cmd = 'F';    //Avant
 +        else if('B' == serialData )  cmd = 'B';    //Arriere
 +        else if('L' == serialData )  cmd = 'L';    //Tourne Gauche 
 +        else if('R' == serialData )  cmd = 'R';    //Tourne Droite 
 +        else if('S' == serialData )  cmd = 'S';    //Stop 
 +        else if('A' == serialData )  cmd = 'A';    //Lateral Droite 
 +        else if('C' == serialData )  cmd = 'C';    //Lateral Gauche
 +        else if('D' == serialData )  cmd = 'D';    //Demi Tour 
 +        else if('J' == serialData )  cmd = 'J';    //Biais Droite
 +        else if('K' == serialData )  cmd = 'K';    //Biais Gauche
 +        
                  
-        if     ('F' == serialData )  cmd = 'F';     //If the data received by the serial port is character F, save F to CMD 
-        else if('B' == serialData )  cmd = 'B';     //If the data received by the serial port is character B, save F to CMD 
-        else if('L' == serialData )  cmd = 'L';     //If the serial port receives data as the character L, save F to CMD 
-        else if('R' == serialData )  cmd = 'R';     //If the serial port receives data as the character R, save F to CMD 
-        else if('S' == serialData )  cmd = 'S';     //If the serial port receives data as character S, save F to CMD 
  
         else if( serialData == '+' && vit < 245)//If you receive a string plus, the speed increases         else if( serialData == '+' && vit < 245)//If you receive a string plus, the speed increases
         {         {
-            vit += 10;   //We're going to increase the velocity by 10 at a time+            comp += 5;   //We're going to increase the velocity by 10 at a time
                          
         }         }
         else if( serialData == '-' && vit > 30)//When I receive a string -- the speed decreases         else if( serialData == '-' && vit > 30)//When I receive a string -- the speed decreases
         {         {
-           vit -= 10;   //I'm going to subtract 10 at a time +           comp -= 5;   //I'm going to subtract 10 at a time   
-            +
         }         }
 +        
 +    }
  
-         else if('A' == serialData) //Bluetooth received the string A, car right translation+    if('F' == cmd)   //If Bluetooth receives the string F, the dolly moves forward and enables obstacle avoidance 
 +    { 
 +        avant();  
 +        delay(tempo10); 
 +        cmd ='S'; 
 +        serialData = 'S'; 
 +        arret(); 
 +        
 +         
 +        
 +         
 +         
 + 
 +           
 +       //AvoidingObstacles();//The ultrasonic obstacle avoidance function is called to realize the obstacle avoidance function 
 +    } 
 +    else if('A' == serialData)       
 +    {  
 +          
 +         LatDroite(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +         
 +    } 
 +    else if('J' == serialData)       
 +    {  
 +          
 +         BiaisDroite(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +          
 +    } 
 +    else if('K' == serialData)       
 +    {  
 +          
 +         BiaisGauche(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +          
 +    } 
 +    else if('C' == serialData)       
 +    {  
 +          
 +          
 +         LatGauche(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +         
 +    } 
 +    else if('B' == cmd)     //Bluetooth receives string B, car backs up 
 +    {    
 +        arriere(); 
 +        delay(tempo10); 
 +        cmd ='S'; 
 +        serialData = 'S'; 
 +        arret(); 
 +         
 +    } 
 +    else if('L' == cmd)    //Bluetooth received the string L, car left translation 
 +    {               
 +        TourneGauche(); 
 +        delay(tempo10); 
 +        cmd ='S'; 
 +        serialData = 'S'; 
 +        arret(); 
 +         
 +    } 
 +     
 +    else if('R' == cmd)     //Bluetooth received the string R, car right translation 
 +    { 
 +        TourneDroite();     //right translation   
 +        delay(tempo10); 
 +        cmd ='S'; 
 +        serialData = 'S';  
 +        arret(); 
 +         
 +    } 
 +    
 +    else if('S' == serialData)      //When the string S is received, the cart stops moving 
 +    {  
 +        arret(); 
 +    } 
 +     
 +     
 +    else if('D' == serialData)       
 +    {  
 +         DemiTourDroite(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +         
 +    } 
 +}  
 +void setup() 
 +{ 
 + Serial.begin(9600); 
 + 
 + pinMode(Trig,OUTPUT);//The Trig pin connected to the ultrasound is set to output mode 
 +pinMode(Echo,INPUT);//The Echo pin connected to the ultrasound is set to input mode 
 + 
 +void Motor(int Dri,int Speed1,int Speed2,int Speed3,int Speed4); 
 +int SR04(int Trig,int Echo); 
 +void AvoidingObstacles(); 
 +void HC05(); 
 +  
 +} 
 + 
 +void loop() 
 +{ 
 +    //distance = SR04(Trig,Echo);       //Acquisition of ultrasonic distance 
 + 
 +    HC05(); //Call the Bluetooth car control function 
 +} 
 +</code>  
 + 
 +==== Robot Mecanum Version 4 modifié par GL ==== 
 + 
 +<code c Robot_Mecanum004.ino> 
 + 
 +// Tester par GL le 22 Aout 2022 
 +// Avec touche "G" symbole "O" pour le programme de demonstration 
 +//Avec compensation vitesse moteur gauche ( M1 et M2 ) 
 +/* 
 +                               ^ 
 +                       M1------|------M4 
 +                               | 
 +                               | 
 +                               | 
 +                               | 
 +                               | 
 +                       M2------|------M3        
 +                        
 +*/                        
 +#include <MotorDriver.h> 
 + 
 +//Define the pin of ultrasonic obstacle avoidance sensor 
 +const int Trig = A2;       //A2 is defined as the pin Trig connected to the ultrasonic sensor 
 +const int Echo = A3;       //A3 is defined as the pin Echo connected to the ultrasonic sensor 
 + 
 +MotorDriver m; 
 + 
 + 
 +int vit=180; 
 +int comp = vit /3; 
 +int vitcomp= (vit - comp); 
 +; 
 +int tempo = 1000; 
 +int tempo10 = 1000; 
 +int dure= 2; 
 + 
 +int distance = 0;           //Variables for storing ultrasonic sensor measurements 
 +char serialData;             
 +char cmd;         
 + 
 +void arret() 
 +{ 
 +  m.motor(1,RELEASE,0); 
 +  m.motor(2,RELEASE,0); 
 +  m.motor(3,RELEASE,0);  
 +  m.motor(4,RELEASE,0);   
 +} 
 + 
 +void avant() 
 +{ 
 +  m.motor(1,FORWARD,vitcomp); 
 +  m.motor(2,FORWARD,vitcomp); 
 +  m.motor(3,FORWARD,vit); 
 +  m.motor(4,FORWARD,vit); 
 +} 
 + 
 +void arriere() 
 +{ 
 +  m.motor(1,BACKWARD,vitcomp); 
 +  m.motor(2,BACKWARD,vitcomp); 
 +  m.motor(3,BACKWARD,vit); 
 +  m.motor(4,BACKWARD,vit); 
 +} 
 + 
 +void LatDroite() 
 +{ 
 +  m.motor(1,FORWARD,vitcomp); 
 +  m.motor(2,BACKWARD,vitcomp); 
 +  m.motor(3,FORWARD,vit); 
 +  m.motor(4,BACKWARD,vit);  
 +} 
 + 
 +void LatGauche() 
 +{ 
 +  m.motor(1,BACKWARD,vitcomp); 
 +  m.motor(2,FORWARD,vitcomp); 
 +  m.motor(3,BACKWARD,vit); 
 +  m.motor(4,FORWARD,vit);  
 +} 
 +void BiaisDroite() 
 +{ 
 +  m.motor(1,FORWARD,vitcomp); 
 +  m.motor(2,RELEASE,0); 
 +  m.motor(3,FORWARD,vit); 
 +  m.motor(4,RELEASE,0);  
 +} 
 + 
 + 
 +void BiaisGauche() 
 +{ 
 +  m.motor(1,RELEASE,0); 
 +  m.motor(2,FORWARD,vitcomp); 
 +  m.motor(3,RELEASE,0); 
 +  m.motor(4,FORWARD,vit); 
 +   
 +} 
 + 
 +void TourneDroite() 
 +{ 
 +  m.motor(1,FORWARD,vitcomp); 
 +  m.motor(2,FORWARD,vitcomp); 
 +  m.motor(3,RELEASE,0); 
 +  m.motor(4,RELEASE,0); 
 +} 
 + 
 +void TourneGauche() 
 +{ 
 +  m.motor(1,RELEASE,0); 
 +  m.motor(2,RELEASE,0); 
 +  m.motor(3,FORWARD,vit); 
 +  m.motor(4,FORWARD,vit);   
 +} 
 + 
 + 
 +void DemiTourDroite() 
 +{ 
 +  m.motor(1,FORWARD,vitcomp); 
 +  m.motor(2,FORWARD,vitcomp); 
 +  m.motor(3,BACKWARD,vit); 
 +  m.motor(4,BACKWARD,vit);  
 +} 
 + 
 +void DemiTourGauche() 
 +{ 
 +  m.motor(1,BACKWARD,vitcomp); 
 +  m.motor(2,BACKWARD,vitcomp); 
 +  m.motor(3,FORWARD,vit); 
 +  m.motor(4,FORWARD,vit); 
 +} 
 + 
 + 
 + 
 +void Demo() 
 +{ 
 +for (int i=0; i < 3 ; i++){  
 +  avant(); 
 +  delay(tempo10); 
 +  arriere(); 
 +  delay(tempo10); 
 +  LatDroite(); 
 +  delay(tempo10); 
 +  LatGauche(); 
 +  delay(tempo10); 
 +  BiaisDroite(); 
 +  delay(tempo10); 
 +  BiaisGauche(); 
 +  delay(tempo10); 
 +  TourneDroite(); 
 +  delay(tempo10); 
 +  TourneGauche(); 
 +  delay(tempo10); 
 +  DemiTourDroite(); 
 +  delay(tempo10); 
 +  DemiTourGauche(); 
 +  delay(tempo10); 
 +  } 
 +} 
 +int SR04(int Trig,int Echo) 
 +{ 
 +    float cm = 0; 
 + 
 +    digitalWrite(Trig,LOW);     //Trig is set to low level 
 +    delayMicroseconds(2);       //Wait 2 microseconds 
 +    digitalWrite(Trig,HIGH);    //Trig is set to high level 
 +    delayMicroseconds(15);      //Wait 15 microseconds 
 +    digitalWrite(Trig,LOW);     //Trig is set to low 
 + 
 +    cm = pulseIn(Echo,HIGH)/58.8; //Convert the ranging time to CM 
 +    cm = (int(cm * 100.0))/100.0; //Leave 2 as a decimal 
 +    //Serial.print("Distance:");    //Character Distance displayed in serial port monitor window: 
 +    //Serial.print(cm);  
 +    //Serial.println("cm");  
 + 
 +    return cm;      //Returns cm value ranging data 
 +} 
 + 
 +void AvoidingObstacles() 
 +{ 
 +    if((distance > 20 ) || cmd == 'F')//If the distance is greater than 20cm or bluetooth receives a command equal to F 
 +    { 
 +        delay(100);//Delay of 100 ms 
 +        if(distance > 20)//Again, determine if the distance is really greater than 20cm
         {         {
-            LatDroite();  // +           avant();   
-            delay(100);+
         }         }
-        else if('C' == serialData) //Bluetooth received the string C, car left translation+        else //Otherwise the distance is less than 20
         {         {
-            LatGauche();   // +            arriere(); 
-            delay(100);+            delay(500); 
 +            TourneGauche(); 
 +            delay(500);
         }         }
 +    } 
 +}
 +
 +
 +void HC05()
 +{
 +    if(Serial.available() == -1) {  
 +       arret();
 +       cmd='S';
 +          }
 +    if(Serial.available() > 0)      //Determine if the received data is greater than 0
 +    {
 +        serialData = Serial.read(); //Receiving function
 +              
 +        if     ('F' == serialData )  cmd = 'F';    //Avant
 +        else if('B' == serialData )  cmd = 'B';    //Arriere
 +        else if('L' == serialData )  cmd = 'L';    //Tourne Gauche 
 +        else if('R' == serialData )  cmd = 'R';    //Tourne Droite 
 +        else if('S' == serialData )  cmd = 'S';    //Stop 
 +        else if('A' == serialData )  cmd = 'A';    //Lateral Droite 
 +        else if('C' == serialData )  cmd = 'C';    //Lateral Gauche
 +        else if('D' == serialData )  cmd = 'D';    //Demi Tour 
 +        else if('J' == serialData )  cmd = 'J';    //Biais Droite
 +        else if('K' == serialData )  cmd = 'K';    //Biais Gauche
 +        else if('G' == serialData )  cmd = 'G';    //Demo
 +        
 +        
 +
 +        else if( serialData == '+' && vit < 245)//If you receive a string plus, the speed increases
 +        {
 +            comp += 5;   //We're going to increase the velocity by 10 at a time
 +            
 +        }
 +        else if( serialData == '-' && vit > 30)//When I receive a string -- the speed decreases
 +        {
 +           comp -= 5;   //I'm going to subtract 10 at a time   
 +        }
 +        
     }     }
  
     if('F' == cmd)   //If Bluetooth receives the string F, the dolly moves forward and enables obstacle avoidance     if('F' == cmd)   //If Bluetooth receives the string F, the dolly moves forward and enables obstacle avoidance
-    {       +    { 
-       AvoidingObstacles();//The ultrasonic obstacle avoidance function is called to realize the obstacle avoidance function+        avant();  
 +        delay(tempo10); 
 +        cmd ='S'; 
 +        serialData = 'S'; 
 +        arret(); 
 +        
 +         
 +        
 +         
 +         
 + 
 +           
 +       //AvoidingObstacles();//The ultrasonic obstacle avoidance function is called to realize the obstacle avoidance function 
 +    } 
 +    else if('A' == serialData)       
 +    {  
 +          
 +         LatDroite(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +         
 +    } 
 +    else if('J' == serialData)       
 +    {  
 +          
 +         BiaisDroite(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +          
 +    } 
 +    else if('K' == serialData)       
 +    {  
 +          
 +         BiaisGauche(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +          
 +    } 
 +    else if('C' == serialData)       
 +    {  
 +          
 +          
 +         LatGauche(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +        
     }     }
     else if('B' == cmd)     //Bluetooth receives string B, car backs up     else if('B' == cmd)     //Bluetooth receives string B, car backs up
     {        {   
         arriere();         arriere();
 +        delay(tempo10);
 +        cmd ='S';
 +        serialData = 'S';
 +        arret();
 +        
     }     }
     else if('L' == cmd)    //Bluetooth received the string L, car left translation     else if('L' == cmd)    //Bluetooth received the string L, car left translation
     {                   {              
         TourneGauche();         TourneGauche();
 +        delay(tempo10);
 +        cmd ='S';
 +        serialData = 'S';
 +        arret();
 +        
     }     }
 +    
     else if('R' == cmd)     //Bluetooth received the string R, car right translation     else if('R' == cmd)     //Bluetooth received the string R, car right translation
     {     {
-        TourneDroite();     //right translation   +        TourneDroite();     //right translation   
 +        delay(tempo10); 
 +        cmd ='S'; 
 +        serialData = 'S';  
 +        arret(); 
 +        
     }     }
        
Ligne 1662: Ligne 2096:
         arret();         arret();
     }     }
-} +     
- +     
-  +    else if('D' == serialData)       
 +    {  
 +         DemiTourDroite(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +         
 +    } 
 +    else if('G' == serialData)       
 +    {  
 +         Demo(); 
 +         delay(tempo10); 
 +         cmd ='S'; 
 +         serialData = 'S'; 
 +         arret(); 
 +         
 +    } 
 +} 
 void setup() void setup()
 { {
Ligne 1681: Ligne 2133:
 void loop() void loop()
 { {
-    distance = SR04(Trig,Echo);       //Acquisition of ultrasonic distance+    //distance = SR04(Trig,Echo);       //Acquisition of ultrasonic distance
  
     HC05(); //Call the Bluetooth car control function     HC05(); //Call the Bluetooth car control function
 } }
 +</code>
 +
 +
 +===== Cablage Robot Mecanum =====
 +
 +{{ :start:arduino:robots:mecanum_robot_002.jpg?direct&600 |}}
  
-</code>  
  
 ===== Cablage SR04 ===== ===== Cablage SR04 =====
Ligne 1693: Ligne 2150:
  
 {{ :start:arduino:robots:sr04_002.jpg?direct&600 |}} {{ :start:arduino:robots:sr04_002.jpg?direct&600 |}}
 +
 +
 +======= Robot Mecanum commandé via PS2 sans fil =======
 +
 +{{ :start:arduino:robots:instructionsm.jpg?direct&600 |}}
 +
 +
 +==== programme1 test mecanum PS2 ====
 +
 +
 +
 +<code c mecanum-test001.ino>
 +
 +//programme en cours de test par GL le 13/12/2022
 +#include <PS2X_lib.h>  //for MOEBIUS
 +#include "FaBoPWM_PCA9685.h"
 +
 +//#include "servo.hpp"
 +
 +FaBoPWM faboPWM;
 +int pos = 0;
 +int MAX_VALUE = 2000;   // motor Motor_PWM 
 +int MIN_VALUE = 300;
 +
 +//PS2手柄引脚；PS2 handle Pin
 +#define PS2_DAT        13
 +#define PS2_CMD        11
 +#define PS2_SEL        10
 +#define PS2_CLK        12
 +
 +//MOTOR CONTROL Pin
 +#define DIRA1 0
 +#define DIRA2 1
 +#define DIRB1 2
 +#define DIRB2 3
 +#define DIRC1 4
 +#define DIRC2 5
 +#define DIRD1 6
 +#define DIRD2 7
 +
 +char speed;
 +// #define pressures   true
 +#define pressures   false
 +// #define rumble      true
 +#define rumble      false
 +
 +PS2X ps2x; // create PS2 Controller Class
 +
 +//right now, the library does NOT support hot pluggable controllers, meaning
 +//you must always either restart your Arduino after you connect the controller,
 +//or call config_gamepad(pins) again after connecting the controller.
 +
 +int error = 0;
 +byte type = 0;
 +byte vibrate = 0;
 +
 +void (* resetFunc) (void) = 0;
 +
 +//电机控制，前进、后退、停止   motor control advance\back\stop
 +#define MOTORA_FORWARD(pwm)    do{faboPWM.set_channel_value(DIRA1,pwm);faboPWM.set_channel_value(DIRA2, 0);}while(0)
 +#define MOTORA_STOP(x)         do{faboPWM.set_channel_value(DIRA1,0);faboPWM.set_channel_value(DIRA2, 0);}while(0)
 +#define MOTORA_BACKOFF(pwm)    do{faboPWM.set_channel_value(DIRA1,0);faboPWM.set_channel_value(DIRA2, pwm);}while(0)
 +
 +#define MOTORB_FORWARD(pwm)    do{faboPWM.set_channel_value(DIRB1,pwm);faboPWM.set_channel_value(DIRB2, 0);}while(0)
 +#define MOTORB_STOP(x)         do{faboPWM.set_channel_value(DIRB1,0);faboPWM.set_channel_value(DIRB2, 0);}while(0)
 +#define MOTORB_BACKOFF(pwm)    do{faboPWM.set_channel_value(DIRB1,0);faboPWM.set_channel_value(DIRB2, pwm);}while(0)
 +
 +#define MOTORC_FORWARD(pwm)    do{faboPWM.set_channel_value(DIRC1,pwm);faboPWM.set_channel_value(DIRC2, 0);}while(0)
 +#define MOTORC_STOP(x)         do{faboPWM.set_channel_value(DIRC1,0);faboPWM.set_channel_value(DIRC2, 0);}while(0)
 +#define MOTORC_BACKOFF(pwm)    do{faboPWM.set_channel_value(DIRC1,0);faboPWM.set_channel_value(DIRC2, pwm);}while(0)
 +
 +#define MOTORD_FORWARD(pwm)    do{faboPWM.set_channel_value(DIRD1,pwm);faboPWM.set_channel_value(DIRD2, 0);}while(0)
 +#define MOTORD_STOP(x)         do{faboPWM.set_channel_value(DIRD1,0);faboPWM.set_channel_value(DIRD2, 0);}while(0)
 +#define MOTORD_BACKOFF(pwm)    do{faboPWM.set_channel_value(DIRD1,0);faboPWM.set_channel_value(DIRD2, pwm);}while(0)
 +
 +#define SERIAL  Serial
 +
 +//#define SERIAL  Serial3
 +
 +#define LOG_DEBUG
 +
 +#ifdef LOG_DEBUG
 +#define M_LOG SERIAL.print
 +#else
 +#define M_LOG 
 +#endif
 +
 +//PWM参数
 +#define MAX_PWM   2000
 +#define MIN_PWM   300
 +
 +int Motor_PWM = 1900;
 +int Motor_PWM_mod = Motor_PWM - 400;
 + 
 +//------AVANCE--------------------------------------------------------------------
 +//    ↑A-----B↑   
 +//     |  ↑  |
 +//     |  |  |
 +//    ↑C-----D↑
 +void ADVANCE(uint8_t pwm_A,uint8_t pwm_B,uint8_t pwm_C,uint8_t pwm_D)
 +{
 +  MOTORA_FORWARD(Motor_PWM_mod);
 +  MOTORB_FORWARD(Motor_PWM);    
 +  MOTORC_FORWARD(Motor_PWM_mod);
 +  MOTORD_FORWARD(Motor_PWM);    
 +}
 +//-------RECUL------------------------------------------------------------------------
 +//    ↓A-----B↓ 
 +//     |  |  |
 +//     |  ↓  |
 +//    ↓C-----D↓
 +void BACK()
 +{
 +  MOTORA_BACKOFF(Motor_PWM_mod);MOTORB_BACKOFF(Motor_PWM);
 +  MOTORC_BACKOFF(Motor_PWM_mod);MOTORD_BACKOFF(Motor_PWM);
 +}
 +//-------GAUCHE AVANT------------------------------------------------------------------------
 +//    =B-----A↑   
 +//     |   ↖ |
 +//     | ↖   |
 +//    ↑C-----D=
 +void LEFT_1()
 +{
 +  MOTORA_FORWARD(Motor_PWM);MOTORB_STOP(Motor_PWM);
 +  MOTORC_FORWARD(Motor_PWM);MOTORD_STOP(Motor_PWM);
 +}
 +//-------GAUCHE---------------------------------------------------------------------------
 +//
 +//
 +//    ↓A-----B↑   
 +//     |  ←  |
 +//     |  ←  |
 +//    ↑C-----D↓
 +void LEFT_2()
 +{
 +  MOTORA_BACKOFF(Motor_PWM);MOTORB_FORWARD(Motor_PWM);
 +  MOTORC_FORWARD(Motor_PWM);MOTORD_BACKOFF(Motor_PWM);
 +}
 +//---------GAUCHE ARRIERE----------------------------------------------------------------------------
 +//
 +//
 +//    ↓A-----B=   
 +//     | ↙   |
 +//     |   ↙ |
 +//    =C-----D↓
 +void LEFT_3()
 +{
 +  MOTORA_BACKOFF(Motor_PWM);MOTORB_STOP(Motor_PWM);
 +  MOTORC_STOP(Motor_PWM);MOTORD_BACKOFF(Motor_PWM);
 +}
 +//-----------DROITE AVANT-----------------------------------------------------------------------
 +//    ↑A-----B=   
 +//     | ↗   |
 +//     |   ↗ |
 +//    =C-----D↑
 +void RIGHT_1()
 +{
 +  MOTORA_FORWARD(Motor_PWM);MOTORB_STOP(Motor_PWM);
 +  MOTORC_STOP(Motor_PWM);MOTORD_FORWARD(Motor_PWM); 
 +}
 +//--------------DROITE-----------------------------------------------------------------------
 +//    ↑A-----B↓   
 +//     |  →  |
 +//     |  →  |
 +//    ↓C-----D↑
 +void RIGHT_2()
 +{
 +  MOTORA_FORWARD(Motor_PWM);MOTORB_BACKOFF(Motor_PWM);
 +  MOTORC_BACKOFF(Motor_PWM);MOTORD_FORWARD(Motor_PWM);
 +}
 +//-----------------DROITE ARRIERE--------------------------------------------------------------
 +//    =A-----B↓   
 +//     |   ↘ |
 +//     | ↘   |
 +//    ↓C-----D=
 +void RIGHT_3()
 +{
 +  MOTORA_STOP(Motor_PWM);MOTORB_BACKOFF(Motor_PWM);
 +  MOTORC_BACKOFF(Motor_PWM);MOTORD_STOP(Motor_PWM);
 +}
 +//---------------------TOURNE---DROITE-------------------------------------------------------
 +//    ↑A-----B↓   
 +//     | ↗ ↘ |
 +//     | ↖ ↙ |
 +//    ↑C-----D↓
 +void rotate_1()  //tate_1(uint8_t pwm_A,uint8_t pwm_B,uint8_t pwm_C,uint8_t pwm_D) 
 +{
 +  MOTORA_FORWARD(Motor_PWM);MOTORB_BACKOFF(Motor_PWM);
 +  MOTORC_FORWARD(Motor_PWM);MOTORD_BACKOFF(Motor_PWM);
 +}
 +//----------------------TOURNE GAUCHE----------------------------------------------------------
 +//    ↓A-----B↑   
 +//     | ↙ ↖ |
 +//     | ↘ ↗ |
 +//    ↓C-----D↑
 +void rotate_2()  // rotate_2(uint8_t pwm_A,uint8_t pwm_B,uint8_t pwm_C,uint8_t pwm_D)
 +{
 +  MOTORA_BACKOFF(Motor_PWM);MOTORB_FORWARD(Motor_PWM);
 +  MOTORC_BACKOFF(Motor_PWM);MOTORD_FORWARD(Motor_PWM);
 +}
 +//---------------------------ARRET-------------------------------------------------------
 +//    =A-----B=  
 +//     |  =  |
 +//     |  =  |
 +//    =C-----D=
 +void STOP()
 +{
 +  MOTORA_STOP(Motor_PWM);MOTORB_STOP(Motor_PWM);
 +  MOTORC_STOP(Motor_PWM);MOTORD_STOP(Motor_PWM);
 +}
 +//-----------------------------------------------------------------------------------
 +
 +void UART_Control()
 +{
 +  char Uart_Date=0;
 +  if(SERIAL.available())
 +  {
 +    Uart_Date = SERIAL.read();
 +  }
 +  switch(Uart_Date)
 +  {
 +     case 'A':  ADVANCE(500,500,500,500);  M_LOG("Run!\r\n");        break;
 +     case 'B':  RIGHT_1();  M_LOG("Right up!\r\n");     break;
 +     case 'C':  rotate_2();                            break;      
 +     case 'D':  RIGHT_3();  M_LOG("Right down!\r\n");   break;
 +     case 'E':  BACK();     M_LOG("Run!\r\n");          break;
 +     case 'F':  LEFT_3();   M_LOG("Left down!\r\n");    break;
 +     case 'G':  rotate_1();                              break;         
 +     case 'H':  LEFT_1();   M_LOG("Left up!\r\n");     break;
 +     case 'Z':  STOP();     M_LOG("Stop!\r\n");        break;
 +     case 'z':  STOP();     M_LOG("Stop!\r\n");        break;
 +     case 'd':  LEFT_2();   M_LOG("Left!\r\n");        break;
 +     case 'b':  RIGHT_2();  M_LOG("Right!\r\n");        break;
 +     case 'L':  Motor_PWM = 1500;                      break;
 +     case 'M':  Motor_PWM = 500;                       break;
 +   }
 +}
 +
 +void IO_init()
 +{
 +  STOP();
 +}
 +
 +void setup()
 +{
 +   IO_init();
 +   SERIAL.begin(9600);
 +  if(faboPWM.begin()) 
 +  {
 +    Serial.println("Find PCA9685");
 +    faboPWM.init(300);
 +  }
 +  faboPWM.set_hz(50);
 +  SERIAL.print("Start"); 
 +
 +   delay(300) ; //added delay to give wireless ps2 module some time to startup, before configuring it
 +  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
 +
 +  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
 +  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
 +
 +  if (error == 0) {
 +    Serial.print("Found Controller, configured successful ");
 +    Serial.print("pressures = ");
 +    if (pressures)
 +      Serial.println("true ");
 +    else
 +      Serial.println("false");
 +    Serial.print("rumble = ");
 +    if (rumble)
 +      Serial.println("true)");
 +    else
 +      Serial.println("false");
 +    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
 +    Serial.println("holding L1 or R1 will print out the analog stick values.");
 +    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
 +  }
 +  else if (error == 1)
 +  {
 +    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
 +    resetFunc();
 +    
 +  }
 +
 +  else if (error == 2)
 +    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
 +
 +  else if (error == 3)
 +    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
 +
 +//  Serial.print(ps2x.Analog(1), HEX);
 +
 +  type = ps2x.readType();
 +  switch (type) {
 +  case 0:
 +    Serial.print("Unknown Controller type found ");
 +    break;
 +  case 1:
 +    Serial.print("DualShock Controller found ");
 +    break;
 +  case 2:
 +    Serial.print("GuitarHero Controller found ");
 +    break;
 +  case 3:
 +    Serial.print("Wireless Sony DualShock Controller found ");
 +    break;
 +  }
 +}
 +
 +
 +
 +void loop()
 +{
 +   // UART_Control();
 +    //CAR_Control();
 +      /* You must Read Gamepad to get new values and set vibration values
 +    ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
 +    if you don't enable the rumble, use ps2x.read_gamepad(); with no values
 +    You should call this at least once a second
 +  */
 +  if (error == 1) //skip loop if no controller found
 +    return;
 +
 +  if (type == 2) { //Guitar Hero Controller
 +    return;
 +  }
 +  else  { //DualShock Controller
 +    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' Motor_PWM
 +
 +
 +//start 
 +    if (ps2x.Button(PSB_START))  {
 +      Serial.println("Start is being held");
 +      ADVANCE(500,500,500,500);
 +
 +
 +    }
 +
 +    if (ps2x.Button(PSB_PAD_UP)) {
 +      Serial.println("Up held this hard: ");
 +     ADVANCE(500,500,500,500);
 +    }
 +
 +
 +    if (ps2x.Button(PSB_PAD_DOWN)) {
 +      Serial.print("Down held this hard: ");
 +      BACK();
 +    }
 +
 +
 +    if (ps2x.Button(PSB_PAD_LEFT)) {
 +      Serial.println("turn left ");
 +      LEFT_2();
 +    }
 +
 +
 +    if (ps2x.Button(PSB_PAD_RIGHT)) {
 +      Serial.println("turn right");
 +      RIGHT_2();
 +    }
 +// Stop
 +    if (ps2x.Button(PSB_SELECT)) {
 +      Serial.println("stop");
 +      STOP();
 +    }
 +// 左平移
 +    if (ps2x.Button(PSB_PINK)) {
 +      Serial.println("motor_pmove_left");
 +      LEFT_1();
 +    }
 +
 +    if (ps2x.Button(PSB_RED)) {
 +      Serial.println("motor_pmove_right");
 +      RIGHT_1();
 +    }
 +    delay(20);
 +
 +  }
 +  if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE
 +    Serial.print("Stick Values:");
 +    Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
 +    Serial.print(",");
 +    Serial.print(ps2x.Analog(PSS_LX), DEC);
 +    Serial.print(",");
 +    Serial.print(ps2x.Analog(PSS_RY), DEC);
 +    Serial.print(",");
 +    Serial.println(ps2x.Analog(PSS_RX), DEC);
 +
 +    int LY = ps2x.Analog(PSS_LY);
 +    int LX = ps2x.Analog(PSS_LX);
 +    int RY = ps2x.Analog(PSS_RY);
 +    int RX = ps2x.Analog(PSS_RX);
 +
 +    if (LY < 127) 
 +    {
 +
 +     // Motor_PWM = 1.5 * (127 - LY);
 +      ADVANCE(500,500,500,500);
 +      //Motor_PWM = 1.5 * (127 - LX);
 +      delay(20);
 +      Serial.print("ADVANCE");
 +    }
 +   
 +    if (LY > 127)
 +    {
 +      //Motor_PWM = 1.5 * (LY - 128);
 +      BACK();
 +      delay(20);
 +      Serial.print("ARRIERE");
 +    }
 +    
 +    if (LX < 128)
 +    {
 +      //Motor_PWM = 1.5 * (127 - LX);
 +      LEFT_2();
 +      delay(20);
 +      Serial.print("LEFT_2");
 +    }
 +    
 +    if (LX > 128)
 +    {
 +      //Motor_PWM = 1.5 * (LX - 128);
 +      RIGHT_2();
 +      delay(20);
 +      Serial.print("RIGHT_2");
 +    }
 +    
 +    if (LY == 127 && LX == 128)
 +    {
 +      STOP();
 +      delay(20);
 +    }
 +
 +  }
 +}
 +</code>
/home/chanteri/www/fablab37110/data/attic/start/arduino/robots/mecanum.1659197988.txt.gz · Dernière modification : 2023/01/27 16:08 (modification externe)