FRC_equipe1 / Mbed 2 deprecated FRC_2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Classes/Robot.cpp@29:6bce50d6530c, 2019-06-07 (annotated)

Committer:
Wael_H
Date:
Fri Jun 07 09:30:46 2019 +0000
Revision:
29:6bce50d6530c
Parent:
28:82571fd665bf
Child:
32:84bcb8f2667a
version propre pour tout le monde avec 3 balles

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wael_H 10:efa507ba2b35 1 #include "mbed.h"
Wael_H 10:efa507ba2b35 2 #include "Robot.h"
Wael_H 10:efa507ba2b35 3 #include "CAN_asser.h"
Wael_H 10:efa507ba2b35 4
Wael_H 10:efa507ba2b35 5 #include "Deplacement.h"
Wael_H 10:efa507ba2b35 6 #include "Laser.h"
Wael_H 10:efa507ba2b35 7 #include "Rouleau.h"
Wael_H 10:efa507ba2b35 8 #include <string>
Wael_H 10:efa507ba2b35 9
Wael_H 10:efa507ba2b35 10 using namespace std;
Wael_H 10:efa507ba2b35 11
Wael_H 19:dad67302af25 12 DigitalIn Robot::Jack(PA_15);
Wael_H 10:efa507ba2b35 13
theolp 20:a1b5d032b422 14 Robot::Robot() : laserG(Laser(PB_1)), laserD(Laser(PC_5)), laserA(Laser(PC_4)),
theolp 20:a1b5d032b422 15 rouleau(Rouleau(D5, D4, D6)), /*Pins order : PWM, Ejecte, Gobe*/
theolp 20:a1b5d032b422 16 BPs(DigitalIn(PC_2, PullDown)),
Wael_H 23:bb1535360a98 17 CNY_G(AnalogIn(A2)), CNY_D(AnalogIn(A3)), //Infos plus haut pour les pins
Wael_H 23:bb1535360a98 18 Cerveau_Bras(PwmOut(PB_7)), Disquette(PwmOut(PB_6))
Wael_H 10:efa507ba2b35 19 {
Wael_H 10:efa507ba2b35 20 can_init();
Wael_H 19:dad67302af25 21 this->setPos(500,0,0);
Wael_H 29:6bce50d6530c 22 this->setSpeed(80,450); // vitesse de rotation de recherche de la balle
Wael_H 23:bb1535360a98 23
Wael_H 23:bb1535360a98 24 this->Cerveau_Bras.period_ms(20);
Wael_H 26:fa8a8fa175cb 25 this->Cerveau_Bras = POS_BAS;
Wael_H 23:bb1535360a98 26
Wael_H 23:bb1535360a98 27 this->Disquette.period_ms(20);
Wael_H 10:efa507ba2b35 28 }
Wael_H 10:efa507ba2b35 29
Wael_H 10:efa507ba2b35 30 bool Robot::aBalle()
Wael_H 10:efa507ba2b35 31 {
Wael_H 10:efa507ba2b35 32 return BPs.read();
Wael_H 10:efa507ba2b35 33 }
Wael_H 10:efa507ba2b35 34
Wael_H 19:dad67302af25 35 /// Fonctions Odometrie ///
Wael_H 19:dad67302af25 36 void Robot::setPos(unsigned short x, unsigned short y, signed short theta) const
Wael_H 19:dad67302af25 37 {
Wael_H 19:dad67302af25 38 SetOdometrie(ODOMETRIE_POSITION, y, x, theta); // Oui les x et les y sont inversés...
Wael_H 19:dad67302af25 39 }
Wael_H 19:dad67302af25 40
Wael_H 10:efa507ba2b35 41 /// retourne l'adresse de la premiere case d'un tableau contenant dans l'ordre la position en x et en y et l'angle theta ///
theolp 20:a1b5d032b422 42 short Robot::pos(T_POS pos)
Wael_H 10:efa507ba2b35 43 {
theolp 20:a1b5d032b422 44 return this->deplacement.getPos(pos);
Wael_H 16:05665faaa489 45 }
Wael_H 16:05665faaa489 46
Wael_H 27:e103da412e2b 47 bool Robot::immobile()
Wael_H 27:e103da412e2b 48 {
Wael_H 27:e103da412e2b 49 return this->deplacement.immobile();
Wael_H 27:e103da412e2b 50 }
Wael_H 27:e103da412e2b 51
Wael_H 10:efa507ba2b35 52
Wael_H 10:efa507ba2b35 53 /// Toutes les fonctions de déplacement ///
Wael_H 15:3d4543a6c100 54 bool Robot::avance(signed short dist)
Wael_H 10:efa507ba2b35 55 {
Wael_H 10:efa507ba2b35 56 return deplacement.avance(dist);
Wael_H 10:efa507ba2b35 57 }
Wael_H 10:efa507ba2b35 58
Wael_H 15:3d4543a6c100 59 bool Robot::tourne(signed short angle)
Wael_H 10:efa507ba2b35 60 {
Wael_H 10:efa507ba2b35 61 return deplacement.tourne(angle);
Wael_H 10:efa507ba2b35 62 }
Wael_H 10:efa507ba2b35 63
Wael_H 15:3d4543a6c100 64 bool Robot::GoToXYT(unsigned short x, unsigned short y, unsigned short t, signed char sens) //sens = 0 par défaut
Wael_H 10:efa507ba2b35 65 {
Wael_H 10:efa507ba2b35 66 return deplacement.GoToXYT(x,y,t,sens);
Wael_H 10:efa507ba2b35 67 }
Wael_H 10:efa507ba2b35 68
Wael_H 15:3d4543a6c100 69 void Robot::setSpeed(unsigned short vitesse, unsigned short acceleration) const
Wael_H 10:efa507ba2b35 70 {
Wael_H 10:efa507ba2b35 71 SendSpeed(vitesse,acceleration);
Wael_H 10:efa507ba2b35 72 }
Wael_H 10:efa507ba2b35 73
Wael_H 13:9c62e263f245 74 void Robot::stop()
Wael_H 13:9c62e263f245 75 {
Wael_H 13:9c62e263f245 76 deplacement.stop();
Wael_H 13:9c62e263f245 77 }
Wael_H 13:9c62e263f245 78
Wael_H 10:efa507ba2b35 79
Wael_H 10:efa507ba2b35 80 /// Fonction distance lasers ///
Wael_H 13:9c62e263f245 81 float Robot::getDist(int laserENUM, string unite) //="cm" par défaut
Wael_H 10:efa507ba2b35 82 {
Wael_H 10:efa507ba2b35 83 switch(laserENUM) {
Wael_H 13:9c62e263f245 84 case Laser::Gauche:
Wael_H 10:efa507ba2b35 85 return this->laserG.getDist(unite);
Wael_H 10:efa507ba2b35 86
Wael_H 13:9c62e263f245 87 case Laser::Droit:
Wael_H 10:efa507ba2b35 88 return this->laserD.getDist(unite);
Wael_H 10:efa507ba2b35 89
Wael_H 13:9c62e263f245 90 case Laser::Arriere:
Wael_H 10:efa507ba2b35 91 return this->laserA.getDist(unite);
Wael_H 10:efa507ba2b35 92 }
Wael_H 10:efa507ba2b35 93
Wael_H 10:efa507ba2b35 94 return 0;
Wael_H 10:efa507ba2b35 95 }
Wael_H 10:efa507ba2b35 96
theolp 20:a1b5d032b422 97 /// Fonction CNY surBlanc ou pas ///
theolp 20:a1b5d032b422 98 bool Robot::surBlanc( T_CNY cny )
theolp 20:a1b5d032b422 99 {
Wael_H 29:6bce50d6530c 100 static const float SEUIL_CNY_G = 0.5f,
Wael_H 29:6bce50d6530c 101 SEUIL_CNY_D = 0.5f;
Wael_H 25:033263cf832c 102
Wael_H 26:fa8a8fa175cb 103 //dbug.printf("Gauche : %f\n\rDroit : %f\n\r",this->CNY_G.read(), this->CNY_D.read());
theolp 21:9ff2eab240fb 104
theolp 20:a1b5d032b422 105 if( cny == Robot::CNY_GAUCHE )
Wael_H 26:fa8a8fa175cb 106 return ( this->CNY_G.read() >= SEUIL_CNY_G );
theolp 20:a1b5d032b422 107
theolp 20:a1b5d032b422 108 else if( cny == Robot::CNY_DROIT )
Wael_H 26:fa8a8fa175cb 109 return ( this->CNY_D.read() >= SEUIL_CNY_D );
theolp 20:a1b5d032b422 110
theolp 20:a1b5d032b422 111 else return false;
theolp 20:a1b5d032b422 112 }
Wael_H 10:efa507ba2b35 113
Wael_H 10:efa507ba2b35 114 /// Fonctions Rouleau ///
Wael_H 10:efa507ba2b35 115 void Robot::ejecte(int vitesse) // vitesse max = 100
Wael_H 10:efa507ba2b35 116 {
Wael_H 10:efa507ba2b35 117 rouleau.ejecte(vitesse);
Wael_H 10:efa507ba2b35 118 }
Wael_H 10:efa507ba2b35 119
Wael_H 10:efa507ba2b35 120 void Robot::gobe(int vitesse) // vitesse max = 100
Wael_H 10:efa507ba2b35 121 {
Wael_H 10:efa507ba2b35 122 rouleau.gobe(vitesse);
Wael_H 10:efa507ba2b35 123 }
Wael_H 10:efa507ba2b35 124
Wael_H 10:efa507ba2b35 125 void Robot::stopRouleau()
Wael_H 10:efa507ba2b35 126 {
Wael_H 10:efa507ba2b35 127 rouleau.stop();
Wael_H 10:efa507ba2b35 128 }
Wael_H 23:bb1535360a98 129
Wael_H 23:bb1535360a98 130 /// Bras ///
Wael_H 23:bb1535360a98 131 bool Robot::leveBras()
Wael_H 23:bb1535360a98 132 {
Wael_H 23:bb1535360a98 133 static double posCerveau;
Wael_H 23:bb1535360a98 134 posCerveau = this->Cerveau_Bras;
Wael_H 23:bb1535360a98 135
Wael_H 26:fa8a8fa175cb 136 if(posCerveau <= POS_HAUT)
Wael_H 23:bb1535360a98 137 {
Wael_H 26:fa8a8fa175cb 138 posCerveau += PAS;
Wael_H 23:bb1535360a98 139 this->Cerveau_Bras = posCerveau;
Wael_H 23:bb1535360a98 140 return false;
Wael_H 23:bb1535360a98 141 }
Wael_H 23:bb1535360a98 142 else return true;
Wael_H 23:bb1535360a98 143 }
Wael_H 23:bb1535360a98 144
Wael_H 23:bb1535360a98 145 bool Robot::baisseBras()
Wael_H 23:bb1535360a98 146 {
Wael_H 23:bb1535360a98 147 static double posCerveau;
Wael_H 23:bb1535360a98 148 posCerveau = this->Cerveau_Bras;
Wael_H 23:bb1535360a98 149
Wael_H 26:fa8a8fa175cb 150 if(posCerveau >= POS_BAS)
Wael_H 23:bb1535360a98 151 {
Wael_H 26:fa8a8fa175cb 152 posCerveau -= PAS;
Wael_H 23:bb1535360a98 153 this->Cerveau_Bras = posCerveau;
Wael_H 23:bb1535360a98 154 return false;
Wael_H 23:bb1535360a98 155 }
Wael_H 23:bb1535360a98 156 else return true;
Wael_H 23:bb1535360a98 157 }
Wael_H 23:bb1535360a98 158
Wael_H 23:bb1535360a98 159 void Robot::exploseBallon()
Wael_H 23:bb1535360a98 160 {
Wael_H 23:bb1535360a98 161 this->Disquette.write(0.5);
Wael_H 23:bb1535360a98 162 }
Wael_H 23:bb1535360a98 163
Wael_H 23:bb1535360a98 164 void Robot::arreteDisquette()
Wael_H 23:bb1535360a98 165 {
Wael_H 23:bb1535360a98 166 this->Disquette.write(0);
Wael_H 23:bb1535360a98 167 }