API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jun 17th, 2019
71
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 13.87 KB | None | 0 0
raw download clone embed print report
  1. #include "Grove_I2C_Motor_Driver.h"
  2.  
  3. // default I2C address is 0x0f
  4. #define I2C_ADDRESS 0x0f
  5.  
  6. double xStart=0.0;
  7. double yStart=0.0;
  8.  
  9. bool etatCapteurGauche;
  10. bool etatCapteurCentreD;
  11. bool etatCapteurCentreG;
  12. bool etatCapteurDroit;
  13.  
  14. const int capteurGauche = 2;//Capteur Gauche
  15. const int capteurCentreD = 4;//Capteur MilieuD
  16. const int capteurDroit = 5;//Capteur Droit
  17. const int capteurCentreG = 3;//Capteur MilieuG
  18.  
  19. double xEnd=0.0;
  20. double yEnd=1.70;
  21.  
  22. double vitesseVoiture = 0.47;
  23. //Création de la classe Point.
  24. class Point {
  25.  
  26. //vitesse moyenne de la voiture = 0,43m/s
  27. double timeDelay = NULL;
  28. double averageSpeed = 0.43;
  29. double distance = NULL;
  30.  
  31. //Variables pour le chemin de la voiture
  32. int previousPoint = NULL;
  33. int actualPoint = NULL;
  34. int nextPoint = NULL;
  35. int finalPoint = NULL;
  36.  
  37.  private :
  38.  //On créé les attributs x et y qui sont les coordonnées d'un point, et on créé une direction.
  39.  double x,y;
  40.  Point* south;
  41.  Point* west;
  42.  Point* north;
  43.  Point* east;
  44.  
  45.  
  46.   public :
  47.  Point(double initializeX, double initializeY){
  48.   this->x=initializeX;
  49.   this->y=initializeY;
  50.  }
  51.  
  52.  
  53.  double getX(){
  54.   return this->x;
  55.  }
  56.  
  57.  
  58.  double getY(){
  59.   return this->y;
  60.  }
  61.  
  62.  
  63.  void setPointAdjacent(Point* nextSouth,Point* nextWest, Point* nextNorth, Point* nextEast){
  64.  
  65.  if(nextSouth != nullptr){
  66.  
  67.   this->south = nextSouth;
  68.  
  69.  }
  70.  if(nextWest != nullptr){
  71.  
  72.   this ->west = nextWest;
  73.  }
  74.  
  75.  if (nextNorth != nullptr)
  76.  {
  77.   this->north = nextNorth;
  78.  }
  79.  
  80.  if (nextEast != nullptr){
  81.   this->east = nextEast;
  82.  }
  83.  
  84.  }
  85. };
  86.  
  87. //Initialisation de tous les points.
  88. Point* pA = new Point(0.0,0.0);
  89. Point* pB = new Point(0.0,1.70);
  90. Point* pC = new Point(0.0,3.08);
  91. Point* pD = new Point(0.0,4.24);
  92. Point* pE = new Point(2.08,4.24);
  93. Point* pF = new Point(2.08,3.08);
  94. Point* pG = new Point(2.79,0.92);
  95. Point* pH = new Point(8.9,3.16);
  96. Point* pI = new Point(8.9,4.20);
  97. Point* pJ = new Point(11.50,4.16);
  98. Point* pK = new Point(11.48,1.59);
  99. Point* pL = new Point(11.46,0.92);
  100. Point* pM = new Point(12.8,0.0);
  101. Point* pN = new Point(12.8,1.58);
  102. Point* pO = new Point(12.8,4.16);
  103. Point* pP = new Point(2.79,0.0);
  104.  
  105. //Initialisation des points de début et de fin.
  106. Point* pStart = new Point(xStart, yStart);
  107. Point* pEnd = new Point(xEnd, yEnd);
  108.  
  109. //Initialise notre map
  110. void map() {
  111.  
  112.   pA->setPointAdjacent(nullptr,nullptr,pB,pP); //nullptr est une convention qui permet d'initialiser un pointeur à null,et donc qui ne change pas la valeur initiale.
  113.   pB->setPointAdjacent(pA,nullptr,pC,pK);
  114.   pC->setPointAdjacent(pB,nullptr,pO,pF);
  115.   pD->setPointAdjacent(pC,nullptr,nullptr,pE);
  116.   pE->setPointAdjacent(pF,pD,nullptr,pI);
  117.   pF->setPointAdjacent(nullptr,pC,pE,pH);
  118.   pG->setPointAdjacent(pP,nullptr,nullptr,pL);
  119.   pH->setPointAdjacent(nullptr,pF,pI,nullptr);
  120.   pI->setPointAdjacent(pH,pE,nullptr,pJ);
  121.   pJ->setPointAdjacent(pK,pI,nullptr,pO);
  122.   pK->setPointAdjacent(pL,pB,pJ,pN);
  123.   pL->setPointAdjacent(nullptr,pG,pK,nullptr);
  124.   pM->setPointAdjacent(nullptr,pP,pN,nullptr);
  125.   pN->setPointAdjacent(pM,pK,pO,nullptr);
  126.   pO->setPointAdjacent(pN,pJ,nullptr,nullptr);
  127.   pP->setPointAdjacent(nullptr,pA,pG,pM);
  128.    
  129.   pStart->setPointAdjacent(nullptr,nullptr,pB,pP);
  130.   //pEnd->setPointAdjacent(
  131. };
  132.  
  133.  
  134. while(actualPoint != finalPoint){
  135. switch(actualPoint){
  136.   case pA:
  137.   if(previousPoint = pP){
  138.     if(nextPoint = nextNorth){
  139.       turnRight();
  140.       delay(delayTime);
  141.     }
  142.   }
  143.   else if (previousPoint = pB){
  144.     if(nextPoint = nextEast){
  145.       turnLeft();
  146.     }
  147.   }
  148.   else{
  149.     moveForward();
  150.   }
  151.   previousPoint = pA;
  152.   break;
  153.   case pB:
  154.   if(previousPoint = pA){
  155.     if(nextPoint = nextNorth){
  156.       moveForward();
  157.     }
  158.     else if(nextPoint = nextEast){
  159.       turnRight();
  160.       }
  161.   }
  162.   else if(previousPoint = pK){
  163.     if(nextPoint = pC){
  164.       turnRight();
  165.     }
  166.     else if(nextPoint = pA){
  167.       turnLeft();
  168.       }
  169.   }
  170.   else if(previousPoint = pC){
  171.     if(nextPoint = pK){
  172.       turnLeft();
  173.     }
  174.     else if(nextPoint = pA){
  175.       moveForward();
  176.     }
  177.   }
  178.   else if(previousPoint = pK){
  179.     if(nextPoint = pC){
  180.       turnRight();
  181.     }
  182.     else if(nextPoint = pB){
  183.       turnLeft();
  184.     }
  185.   }
  186.   previousPoint = pB;
  187.   break;
  188.   case pC:
  189.   if(previousPoint = pB){
  190.     if(nextPoint = pF){
  191.       turnRight();
  192.     }
  193.     else if(nextPoint = pD){
  194.       moveForward();
  195.     }
  196.   }
  197.   else if(previousPoint = pF){
  198.     if(nextPoint = pD){
  199.       turnRight();
  200.     }
  201.     else if(nextPoint = pB){
  202.       turnLeft();      
  203.     }
  204.   }
  205.   else if(previousPoint = pD){
  206.     if(nextPoint = pF){
  207.       turnLeft();
  208.     }
  209.     else if(nextPoint = pB){
  210.       moveForward();
  211.     }
  212.   }
  213.   previousPoint = pC;
  214.   break;
  215.   case pD:
  216.   if(previousPoint = pC){
  217.     if(nextPoint = pE){
  218.       turnRight();  
  219.     }
  220.   }
  221.   else(previousPoint = pE){
  222.    if(nextPoint = pC){
  223.      turnLeft();
  224.    }
  225.   }
  226.   previousPoint = pD;
  227.   break;
  228.   case pE:
  229.   if(previousPoint = pD){
  230.     if(nextPoint = pI){
  231.       moveForward();      
  232.     }
  233.     else if(nextPoint = pF){
  234.       turnRight();
  235.     }
  236.   }
  237.   else if(previousPoint = pF){
  238.    if(nextPoint = pI){
  239.      turnRight();
  240.    }
  241.    else if(nextPoint = pD){
  242.     turnLeft();
  243.    }
  244.   }
  245.   else if(previousPoint = pI){
  246.     if(nextPoint = pF){
  247.       turnLeft();
  248.     }
  249.     else if(nextPoint = pD){
  250.       moveForward();
  251.     }
  252.   }
  253.   previousPoint = pE;
  254.   break;
  255.   case pF:
  256.   if(previousPoint = pE){
  257.     if(nextPoint = pC){
  258.       turnRight();
  259.     }
  260.     else if(nextPoint = pH){
  261.       turnLeft();
  262.     }
  263.   }
  264.   else if(previousPoint = pH){
  265.     if(nextPoint = pE){
  266.       turnRight();
  267.     }
  268.     else if(nextPoint = pC){
  269.       moveForward();
  270.     }
  271.   }
  272.   else if(previousPoint = pC){
  273.    if(nextPoint = pE){
  274.     turnLeft();
  275.    }
  276.    else if(nextPoint = pH){
  277.     moveForward();
  278.    }
  279.   }
  280.   previousPoint = pF;
  281.   break;
  282.   case pH:
  283.   if(previousPoint = pF){
  284.     if(nextPoint = pI){
  285.       turnLeft();
  286.     }
  287.   }
  288.   else if(previousPoint = pI){
  289.     if(nextPoint = pF){
  290.       turnRight();
  291.     }
  292.   }
  293.   previousPoint = pH;
  294.   break;
  295.   case pI:
  296.   if(previousPoint = pJ){
  297.     if(nextPoint = pH){
  298.       turnLeft();
  299.     }
  300.     else if(nextPoint = pE){
  301.       moveForward();
  302.     }
  303.   }
  304.   else if(previousPoint = pH){
  305.     if(nextPoint = pJ){
  306.       turnRight();
  307.     }
  308.     else if(nextPoint = pE){
  309.       turnLeft();
  310.     }
  311.   }
  312.   else if(previousPoint = pE){
  313.     if(nextPoint = pJ){
  314.       moveForward();
  315.     }
  316.     else if(nextPoint = pH){
  317.       turnRight();
  318.     }
  319.   }
  320.   previousPoint = pI;
  321.   break;
  322.   case pJ:
  323.   if(previousPoint = pI){
  324.     if(nextPoint = pK){
  325.       turnRight();
  326.     }
  327.     else if(nextPoint = pO){
  328.       moveForward();
  329.     }
  330.   }
  331.   else if(previousPoint = pK){
  332.     if(nextPoint = pO){
  333.       turnRight(),  
  334.     }
  335.     else if(nextPoint = pI){
  336.       turnLeft();
  337.     }
  338.   }
  339.   else if(previousPoint = pO){
  340.     if(nextPoint = pK){
  341.       turnLeft();
  342.     }
  343.     else if(nextPoint = pI){
  344.       moveForward();
  345.     }
  346.   }
  347.   previousPoint = pJ;
  348.   break;
  349.   case pK:
  350.   if(previousPoint = pJ){
  351.     if(nextPoint = pN){
  352.       turnLeft();
  353.     }
  354.     else if(nextPoint = pL){
  355.       moveForward();
  356.     }
  357.     else if(nextPoint = pB){
  358.       turnRight();
  359.     }
  360.   }
  361.   else if(previousPoint = pN){
  362.     if(nextPoint = pJ){
  363.       turnRight();
  364.     }
  365.     else if(nextPoint = pL){
  366.       turnLeft();
  367.     }
  368.     else if(nextPoint = pB){
  369.       moveForward();
  370.     }
  371.   }
  372.   else if(previousPoint = pL){
  373.     if(nextPoint = pJ){
  374.       moveForward();
  375.     }
  376.     else if(nextPoint = pN){
  377.       turnRight();
  378.     }
  379.     else if(nextPoint = pB){
  380.       turnLeft();
  381.     }
  382.   }
  383.   else if(previousPoint = pB){
  384.     if(nextPoint = pL){
  385.       turnRight();
  386.     }
  387.     else if(nextPoint = pN){
  388.       moveForward();
  389.     }
  390.     else if(nextPoint = pJ){
  391.       turnLeft();
  392.     }
  393.   }
  394.   previousPoint = pK;
  395.   break;
  396.   case pL:
  397.   if(previousPoint = pK){
  398.     if(nextPoint = pG){
  399.       turnRight();
  400.     }
  401.   }
  402.   else if(previousPoint = pG){
  403.     if(nextPoint = pK){
  404.       turnLeft();
  405.     }
  406.   }
  407.   previousPoint = pL;
  408.   break;
  409.   case pM:
  410.   if(previousPoint = pN){
  411.     if(nextPoint = pP){
  412.       turnRight();
  413.     }
  414.   }
  415.   else if(previousPoint = pP){
  416.     if(nextPoint = pN){
  417.       turnLeft();
  418.     }
  419.   }
  420.   previousPoint = pM;
  421.   break;
  422.   case pN:
  423.   if(previousPoint = pK){
  424.     if(nextPoint = pO){
  425.       turnLeft();
  426.     }
  427.     else if(nextPoint = pM){
  428.       turnRight();
  429.     }
  430.   }
  431.   else if(previousPoint = pO){
  432.     if(nextPoint = pK){
  433.       turnRight();
  434.     }
  435.     else if(nextPoint = pM){
  436.       moveForward();
  437.     }
  438.   }
  439.   else if(previousPoint = pM){
  440.     if(nextPoint = pK){
  441.       turnLeft();
  442.     }
  443.     else if(nextPoint = pO){
  444.       moveForward();
  445.     }
  446.   }
  447.   previousPoint = pN;
  448.   break;
  449.   case pO:
  450.   if(previousPoint = pJ){
  451.     if(nextPoint = pN){
  452.       turnRight();
  453.     }
  454.   }
  455.   else if(previousPoint = pN){
  456.     if(nextPoint = pJ){
  457.       turnLeft();
  458.     }
  459.   }
  460.   previousPoint = pO;
  461.   break;
  462.   case pP:
  463.   if(previousPoint = pA){
  464.     if(nextPoint = pG){
  465.       turnLeft();
  466.     }
  467.     else if(nextPoint = pM){
  468.       moveForward();
  469.     }
  470.   }
  471.   else if(previousPoint = pG){
  472.     if(nextPoint = pA){
  473.       turnRight();
  474.     }
  475.     else if(nextPoint = pM){
  476.       turnLeft();
  477.     }
  478.   }
  479.   else if(previousPoint = pM){
  480.     if(nextPoint = pA){
  481.       moveForward();
  482.     }
  483.     else if(nextPoint = pG){
  484.       turnRight();
  485.     }
  486.   }
  487.   previousPoint = pP;
  488.   break;
  489. }
  490. else{
  491.   Motor.stop(MOTOR1);
  492.   Motor.stop(MOTOR2);
  493.   Serial.print("Vous êtes arrivés!")
  494. }
  495. }
  496.  
  497.  
  498.  
  499. /*if(previousPoint = nextNorth){
  500.   if(nextPoint = nextSouth){
  501.     moveForward();
  502.   }
  503.   if(nextPoint = nextWest){
  504.     turnRight();
  505.   }
  506.   if(nextPoint = nextEast){
  507.     turnLeft();
  508.   }
  509. }
  510. else if(previousPoint = nextSouth){
  511.   if(nextPoint = nextNorth){
  512.     moveForward();
  513.   }
  514.   if(nextPoint = nextWest){
  515.     turnLeft();
  516.   }
  517.   if(nextPoint = nextEast){
  518.     turnRight();
  519.   }
  520. }
  521. else if(previousPoint = nextEast){
  522.   if(nextPoint = nextNorth){
  523.     turnRight();
  524.   }
  525.   if(nextPoint = nextSouth){
  526.     turnLeft();
  527.   }
  528.   if(nextPoint = nextWest){
  529.     moveForward();
  530.   }
  531. }
  532. else if(previousPoint = nextWest){
  533.   if(nextPoint = nextSouth){
  534.     turnRight();
  535.   }
  536.   if(nextPoint = nextNorth){
  537.     turnLeft();
  538.   }
  539.   if(nextPoint = nextEast){
  540.     moveForward();
  541.   }
  542. }*/
  543.  
  544. //Les situations des capteurs
  545. bool isMoveForwardOnly(){
  546.   if(!etatCapteurGauche && etatCapteurCentreG && etatCapteurCentreD && !etatCapteurDroit)
  547.   {
  548.     return true;
  549.   }
  550.   return false;
  551. }
  552.  
  553. bool tooFarRight(){
  554.   if(!etatCapteurGauche && etatCapteurCentreG && !etatCapteurCentreD && !etatCapteurDroit)
  555.   {
  556.     return true;
  557.   }
  558.   return false;
  559. }
  560. bool tooFarLeft(){
  561.   if(!etatCapteurGauche && !etatCapteurCentreG && etatCapteurCentreD && !etatCapteurDroit)
  562.   {
  563.     return true;
  564.   }
  565.   return false;
  566. }
  567. bool offRoad(){
  568.   if(!etatCapteurGauche && !etatCapteurCentreG && !etatCapteurCentreD && !etatCapteurDroit){
  569.     return true;
  570.   }
  571.   return false;
  572. }
  573. class movement {
  574.  
  575.   public :
  576.   char Intersec ;
  577.   char NextIntersec;
  578.  
  579.   public :
  580.   String Move;
  581. //Correction à droite
  582.   void correctRight(){
  583.     while(!isMoveForwardOnly())
  584.     {
  585.       Motor.speed(MOTOR1, 100);
  586.       Motor.speed(MOTOR2, -25);
  587.     }
  588.   }
  589. //Correction à gauche
  590.   void correctLeft(){
  591.     while(!isMoveForwardOnly())
  592.     {
  593.       Motor.speed(MOTOR1, -25);
  594.       Motor.speed(MOTOR2, 100);
  595.     }
  596.   }
  597. //S'arrêter
  598.   void Stop(){
  599.     Motor.stop(MOTOR1);
  600.     Motor.stop(MOTOR2);
  601.   }
  602. //Reculer
  603.   void moveBack(){
  604.     Motor.speed(MOTOR1, -50);
  605.     Motor.speed(MOTOR2, -50);
  606.   }
  607. //Avancer
  608.   void moveForward(){
  609.     while(isMoveForwardOnly()){
  610.     Motor.speed(MOTOR1, 100);
  611.     Motor.speed(MOTOR2, 100);    
  612.     }
  613.   }
  614. //Tourner à gauche
  615.   void turnLeft(){
  616.     while(!isMoveForwardOnly()){
  617.       Motor.speed(MOTOR1, -50);
  618.       Motor.speed(MOTOR2, 50);
  619.     }
  620.   }
  621. //Tourner à droite
  622.   void turnRight(){
  623.     while(!isMoveForwardOnly()){
  624.       Motor.speed(MOTOR1, 50);
  625.       Motor.speed(MOTOR2, -50);
  626.     }
  627.   }
  628. };
  629.  
  630. enum move {
  631.  
  632.   RIGHT,
  633.   LEFT
  634.  
  635. };
  636.  
  637. class calcul{
  638.   public :
  639.  
  640.   double calculDistance(double xFirstPoint, double yFirstPoint, double xSecondPoint, double ySecondPoint){
  641.  
  642.       double res;
  643.       res=sqrt((xSecondPoint-xFirstPoint)*(xSecondPoint-xFirstPoint)+(ySecondPoint-yFirstPoint)*(ySecondPoint-yFirstPoint));
  644.       return res;
  645.    
  646.   }
  647.  
  648.   double calculTemps(double distance){
  649.  
  650.   double res;
  651.  
  652.   res=(distance/vitesseVoiture);
  653.  
  654.   return res;
  655.    
  656.   }
  657. };
  658.  
  659. class voiture {
  660.  
  661.   void pointAller(Point* pAvant, Point* pPresent){
  662.  
  663.  
  664.    
  665.   }
  666.  
  667.  
  668. };
  669.  
  670.  
  671.  
  672.  
  673.  
  674. void setup() {
  675.    Serial.begin(9600);
  676.   pinMode(capteurGauche, INPUT);
  677.   pinMode(capteurCentreD, INPUT);
  678.   pinMode(capteurCentreG, INPUT);
  679.   pinMode(capteurDroit, INPUT);
  680.   Motor.begin(I2C_ADDRESS);
  681.   map();
  682. };
  683. calcul* cal = new calcul();
  684.  
  685. void loop() {
  686.  
  687.   etatCapteurGauche = digitalRead(capteurGauche);
  688.   etatCapteurCentreD = digitalRead(capteurCentreD);
  689.   etatCapteurCentreG = digitalRead(capteurCentreG);
  690.   etatCapteurDroit = digitalRead(capteurDroit);
  691.  
  692.   timeDelay = distance / averageSpeed;
  693.  
  694. double resDistance, resTemps;
  695.  
  696. double xA,yA,xB,yB;
  697. xA=pA->getX();
  698. Serial.print("xA : ");
  699. Serial.println(pA->getX());
  700. Serial.print("yA : " );
  701. Serial.println(pA->getY());
  702. Serial.print("xB : ");
  703. Serial.println(pB->getX());
  704. Serial.print("yB : ");
  705. Serial.println(pB->getY());
  706.  
  707. resDistance=cal->calculDistance(pA->getX(),pA->getY(),pB->getX(),pB->getY());
  708. Serial.print("La distance entre A et B est de : ");
  709. Serial.println(resDistance);
  710.  
  711.  
  712. resTemps=cal->calculTemps(resDistance);
  713. Serial.print("Le temps pour aller de A à B est de : ");
  714. Serial.println(resTemps);
  715.  
  716. delay(50000);
  717.   if (isMoveForwardOnly()){
  718.     move->moveForward();
  719.   }
  720.   else if(tooFarRight()){
  721.     move->correctLeft();
  722.   }
  723.   else if(tooFarLeft()){
  724.     move->correctRight();
  725.   }
  726.   else if(offRoad()){
  727.     move->Stop();
  728.   }
  729.  
  730. };
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!