COUPE ROBOTIQUE DES IUT
Le wiki est actuellement en rédaction, certaines parties sont donc partiellement rédigées ou incomplètes.
Sommaire
COUPE ROBOTIQUE 2018
Introduction
Pour la deuxième fois consécutive l'IUT de Troyes auxquelles nous appartenons va participé à la Coupe de France Robotique des Iut qui se déroule chaque années du Jeudi 9 au samedi 11 Juin lors du Festival de Robotique dans la ville de Cachan. L'année dernière le But de la Coupe étais de se déplacé d'un point A à un point B en évitant des obstacles. Cette année l'objectif est radicalement différent puisqu'il s'agit de joué au Tennis !
Nous avons donc développé un robot répondant à une problématique et à un cahier des charges bien précis fixé par les organisateurs de la coupe. Je vous invite à consulté le règlement afin de bien comprendre le principe de la coupe de cette année.
Vous pouvez retrouvé ce règlement ici
Cette année nous devons donc joué au tennis, voici donc un bref résumé du déroulement de la coupe :
-Deux robots se font face sur un terrain de Tennis de dimension 8*4 m.
-Chaque robot à sur sont terrains un certain nombre de balles de Tennis par terre.
-Le centre de la piste est délimité par une ligne.
-Il y a deux ballons de chaque côtés des deux camps leurs explosions déclenche un surplus de balle dans le camp adverse.
-Un match dure 90 secondes.
-A la fin des 90 secondes l'envoi d'un projectile autre qu'une balle dans le camps adverses ajoute une balle supplémentaires dans le terrain adverses.
Il Faut donc avoir le moins de balles possibles dans sont camps à la fin de la partie.
Voici un aperçu du terrain:
Etudes
Il y a donc de nombreux But à atteindre, nous avons donc à l'aide d'un diagramme fonctionnelle décomposé tous ces Buts en Tâches à effectuées:
Cela nous à permis de définir les tâches que le robot doit effectué et les solutions techniques associées :
-Se déplacé => Deux moteurs commandés par un micro controleur Atmega 2560 via un pont en H l298n.
-Détecté les balles de Tennis => Première Caméra Pixy.
-Détecté la balise pour orienté le robot => Seconde Caméra Pixy.
-orientation de la deuxième caméra pixy => Moteur pas à pas.
-Détecté la proximité avec la balle et l'attrapée => Capteur de distance SHARP et Bras préhenseur.
-Propulsé la balle dans le camps adverse => Verrin.
-Atteindre les ballons situés en bordure du terrain => Bras rotatif.
-Faire éclaté les ballons en bordure du terrain => Arc électrique supporté par le bras rotatif nommé précédemment.
-Détecté les délimitations centrale du terrain => Capteur Cny70.
-Détecté la proximité avec les parois du terrain => Capteurs temps de vols.
Principe de fonctionnement
Comme nous l'avons vus précédemment il y a de nombreuses taches à effectuées, elles doivent toutes êtres réalisées en synchronisme cela implique un fonctionnement assez complexe que je vais vous expliquer.
Tous d'abord la direction, comment le robot se dirige t'il ? Le robot doit repérer les balles et les attrapées, pour ce faire la caméra pixy située à l'avant du robot va repérer les balles, selon la position de la balle dans le champ de vision de la caméra, c'elle si va renvoyé des informations à notre microcontrôleur. Le programme que contient le microcontrôleur va commandé les moteurs dans le but de maintenir la balle au centre du champs de vision de la caméra. Il va également tâché de faire grossir la balle dans l'objectif de la caméra, c'est à dire ce rapprocher de la balle.
Une fois la balle arrivée à une certaine distance de la caméra (la balle remplis quasiment l'objectif de la caméra) un capteur de distance infrarouge Sharp va détecter la présence de la balle à l'endroit voulus, le bras va ensuite ce refermer pour "capturé" la balle. Pendant toutes cette période la deuxième caméra Pixy n'est pas restée inactive, en effet pendant ce temps elle fixe en permanence une balise émettant une lumière rouge que nous avons préalablement posé en fin de piste c'est à dire à l'arrière de notre camps. Nous pouvons donc déterminé en fonction des mouvements que notre deuxième caméra pixy à effectué ou se situe l'arrière de notre terrain.
Nous pouvons donc déterminé la direction dans laquelle notre robot doit envoyé la balle !
Realisation
PREMIER PROTOTYPE
Les différents éléments
Nous avons choisi de concevoir un chassi qui pourrais accueillir tous nos composants, nous ne savions pas à ce moment la leurs dimensionnement.
Nous avons donc découpé en fonction du besoins des plaques de plastiques que nous avons assemblé.
Vous pouvez voir dans la photo précédente le robot complètement assemblé.
Ce robot ce déplace grâce à deux moteur fournis par les organisateurs de la coupe.
Ce sont deux moteurs Dunkermotoren G 42*25 qui permettent au robot de se mouvoir, vous pourrez retrouvé toutes leurs caractéristiques techniques ici
Ces moteurs et l'intégralités des dispositifs du robot sont alimentés par une batterie également définie par les organisateur de la coupe :
C'est une batterie 12 V avec une capacité de 7 Ah.
Déplacement:
Ces deux moteurs sont donc contrôlés par un pont en H l298n, vous pourrez trouvez toutes les informations utiles concernant ce composant ici
Ce composant serras placé sur la carte principale du robot, en effet au lieu de faire une carte pour la gestion des moteurs, une autre pour la gestion des caméras etc nous avons choisi de tous centralisé sur un seul shield d'une arduino mega.
Vous pouvez également distingué sur le Robot les deux caméras Pixys, une à l'avant, une autre à l'arrière pour la détection de la Balle, une description plus précise des Pixys serras faites plus en Avale en effet elles seront abordés dans la partie traitant du second prototype.
Vous pouvez voir également à l'avant du Robot le bras servant à attrapé la balle.
Ce bras à été imprimé à l'aide de l'imprimante 3D de l'Iut, il épouse parfaitement les dimensions de la balle, une version améliorée de ce bras à été fabriqué pour le deuxième prototype. Ce bras à été réalisé à l'aide d'une logiciel Freecad, ce logiciel libre est facile à prendre en mains c'est pour cela que nous l'avons préférés à Solydworks (même si les futures pièces seront surement faites à l'aide de ce dernier logiciel). Pour crée cette pièce nous avons crée une esquisse 2D puis à l'aide de protusion et de perçage nous obtenons une pièce en 3 dimensions. Il suffit ensuite de l'exporter sous un fichier "MESH" que l'imprimante 3D peux lire. Il existe ensuite plusieurs logiciels pouvant lire ce fichier, pour notre part nous disposions de deux imprimantes, les pièces les plus petites ont été réalisées à l'aide du logiciel Repetier Host et de l'imprimante 3D microDelta original et les pièces les plus grandes ont été réalisées grâce au logiciel Cura et à l'imprimante 3D Witbox
Bras préhenseur:
Les capteurs Cny70:
Ces capteurs à pour but de détecter le milieu du terrain en effet notre robot n'as pas à empiéter sur le terrain de l'adversaire, nous avons donc conçu des petites cartes avec le capteurs et ce qui est nécessaire à sont fonctionnement dessus, seul bé mole nous allons devoirs inclinés les Cny en effet notre robot ne doit pas dépasser la ligne, nous devons donc la détecté à l'avance. Encore une fois si vous avez besoins de précision concernant ce photo transistor suivez ce lien : ici
Le moteur Pas à Pas
Dans ce projet, on a voulu mettre un moteur pas à pas pour la rotation de la caméra pixy derrière afin de pouvoir tourner à 360° en effet les servos moteurs présent sur la caméra pixy ne nous permettent de tournés seulement à 180°. On a donc utilisé un contrôleur de moteur pas à pas TCA 3727.
Le câblage du contrôleur TCA 3727 se trouve ci-dessous.
On peut commander le moteur en mode Full Step ou Half Step en envoyant des signaux à des entreés I10 I11 I20 I21. Exemple du full step :
Le générateur d'arc électriques
Il est invisible ici car il est situé à l'intérieur du robot, ce petit générateur produit des petits arcs électriques en ionisant l'air, il est commandé par un transistor Mosfet type N. Nous avons mesuré qu'il consomme au maximum de sont fonctionnement jusqu’à 3 Ampère, en effet plus l'arc électrique est grand plus la consommation de courant est grande. Après avoir effectué plusieurs tests nous en avons conclus que cette arc électrique étais suffisant pour faire éclater un ballon.
Les capteurs temps de vols
Nous allons être amené à nous déplacé sur le terrain , il nous faut donc pouvoir en détecter les limites, comme nous l'avons déjà vus nous avons les Cny pour repérer la ligne délimitant le milieu du terrain mais nous avons aussi besoins de capteurs capables de détecter les rebords du terrain. Pour ce faire nous utiliserons des capteurs temps de vols VL53L0X. Vous pourrez trouvez toutes les informations concernant ce capteur. ici
Le système pneumatique
Pour propulsé la balle nous utiliserons donc un vérin, nous utiliserons un vérin mis à disposition par notre enseignant.
Ce vérin à besoins pour fonctionner d'une réserve d'air comprimée, ne pouvant évidement pas monté un compresseur sur notre petit robot nous utiliserons des cartouches de co2 de 12 G avec tige filetée, elles ont l'avantages non négligeable d'être facilement transportable.
Nous commanderons le fonctionnement de notre vérin à l'aide d'une électrovanne également fournie par notre enseignant, nous sommes actuellement à la recherche d’électrovannes plus petites.
Voici quelques photos du système pneumatique :
On peut voir le régulateur de pression à l'arrière du robot et le vérin démonté :
Sur cette photo l'on peut voir l'ensemble du système pneumatique câblé, nous avons donc notre électrovanne commandé par la carte via un survolteur 12 -> 24V. Notre électrovanne est notre interrupteur dans notre circuit pneumatique.
Le shield arduino mega
Pour commandé tous ces actionneurs que ce soit les moteurs ou les bras servants à percés les ballons ou à attrapé les balles, ainsi que pour centralisé et réagir aux informations fournis par nos nombreux capteurs nous avons besoins d'un micro contrôleur avec un nombre de pattes et un nombre de timers important. Nous avons donc choisi le micro contrôleur Atmega 2560. En effet il réponds parfaitement à nos attentes, cependant nous devons également mettre en oeuvre un pont en H et plusieurs transistors mosfet qui ne sont pas présent sur la carte arduino Mega. Nous allons donc créée un shield à cette carte. Le shield le voici :
C'est le premier shield que nous avons réalisés cependant nous avons rencontrés de nombreuses difficultés lors de sa fabrication, ces difficultés sont principalement du à un routage qui n’étais pas optimum, de très nombreux via ainsi que des piste dont la disposition ne facilitais pas le routage. Par exemple certains composants rendaient difficile le soudage des autres notamment à cause du manque de place. Cela rajouté à des erreurs de soudures ont rendus les essais de fonctionnements de cette cartes très laborieux. Finalement après une semaine à essayer de faire fonctionné correctement cette carte en vain nous avons décidé de fabriqué une version améliorée de celle si.
Voici donc la version corrigée de ce premier shield, le routage à été simplifié autant que possible, des nombreuses erreurs ont été corrigés et la disposition des pistes à été optimisé autant que possible. Nous avons également abordé la fabrication d'une manière différente en tirant les enseignement des erreurs de fabrication du premier shield. Par exemple nous avons soudé les composants traversants dans un ordre bien précis pour ne pas être gêner lors du soudage de ceux de l'un ou de l'autre.
Voici donc le routage finale :
Et voici la carte fabriqué :
La carte principale finale
Après avoir testé et utilisé le shield arduino fabriqué pour le premier prototype, il existait toujours des problèmes qui provoquaient un mauvais fonctionnement du robot. De plus, on voulait ajouter certains composants afin d’améliorer notre robot. Pour ce faire, on a décidé de créer une carte principale en utilisant le micro-contrôleur ATMEGAG 2560 qui se trouve sur la carte Arduino MEGA pour fabriquer une carte qui est à la fois plus compatible et a beaucoup plus de places dans le but d’installer les autres dispositifs nécessaires (module XBEE, les transistors, etc). Cette carte a été fabriquée en tirant les enseignements des erreurs de fabrication des cartes précédente et le routage est optimisé le plus possible pour avoir une carte de qualité et facile à dépanner et tester. Les entrées/sorties non utilisées sont aussi reliées à des connecteurs MOLEX pour un ajout complémentaire des modules en tant que les capteurs, les modules supplémentaires dans le futur sans le besoin de refabriquer une nouvelle carte. Par contre, lors de la fabrication de cette carte principale, on a eu aussi pas mal de problème avec la mise en place du micro -contrôleur sur la carte.
Voici le routage de la carte :
Et voici la carte finie:
Modèle:Thor tank lors de la coupe robotique
SECOND PROTOTYPE
Le Chassi en PLA
Nous avons décidé à la suite de la réalisation du premier prototype de tenir compte au maximum des nécessité techniques que devais remplir le chassi, nous avons donc crée un châssi robuste en PLA sur lequel chacun des actionneurs serait fixé rapidement. Nous l'avons dessiné sur Freecad et fabriqué à l'aide de l'imprimante 3D de l'iut. Il y a eu 3 versions différentes du châssi tous au long du projet voici donc quelques photos du chassi le plus récent une fois terminé : Sur freecad :
Et enfin imprimé :
Les deux caméras pixy
La caméra Pixy est une caméra intelligente laquelle, principalement, fait un traitement d'images des objets qu'on garde dans leur mémoire. Cette caméra nous donne différentes informations telles que: les coordonnées X, Y du centre de l'objet, la largeur ainsi que la hauteur du carreau autour l'objet, la quantité d'objets détectés et leurs couleurs parmi des autres données. Vous pouvez avoir plus d'information ici http://www.cmucam.org/projects/cmucam5/wiki/Arduino_API
Pour ce projet on utilise deux caméras : La caméra positionnée devant du robot sera qui détecte les balles, également les deux ballons à coté du terrain et celle qui est derrière sera capable de nous donner l’angle entre la direction du robot et l’arrière du terrain. Cette dernière donnée nous aidera à envoyer les balles vers la direction correcte!
Pour la première caméra, elle est monté à l’inverse(important pour la programmation puisque cela change l’orientation de l’axe X!) avec qu’un seul servomoteur qui contrôle le mouvement en l’axe Y. Ce mouvement est fait simplement, tout d’abord pour la détection des balles du sol et après pour la détection des ballons. Pourtant, la deuxième est montée sur deux servomoteurs qui contrôle le mouvement dans les deux axes. Il se trouve que si le robot est près de la balise on risque de la perdre, c’est pour cela qu’on utilise le mouvement en Y.
La carte principale
CODE
Code incomplet
/*
Program challenge "Coupe Robotique des IUT 2018"
IUT de Troyes
June - 2018
*/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <Wire.h>
#include <SPI.h>
#include <PixySPI_SS.h>
#include "Adafruit_VL53L0X.h"
#include <ServoTimer2.h>
#define topPWM 255 //Valeur Max du PWM Motors (8bits)
#define X_center 160 //PixyBalls consigne
#define sensor_ball 0 //sensor ball
#define S1 1 //CNY70 left1
#define S2 2 //CNY70 right1
#define S3 3 //CNY70 left2
#define S4 4 //CNY70 right2
#define PixyBalls 22 // camera for detect balls
#define PixyDirection 23 // camera for the direction of the robot
#define Eject_ball 36 // Eject ball
#define Eject_cloth 38 // Eject cloth (pas utilisé)
#define Explode_balloons 34 // set servo to explode balloons
//servos Pixy direction
#define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2)
#define Y_CENTER ((PIXY_MAX_Y-PIXY_MIN_Y)/2)
const int limit = 90;
bool start = false;
bool balise = false;
int s1 = 0;
int s2 = 0;
int s3 = 0;
int s4 = 0;
float new_s1 = 0;
float new_s2 = 0;
float new_s3 = 0;
float new_s4 = 0;
int signature = 0;
int timer = 0;
int16_t size_max_balle = 0;
int16_t size_max_ballon = 0;
int16_t size = 0;
bool caught_ball = false;
//Cameras
PixySPI_SS pixy1(PixyBalls);
PixySPI_SS pixy2(PixyDirection);
//Capteurs temps de vol
Adafruit_VL53L0X sensor1 = Adafruit_VL53L0X();
Adafruit_VL53L0X sensor2 = Adafruit_VL53L0X();
Adafruit_VL53L0X sensor3 = Adafruit_VL53L0X();
Adafruit_VL53L0X sensor4 = Adafruit_VL53L0X();
//Servomoteurs
ServoTimer2 ball; // servo to catch balls
ServoTimer2 arm; // servo to move the arm to explode balloons
ServoTimer2 ball_pixy;
////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
// Pantilt camera direction
///////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
class ServoLoop
{
public:
ServoLoop(int32_t pgain, int32_t dgain);
void update(int32_t error);
int32_t m_pos;
int32_t m_prevError;
int32_t m_pgain;
int32_t m_dgain;
};
ServoLoop panLoop(300, 500);
ServoLoop tiltLoop(500, 700);
ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
m_pos = PIXY_RCS_CENTER_POS;
m_pgain = pgain;
m_dgain = dgain;
m_prevError = 0x80000000L;
}
void ServoLoop::update(int32_t error)
{
long int vel;
char buf[32];
if (m_prevError != 0x80000000)
{
vel = (error * m_pgain + (error - m_prevError) * m_dgain) >> 10;
//sprintf(buf, "%ld\n", vel);
//Serial.print(buf);
m_pos += vel;
if (m_pos > PIXY_RCS_MAX_POS)
m_pos = PIXY_RCS_MAX_POS;
else if (m_pos < PIXY_RCS_MIN_POS)
m_pos = PIXY_RCS_MIN_POS;
}
m_prevError = error;
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
// PixyDirection
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
void direction()
{
uint16_t blocks;
int32_t panError, tiltError;
blocks = pixy2.getBlocks();
if (blocks)
{
panError = X_CENTER - pixy2.blocks[0].x;
tiltError = pixy2.blocks[0].y - Y_CENTER;
panLoop.update(panError);
tiltLoop.update(tiltError);
pixy2.setServos(panLoop.m_pos, tiltLoop.m_pos);
a balise = true; //balise detectee
}
else balise = false;
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
//SetMotors
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void initMotor()//PWM on PE3 (Left) and PH3(Right)
{
//fpwm = fq / (topPWM * p)
//fpwmi: frequency PWM
//fq: frequency quartz
//topPWM: value max PWM
//p: predivisor
//Declaration de sorties
DDRB |= (1 << PB6);//Sense motor left 12 in1
DDRB |= (1 << PB5);//Sense motor left 11 in2
DDRH |= (1 << PH3);//PWM motor left OC4A 6 enA
DDRH |= (1 << PH4);//Sense motor right 7 in3
DDRB |= (1 << PB4);//Sense motor right 10 in4
DDRH |= (1 << PH5);//PWM motor right OC4C 8 enB
//Timer4 PH3
TCCR4A |= (1 << WGM40);//Mode FAST PWM
TCCR4B |= (1 << WGM42);
TCCR4B |= (1 << CS40) | (1 << CS41);//divisor P=64
TCCR4A |= 1 << COM4A1;//PWM on OC4A
TCCR4A |= 1 << COM4C1;//PWM on OC4C
OCR4A = 0;//Value first comparison --> PH3
OCR4C = 0;//Value first comparison --> PH5
}
void setMotorL(int16_t speed)//function motor left
{
if (speed < 0)
{
speed = -speed;
PORTB |= (1 << PB6);//Motor left : sense "-"
PORTB &= ~ (1 << PB5);
}
else {
PORTB &= ~ (1 << PB6);//Motor left : sense "+"
PORTB |= (1 << PB5);
}
if (speed > topPWM) speed = topPWM;//speed max
OCR4A = speed;//Action sur le PWM --> PE3
}
void setMotorR(int16_t speed)//function motor right
{
if (speed < 0)
{
speed = -speed;
PORTH &= ~ (1 << PH4); //Motor right : sense "-"
PORTB |= (1 << PB4);
}
else
{
PORTH |= (1 << PH4); //Motor left : sense "+"
PORTB &= ~ (1 << PB4);
}
if (speed > topPWM) speed = topPWM;//speed max
OCR4C = speed;//Action sur le PWM --> PH3
}
void setSpeed(int16_t vL, int16_t vR)//only one function for 2 motors
{
setMotorL(vL);
setMotorR(vR);
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// play time = 90s
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void timer_90s()
{
//Tt1=(n*p)/fq
//n=Tt1*fq/p
//Tt1: periode du timer
//n=valuer de comparaison (OCR1A)
//p:prediviseur
//fq:frequence du quark (arduino Mega 16Mhz)
TCCR1B |= (1 << CS10) | (1 << CS12); //p=1024
TCCR1B |= 1 << WGM12; //RAZ mode CTC
TIMSK1 |= 1 << OCIE1A; //Autorisation d'interruption de comparaison A
OCR1A = 15625; //Comparaison , Tt1=1s,
}
ISR(TIMER1_COMPA_vect)
{
timer++;
//PORTB^=1<<PB7; activar verin para la tela blanca
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// setCameras
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void setCameras() {
//pixy2.init();
pinMode(PixyBalls, OUTPUT); // Slave output PixyBalls
pinMode(PixyDirection, OUTPUT); // Slave output PixyDirection
digitalWrite(PixyBalls, HIGH); // desactivation PixyBalls
digitalWrite(PixyBalls, HIGH); // desactivation PixyDirection
pixy1.init();
pixy2.init();
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// PixyBalls
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void Pixy_tracking() {
int signature = 0;
//unsigned int16_t size_max_balle = 0;
//unsigned int16_t size_max_ballon = 0;
uint32_t size_balle = 0;
uint16_t posX = 0;
//static int i = 0;
int blocks;
blocks = pixy1.getBlocks();
// grab blocks!
if (blocks)
{
signature = pixy1.blocks[0].signature;
posX = pixy1.blocks[0].x;
size_balle = pixy1.blocks[0].width; * pixy1.blocks[0].height;
}
/*else
{
posX = 0;
size_balle = 0;
}*/
//Serial.println(sizeta /256)
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Activation capteurs temps de vol protocol I2C
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void configAddress() {
pinMode(44, OUTPUT);
pinMode(45, OUTPUT);
pinMode(46, OUTPUT);
pinMode(47, OUTPUT);
digitalWrite(47, HIGH);
digitalWrite(46, LOW);
digitalWrite(45, LOW);
digitalWrite(44, LOW);
sensor1.setAddress(30);
sensor1.begin(30);
delay(10);
// Serial.println("1 device");
digitalWrite(47, HIGH);
digitalWrite(46, HIGH);
digitalWrite(45, LOW);
digitalWrite(44, LOW);
sensor2.setAddress(31);
sensor2.begin(31);
delay(10);
// Serial.println("2 device");
digitalWrite(47, HIGH);
digitalWrite(46, HIGH);
digitalWrite(45, HIGH);
digitalWrite(44, LOW);
sensor3.setAddress(32);
sensor3.begin(32);
delay(10);
//Serial.println("3 device");
digitalWrite(47, HIGH);
digitalWrite(46, HIGH);
digitalWrite(45, HIGH);
digitalWrite(44, HIGH);
sensor3.setAddress(32);
sensor4.setAddress(33);
sensor4.begin(33);
delay(10);
//Serial.println("4 device");
}
void sensorRead() {
VL53L0X_RangingMeasurementData_t measure1; // mesure prope de la librairie
VL53L0X_RangingMeasurementData_t measure2;
VL53L0X_RangingMeasurementData_t measure3;
VL53L0X_RangingMeasurementData_t measure4;
sensor1.rangingTest(&measure1, false);
sensor2.rangingTest(&measure2, false);
sensor3.rangingTest(&measure3, false);
sensor4.rangingTest(&measure4, false);
s1 = measure1.RangeMilliMeter;
//delay(30);
s2 = measure2.RangeMilliMeter;
//delay(30);
s3 = measure3.RangeMilliMeter;
//delay(30);
s4 = measure4.RangeMilliMeter;
delay(30); // mesure rapide, on a des autres posibilitées
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Détection des obstacles(limites du terrain)
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void robot_movement() {
if ( s1 <= limit) new_s1 = 0.55 * limit; // détection de la limite du terrain
else new_s1 = 0;
if ( s2 <= limit) new_s2 = 3.2* limit;
else new_s2 = 0;
if ( s3 <= limit) new_s3 = 3.2* limit;
else new_s3 = 0;
if ( s4 <= limit) new_s4 = 0.55 * limit;
else new_s4 = 0;
//Serial.print(new_s1); Serial.print(" "); Serial.print(new_s2); Serial.print(" "); Serial.print(new_s3); Serial.print(" "); Serial.print(new_s4); Serial.println();
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
//Movement Intégrateur
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void go()
{
sensorRead();
robot_movement();
int leftSpeed = ;
int rightSpeed = ;
/*
int leftSpeed = constrain(180 + posX / 2.0 - 80 - size_balle /256 , 110, 200);
int rightSpeed = constrain(180 - posX / 2.0 + 80 - size_balle /256 , 110, 200);
setSpeed(leftSpeed, rightSpeed);*/
//Serial.print(leftSpeed); Serial.print(" ");Serial.print(rightSpeed);Serial.println();
int speedL = constrain(180 + new_s1 - new_s2 - new_s3 - new_s4 + posX / 2.0 - 80 - size_balle /256, -180, 200);
int speedR = constrain(180 - new_s1 - new_s2 - new_s3 + new_s4 - posX / 2.0 + 80 - size_balle /256, -180, 200);
//Serial.print(speedL);Serial.print(" ");Serial.print(speedR);Serial.println();
setSpeed(speedL, speedR);
}
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Set servos
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
void setServo()
{
ball_pixy.attach(27);
ball.attach(32);
arm.attach(25);
}
int inte = 0;
void setup()
{
pinMode(33, INPUT_PULLUP);
timer_90s(); //timer 90sec
initMotor(); //set Motors
Wire.begin(); //connection I2C
//Serial1.begin(9600);
configAddress();
setServo();
sei(); //timer1
//Serial1.print("Start...");
setCameras(); //déclaration des cameras Balles et Direction
pinMode(sensor_ball, INPUT);
//Commande de transitors MOSFET
pinMode(Eject_ball, OUTPUT);
pinMode(Eject_cloth, OUTPUT);
pinMode(Explode_balloons, OUTPUT);
ball.write(4); // etat Initialize
arm.write(100);
ball_pixy.write(130);
while(inte == 0)inte = digitalRead(33);
start = true;
}
void loop()
{
if (start)
{
Pixy_tracking();
if (timer <= 70 && signature == 1)
{
go();
int mes_sensor_ball = analogRead(sensor_ball);
if (mes_sensor_ball >= 430)
{
ball.write(53); // on attrape la balle
caught_ball = true; //on a une balle dans
}
while (caught_ball) //mesure du capteur des balles
{
setSpeed(0, 0);
if(balise)
{
if (panLoop.m_pos > 750L)
{
setSpeed(180, 0);
delay(1000);
setSpeed(0, 0);
ball.write(4); // On enleve le bras qui attrapait la balle
delay(250); //!!!!!!!!AAAATTTTEEEENNNTTTTIOOOOONNN!!!!! changer ce temps il faut le mesure
digitalWrite(Eject_ball, HIGH); //on tire la balle
delay(10);
digitalWrite(Eject_ball, LOW);
caught_ball = false;
}
else if (panLoop.m_pos < 250L)
{
setSpeed(180, 0);
delay(1000);
setSpeed(0, 0);
ball.write(4); // On enleve le bras qui attrapait la balle
delay(250); //!!!!!!!!AAAATTTTEEEENNNTTTTIOOOOONNN!!!!! changer ce temps il faut le mesure
digitalWrite(Eject_ball, HIGH); //on tire la balle
delay(10);
digitalWrite(Eject_ball, LOW);
caught_ball = false;
}
else
{
ball.write(4); // On enleve le bras qui attrapait la balle
delay(250); //!!!!!!!!AAAATTTTEEEENNNTTTTIOOOOONNN!!!!! changer ce temps il faut le mesure
digitalWrite(Eject_ball, HIGH); //on tire la balle
delay(10);
digitalWrite(Eject_ball, LOW);
caught_ball = false;
}
}
else
{
setSpeed(200, 0);
delay(2000); //delay necessaire pour faire une demi tour
setSpeed(0, 0);
ball.write(4); // On enleve le bras qui attrapait la balle
delay(250); //!!!!!!!!AAAATTTTEEEENNNTTTTIOOOOONNN!!!!! changer ce temps il faut le mesure
digitalWrite(Eject_ball, HIGH); //on tire la balle
delay(10);
digitalWrite(Eject_ball, LOW);
caught_ball = false;
}
}
}
else if (timer > 70 && signature == 2)
{
ball_pixy.write(53); //valor pour laisser de voir des balles et commencer à voir les balloons
go();
if(size_max_balle >= 456) //determiné cette Valeur
{
setSpeed(0,0); //arret du robot_movement
arm.write(170); //mettre en position le servos
digitalWrite(Explode_balloons, HIGH);
delay(10);
digitalWrite(Explode_balloons, LOW);
arm.write(100); //mettre en position le servos
}
}
else if (timer >= 90){setSpeed(0, 0); while(true);// Au bout de 90sec on arrete le jeu :)}
}
else setSpeed(0, 0);// Arrêté avant de commencer
}
Liens vers la compétition de l'année passée : RobotGEII 16-17
