domenica 14 maggio 2017

Controllo mBot, sensore di distanza in uno spazio chiuso

-----------------

Scopo del progetto:

Creare un programma che sia in grado di far spostare l’mBot in una stanza o in un percorso senza farlo scontrare con degli ostacoli. Se un ostacolo viene rilevato, mBot provvederà ad indietreggiare, girarsi in una direzione casuale e riprendere il percorso.

Il Sensore ad Ultrasuoni:

Questo sensore ha il compito di misurare la distanza. Uno degli "occhi" trasmette un suono e l'altro attende l'eco del suono riflesso da un ostacolo. Misurato il tempo di ritorno del suono si può calcolare la distanza dall’ostacolo. Il range del sensore ad ultrasuoni va da 3 fino ai 400 cm. Se un oggetto è al di fuori di questo intervallo, il sensore restituirà un valore di 400.


Programma in Pseudo-Codifica :

Per Sempre:
---- Se un ostacolo è rilevato
---- ---- Cambia direzione
---- Altrimenti
---- ---- Continua dritto



#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <MeMCore.h>

MeDCMotor motor_9(9);
MeDCMotor motor_10(10);
void move(int direction, int speed)
{
     int leftSpeed = 0;
     int rightSpeed = 0;
     if(direction == 1){
        leftSpeed = speed;
        rightSpeed = speed;
     }else if(direction == 2){
        leftSpeed = -speed;
        rightSpeed = -speed;
     }else if(direction == 3){
        leftSpeed = -speed;
        rightSpeed = speed;
     }else if(direction == 4){
        leftSpeed = speed;
        rightSpeed = -speed;
     }
     motor_9.run((9)==M1?-(leftSpeed):(leftSpeed));
     motor_10.run((10)==M1?-(rightSpeed):(rightSpeed));
}
double angle_rad = PI/180.0;
double angle_deg = 180.0/PI;
double distanza;
MeUltrasonicSensor ultrasonic_3(3);
MeRGBLed rgbled_7(7, 7==7?2:4);

void setup(){
}

void loop(){
   motor_9.run((9)==M1?-(155):(155));
   motor_10.run((10)==M1?-(155):(155));
   if((ultrasonic_3.distanceCm()) < (15)){
       distanza = random(0,(1)+1);
       rgbled_7.setColor(0,0,0,0);
       rgbled_7.show();
       if(((distanza)==(0))){
           rgbled_7.setColor(2,0,0,255);
           rgbled_7.show();
           move(2,200);
           rgbled_7.setColor(2,0,0,255);
           rgbled_7.show();
           _delay(0.1);
           move(4,255);
           rgbled_7.setColor(0,0,0,0);
           rgbled_7.show();
           rgbled_7.setColor(1,0,0,255);
           rgbled_7.show();
           rgbled_7.setColor(2,0,0,255);
           rgbled_7.show();
           _delay(0.1);
           rgbled_7.setColor(0,0,0,0);
           rgbled_7.show();
       }
       rgbled_7.setColor(0,0,0,0);
       rgbled_7.show();
       if(((distanza)==(1))){
           rgbled_7.setColor(2,0,0,255);
           rgbled_7.show();
           move(2,200);
           rgbled_7.setColor(1,0,0,255);
           rgbled_7.show();
           _delay(0.1);
           move(3,255);
           rgbled_7.setColor(0,0,0,0);
           rgbled_7.show();
           rgbled_7.setColor(1,0,0,255);
           rgbled_7.show();
           rgbled_7.setColor(2,0,0,255);
           rgbled_7.show();
           _delay(0.1);
           rgbled_7.setColor(0,0,0,0);
           rgbled_7.show();
       }
       rgbled_7.setColor(0,0,0,0);
       rgbled_7.show();
   }
   _loop();
}


void _delay(float seconds){
   long endTime = millis() + seconds * 1000;
   while(millis() < endTime)_loop();
}

void _loop(){
}

Nessun commento:

Posta un commento