Pobot3
Super actif
Sexe: 
Age: 36
Inscrit le: 05 Mai 2011
Messages: 1977
Localisation: Casablanca
|
Posté le: Lun Mai 30, 2011 12:56 pm Sujet du message:
Je poste mon programme ici, codé en langage C, pour ceux que ça
intéresserait, même s'il y en a pas des masses 
| Code: | /**
# Copyright (c) 2011 Amine Zeidane
#
# Permission is hereby granted, free of charge, to any person obtaining a
copy
# of this software and associated documentation files (the
"Software"), to deal
# in the Software without restriction, including without limitation the
rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
*/
/**
* Ceci est le programme du robot Abdelsalem Aka Hurricane Sonic pour
l'édition 2011 de la coupe de robotique de Vierzon
* Le robot doit traverser une table en diagonale, en evitant les
obstacles sur le terrain, le plus vite possible
* Materiel utilisé :
* - 4 Capteurs Infra-rouges Sharp GP2D12
* - 2 Capteurs de Proximité Proxir
* - Une paire de balises infra-rouge Pololu
* - Un capteur de lumière blanche
* - Deux roues codeuses pour l'odometrie
* - 2 moteurs mdp CC 12V
*/
#include <MsTimer2.h>
#include <LiquidCrystal.h>
// Variables //
/* LCD */
LiquidCrystal lcd(6,7,8,9,10,11);
// Variables d'Etat des boutons
#define OK_BUTTONPIN 53 // Pin digital du
bouton OK
#define SCROLL_BUTTONPIN 52 // Pin digital du boutton Scroll
int okButtonState = 0; // Etat
du boutton OK
int scrollButtonState = 0; // Etat du boutton
Scroll
int okLastState=1;
int counter=0;
int greenLedPin = 42, redLedPin = 43; // Pins des Leds Rouge et
Verte
/* Balise IR */
#define DETECTION_CONST 500 // Valeur Limite
de détection
#define BEACON_SPEED 200
int west = 0;
int south = 0;
int east = 0;
int north = 0;
int dir = 0;
int minValue = 800;
boolean beaconDetected = false;
// Variables du mode statistique //
#define NUM_READINGS 10 // Nombre de lectures
pour le calcul du mode
int directionReadings[NUM_READINGS];
int modeOfDirections = 1;
int index = 0;
/* DETECTION IR */
#define THRESHOLD 120 // La distance a laquelle les
IRs doivent detecter l'obstacle ( 120~= 45 cm)
float ir_dist[4]={0};
boolean obsDetected = false; // Variable
logique vraie si un obstacle est détecté à moins de 45 cm
boolean accelerateOn = false; // Variable logique
vraie si aucun obstacle n'est détecter à moins de 75 cm
int proxirPin1 = 36, proxirPin2 = 37; // Pins des
capteurs de proximité
/* MOTEURS */
#define TOPSPEED 255
#define FORWARD_SPEED 200
#define TURN_SPEED 80
// Left motor
int dirPinL = 4;
int speedPinL = 5;
// Right motor
int dirPinR = 2;
int speedPinR = 12;
int oam_speed[2] = {0};
// FONCTIONS //
/*Moteurs*/
// Initialisation des moteurs
void init_motors(){
pinMode(dirPinL, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(dirPinR, OUTPUT);
pinMode(speedPinR, OUTPUT);
}
// Affectation de la vitesse aux moteurs
void set_speed(int left, int right){
analogWrite(speedPinL, left);
analogWrite(speedPinR, right);
oam_speed[0]=left;
oam_speed[1]=right;}
void set_direction (byte j){ // Determine le sens de rotation des
moteurs ==>> 0 = Gauche ; 1 = Droite ; 2 = Marche
arriere ; 3 = Marche avant <<==
if (j==0){
digitalWrite(dirPinL, LOW);
digitalWrite(dirPinR, HIGH);
}
else if (j==1){
digitalWrite(dirPinL, HIGH);
digitalWrite(dirPinR, LOW);
}
else if (j==2){
digitalWrite(dirPinL, LOW);
digitalWrite(dirPinR, LOW);
}
else if (j==3){
digitalWrite(dirPinL, HIGH);
digitalWrite(dirPinR, HIGH);
}}
void go_forward(int left, int right){ // Avance
set_speed(left, right);
set_direction(3);}
void go_backwards(int left, int right){ // Recule
set_speed(left, right);
set_direction(2);}
void turn_right(int left, int right){ // Tourne à droite
set_speed(left,right);
set_direction(1);}
void turn_left(int left, int right){ // Tourne à gauche
set_speed(left,right);
set_direction(0);}
void accelerate_back(int init, int fin){ // Accélération
for (int i=init; i<=fin; i+=1){
go_backwards(i,i);}}
void slowToStop(){
for (int i=TOPSPEED; i>=0 ; i-=5) { //
Décélération
go_forward(i,i);}}
void stoppe(){ // Arret des moteurs
set_speed(0,0);}
void print_speeds(){
Serial.print("L :");
Serial.print(oam_speed[0]);
Serial.print("| R :");
Serial.print(oam_speed[1]);
}
/* Detection IR*/
// Lecture des Infra-rouges
void scan(int a, int b, int c, int d){
ir_dist[0] = analogRead(a);
ir_dist[1] = analogRead(b);
ir_dist[2] = analogRead(c);
ir_dist[3] = analogRead(d);
}
// Fonction logique qui determine si le robot a un champ libre de 75 cm devant
ou pas
boolean champLibre(){
if ((ir_dist[0] < 75) || (ir_dist[1] <
75) || (ir_dist[2] < 75) || (ir_dist[3]
< 75)){
return false;}
else {
return true;}}
// Fonction logique qui retourne VRAI si un des capteurs IR detecte a moins de
45 cm
boolean obstacle(){
if ((ir_dist[0] > THRESHOLD) || (ir_dist[1]
> THRESHOLD) || (ir_dist[2]>THRESHOLD) ||
(ir_dist[3] > THRESHOLD)){
return true;}
else {
return false;}
}
// Module d'évitement d'obstacle
void obstacleAvoidanceModule(){
boolean extrem_L, extrem_R;
// Si le proxir detecte a droite revenir en arriere - gauche
if (digitalRead(proxirPin1)==0 &&
digitalRead(proxirPin2)==1 &&
digitalRead(dirPinL==HIGH) &&
digitalRead(dirPinR==LOW)){
go_backwards(TOPSPEED,0);
}
// Sinon si le proxir detecte a gauche revenir en arriere - droite
else if (digitalRead(proxirPin2)==0 &&
digitalRead(proxirPin1)==1 &&
digitalRead(dirPinL==HIGH) &&
digitalRead(dirPinR==LOW)){
go_backwards(0,TOPSPEED);
}
// Sinon lancer le module d'évitement par infra-rouges
else {
// Si le capteur d'extreme droite detecte, eviter un peu a
gauche
if ((ir_dist[0] > THRESHOLD) &&
((ir_dist[1] < THRESHOLD) &&
(ir_dist[2] < THRESHOLD) && (ir_dist[3]
< THRESHOLD))){
extrem_R = true;
extrem_L = false;
go_forward(TOPSPEED-2,TOPSPEED);
delay(100);
go_forward((FORWARD_SPEED -
int(0.08*ir_dist[0])),(FORWARD_SPEED +
int(0.08*ir_dist[0])));
}
// Sinon si le capteur d'extreme gauche detecte, eviter un peu a
droite
else if (((ir_dist[0] < THRESHOLD)
&& (ir_dist[1] < THRESHOLD) &&
(ir_dist[2] < THRESHOLD)) &&
(ir_dist[3] > THRESHOLD)){
extrem_L = true;
extrem_R = false;
go_forward(TOPSPEED-2,TOPSPEED);
delay(100);
go_forward((FORWARD_SPEED +
int(0.08*ir_dist[0])),(FORWARD_SPEED -
int(0.08*ir_dist[0])));}
// Sinon si les deux capteurs d'extreme gauche et d'extreme
droite detectent, aller tout droit
else if (((ir_dist[0] > THRESHOLD)
&& (ir_dist[3] > THRESHOLD)) &&
((ir_dist[1] < THRESHOLD) &&
(ir_dist[2] < THRESHOLD))){
go_forward(TOPSPEED-2,TOPSPEED);
extrem_L = false;
extrem_R = false;}
// Sinon si les deux capteurs de droite captent tourner a
gauche
else if (((ir_dist[0] >
THRESHOLD)||(ir_dist[1] > THRESHOLD)) &&
extrem_R==false){
turn_left(FORWARD_SPEED,0.5*FORWARD_SPEED);
}
// Sinon si les deux capteurs de gauche captent
tourner a droite
else if (((ir_dist[3] >
THRESHOLD)||(ir_dist[2] > THRESHOLD)) &&
extrem_L==false){
turn_right
(0.5*FORWARD_SPEED,FORWARD_SPEED);
}
// Si les deux capteurs du milieu detectent
if ((ir_dist[1] > THRESHOLD) &&
(ir_dist[2] > THRESHOLD)){
extrem_L = false;
extrem_R=false;
// Si le capteur extreme droite detecte, revenir
en marche arrière vers la gauche
if((ir_dist[0] >
THRESHOLD)&&(ir_dist[3] <
THRESHOLD)){
go_backwards(TOPSPEED,TURN_SPEED);}
// Sinon si le capteur extreme gauche detecte,
revenir en marche arrière vers la droite
else if ((ir_dist[3] >
THRESHOLD)&&(ir_dist[0] <
THRESHOLD)){
go_backwards(TURN_SPEED,TOPSPEED);}
if ((ir_dist[3] >
THRESHOLD)&&(ir_dist[0] >
THRESHOLD)){
if(ir_dist[0] >
ir_dist[3]){
go_backwards(FORWARD_SPEED,0);}
else{
go_backwards(0,FORWARD_SPEED);}}
}
}
}
void print_ir_values(){
Serial.print("ir0:");
Serial.print(ir_dist[0]);
Serial.print(" | ir1:");
Serial.print(ir_dist[1]);
Serial.print(" | ir2:");
Serial.print(ir_dist[2]);
Serial.print(" | ir3:");
Serial.print(ir_dist[3]);
}
/* BALISE IR */
// Fonction deplacement vers la balise
void moveTowardBeacon(){
// Determiner la direction
getDirAndSetMode();
// Si la balise est detectee a l'ouest tourner a gauche
if (modeOfDirections == 4){ //West
turn_left(TOPSPEED, TOPSPEED);}
// Sinon si la balise est detectee a l'est tourner a droite
else if (modeOfDirections == 2){
turn_right(TOPSPEED,TOPSPEED);}
// Sinon si la balise est detectee au nord aller tout droit
else if ( modeOfDirections == 1 ){ //North
go_forward(TOPSPEED,TOPSPEED);}
}
void setModeOfDirections(){
int directionCounts[4] = {0,0,0,0};
//{North(1), East(2), South(3),
West(4)}
for (int i = 0; i < NUM_READINGS; i++){
//Serial.print(directionReadings[i]);
//Serial.print("|");
directionCounts[directionReadings[i]-1]++;}
// Determine les mode des directions a partir du tableau
//Serial.println("");
int modeCount[2] = {1,directionCounts[0]}; //
Ce tableau contient les nombres de repetitions de chaque direction
int maxi=0;
int it=-1;
for(int i = 0; i < 4; i++){
//Serial.print(directionCounts[i]);
//Serial.print("|");
if(maxi < directionCounts[i]){
maxi = directionCounts[i]; //set count
it = i+1;}} // set direction
modeOfDirections = it;
//Serial.println("");
//Serial.print("Direction Mode: ");
//Serial.println(modeOfDirections);
}
void getDirection(){
if(minValue > west){
dir = 4;}
else if(minValue > south){
dir = 3;}
else if(minValue > east){
dir = 2;}
else if(minValue > north){
dir = 1;}
addDirectionToReadings();
}
// Lecture des entrees Balise //
void readTransceiver(int a, int b, int c, int d){
north = analogRead(a);
east = analogRead(b);
south = analogRead(c);
west = analogRead(d);}
void getDirAndSetMode(){
getDirection();
setModeOfDirections();
}
// Fonction logique qui retourne Vrai si la balise est detectee
boolean foundBeacon(){
if((west > DETECTION_CONST) &&
(east > DETECTION_CONST ) && (south >
DETECTION_CONST ) && (north >
DETECTION_CONST)){
return false; }
else{
return true; }
}
void addDirectionToReadings(){
directionReadings[index] = dir;
index = (index + 1);
if (index == NUM_READINGS)
// if we're at the end of the array...
index = 0;}
// Afficher les valeurs sur le Serial Monitor
void print_beacon_values(){
Serial.print("N:");
Serial.print(north);
Serial.print(" | S:");
Serial.print(south);
Serial.print(" | W:");
Serial.print(west);
Serial.print(" | E:");
Serial.print(east);
}
// SETUP ET BOUCLE PRINCIPALE //
// Fonction appelée une fois au début du programme
void setup()
{
lcd.begin(16, 2); // initialisation du LCD
init_motors(); // Initialisation des moteurs
Serial.begin(38400); // initialisation de la communication serie
pinMode (proxirPin1 , INPUT);
pinMode (proxirPin2 , INPUT);
pinMode (greenLedPin, OUTPUT);
pinMode (redLedPin , OUTPUT);
digitalWrite (greenLedPin, HIGH); // Allumer la led verte
digitalWrite (redLedPin, LOW); // Allumer la led rouge
while (digitalRead(17)!=1){ // Tant que le jack
n'est pas enlevé rester dans la boucle
digitalRead(17);}
//MsTimer2::set(5, update); // 500ms period
//MsTimer2::start();
go_forward(TOPSPEED-2,TOPSPEED); // Avancer pendant 2.5
secondes
delay(2500);
}
// Fonction qui se répéte à l'infini
void loop(){
readTransceiver(1,2,3,4); // Lecture de la balise robot
scan(11,12,13,14); // Lecture
des infra-rouges
obsDetected = obstacle(); // Determiner si un
obstacle est detecte
beaconDetected = foundBeacon(); // Determiner si la
balise est detectee
// Si le capteur de lumiere detecte du blanc, s'arreter, allumer la Led
Rouge et eteindre la verte
if (analogRead(5) >= 910){
digitalWrite(greenLedPin, LOW);
digitalWrite(redLedPin , HIGH);
stoppe();
}
// Sinon , si un obstacle est detecté priorite a l'obstacle, sinon
aller vers la balise
else{
digitalWrite(redLedPin , LOW);
digitalWrite(greenLedPin, HIGH);
if (obsDetected==true ||
(digitalRead(proxirPin1)==0) ||
(digitalRead(proxirPin2)==0)){
obstacleAvoidanceModule(); // Module
d'évitement d'obstacle
}else {
if(beaconDetected==true){
moveTowardBeacon(); // Robot
se dirige vers la balise fixe
}else {
go_forward(TOPSPEED-2,TOPSPEED);
}
}
}
}
|
|
