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

Outils pour utilisateurs

Outils du site

Piste :
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/31 11:51] – [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 1456: Ligne 1458:
  
  
-===== Robot Mecanum Version 2 modifié par GL =====+==== Robot Mecanum Version 3 modifié par GL ====
  
  
-<code c Robot_Mecanum002.ino>+<code c Robot_Mecanum003.ino>
  
-// Tester par GL le 31 juillet 2022+// Tester par GL le 19 Aout 2022
 //Avec compensation vitesse moteur gauche ( M1 et M2 ) //Avec compensation vitesse moteur gauche ( M1 et M2 )
    
Ligne 1474: Ligne 1476:
  
 int vit=180; int vit=180;
-int comp = ( vit / 3);// compensation de 1/3 de la vitesse pour les moteurs gauche par rapport aux moteurs droit+int comp = vit /3; // Compensation vitesse pour les moteurs M1 et M2
 int vitcomp= (vit - comp); int vitcomp= (vit - comp);
 ; ;
 int tempo = 1000; int tempo = 1000;
 +int tempo10 = 1000;
 int dure= 2; int dure= 2;
  
Ligne 1615: 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
-        Serial.print("Serial Data:   "); +              
-        Serial.println(serialData); +
-        +
         if     ('F' == serialData )  cmd = 'F';    //Avant         if     ('F' == serialData )  cmd = 'F';    //Avant
         else if('B' == serialData )  cmd = 'B';    //Arriere         else if('B' == serialData )  cmd = 'B';    //Arriere
Ligne 1629: Ligne 1634:
         else if('C' == serialData )  cmd = 'C';    //Lateral Gauche         else if('C' == serialData )  cmd = 'C';    //Lateral Gauche
         else if('D' == serialData )  cmd = 'D';    //Demi Tour          else if('D' == serialData )  cmd = 'D';    //Demi Tour 
 +        else if('J' == serialData )  cmd = 'J';    //Biais Droite
 +        else if('K' == serialData )  cmd = 'K';    //Biais Gauche
 +        
                  
  
Ligne 1640: Ligne 1648:
            comp -= 5;   //I'm going to subtract 10 at a time               comp -= 5;   //I'm going to subtract 10 at a time   
         }         }
-        Serial.print("Vitesse  =  "); +        
-        Serial.println(vit); +
-        Serial.print("Vitesse comp  =  "); +
-        Serial.println(vitcomp);+
     }     }
  
     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
     {     {
-        avant();    +        avant();  
 +        delay(tempo10); 
 +        cmd ='S'; 
 +        serialData = 'S'; 
 +        arret(); 
 +        
 +         
 +        
 +         
 +         
 + 
 +          
        //AvoidingObstacles();//The ultrasonic obstacle avoidance function is called to realize the obstacle avoidance function        //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 1667: Ligne 1740:
     {      { 
         arret();         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
 +        {
 +           avant();   
 +        }
 +        else //Otherwise the distance is less than 20
 +        {
 +            arriere();
 +            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
 +    {
 +        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)           else if('A' == serialData)      
     {      { 
 +         
          LatDroite();          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)           else if('C' == serialData)      
     {      { 
 +         
 +         
          LatGauche();          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)           else if('D' == serialData)      
     {      { 
          DemiTourDroite();          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 1699: 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> +</code> 
  
 ===== Cablage Robot Mecanum ===== ===== Cablage Robot Mecanum =====
Ligne 1715: 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.1659261062.txt.gz · Dernière modification : (modification externe)