NB1 – vi presento il mio robot #2 – il codice

Salve Nerdacci,

non mi sono dimenticato di voi e come promesso vi posto la versione del codice attualmente in utilizzo su NB1. Finalmente i sensori mancanti sono arrivati e quindi a breve potrò partire con i test. Ma non perdiamoci in chiacchiere e guardiamo il codice:

const short MOTOROUTPUT1 = 2;
const short MOTOROUTPUT2 = 3;
const short MOTOROUTPUT3 = 4;
const short MOTOROUTPUT4 = 5;
const short SENSORECHO1 = 6;
const short SENSORTRIG1 = 7;
const short SENSORECHO2 = 8;
const short SENSORTRIG2 = 9;
const short POWERBUTTON = 10;
const short SENSORECHO3 = 12;
const short SENSORTRIG3 = 13;
const short IRRECEIVER1 = A2;
const short IRRECEIVER2 = A0;
const short IRRECEIVER3 = A1;
const bool serialdbg = false;
const bool screendbg = true;
int duration,distance;</code>

void setup()
{
 if(serialdbg) Serial.begin(9600);

 //POWER BUTTON
 pinMode(POWERBUTTON, INPUT);

 //MOTOR PINS
 pinMode(MOTOROUTPUT1, OUTPUT);
 pinMode(MOTOROUTPUT2, OUTPUT);
 pinMode(MOTOROUTPUT3, OUTPUT);
 pinMode(MOTOROUTPUT4, OUTPUT);

 //SONIC SENSOR PINS
 pinMode(SENSORTRIG1, OUTPUT);
 pinMode(SENSORTRIG2, OUTPUT);
 pinMode(SENSORTRIG3, OUTPUT);
 pinMode(SENSORECHO1, INPUT);
 pinMode(SENSORECHO2, INPUT);
 pinMode(SENSORECHO3, INPUT);

 //INFRARED SENSOR PINS
 pinMode(IRRECEIVER1, INPUT);
 pinMode(IRRECEIVER2, INPUT);
 pinMode(IRRECEIVER3, INPUT);

 if(screendbg) {
  //OLED DISPLAY SETUP
  // assign default color value
  if ( u8g.getMode() == U8G_MODE_R3G3B2 ) {
   u8g.setColorIndex(255); // white
  }
  else if ( u8g.getMode() == U8G_MODE_GRAY2BIT ) {
   u8g.setColorIndex(3); // max intensity
  }
  else if ( u8g.getMode() == U8G_MODE_BW ) {
   u8g.setColorIndex(1); // pixel on
  }
  else if ( u8g.getMode() == U8G_MODE_HICOLOR ) {
   u8g.setHiColorByRGB(255, 255, 255);
 }

 u8g.firstPage();
 do {
  initScreen();
 } while ( u8g.nextPage() );
   delay(2000);
 }
}

void initScreen()
{
 u8g.setFont(u8g_font_osb21);
 u8g.drawStr( 40, 22, "NB1");
}

void clearScreen() {}

void draw(short sensor,int value=0) {
 u8g.setFont(u8g_font_osb18);
 u8g.setFontPosTop();
 enum {BufSize=9}; // If a is short use a smaller number, eg 5 or 6
 char buf[BufSize];
 switch (sensor) {
 case 1:
 //front sensor
 u8g.drawFrame(45, 2, 40, 30);
 snprintf (buf, BufSize, "%d", value);
 u8g.drawStr(46, 3, buf);
 break;
 case 2:
 //left sensor
 u8g.drawFrame(2,20,42,30);
 snprintf (buf, BufSize, "%d", value);
 u8g.drawStr( 3, 21, buf);
 break;
 case 3:
 //right sensor
 u8g.drawFrame(86,20,42,30);
 snprintf (buf, BufSize, "%d", value);
 u8g.drawStr(87, 21, buf);
 break;
 case 99:
 //stop
 u8g.drawStr( 40, 22, "Stop");
 break;
 }
}

void loop() {
 //dichiarazione variabili
 int distance;

 //Serial.println("--------------------------------------------");
 while (!proximityChk(1)) {
  if ((!irCheck(2)) && (!irCheck(3))) {
   if(serialdbg) Serial.println("move forward");
   moveForward();
  } else break;
 }
 //se nb1 è circondato
 if (proximityChk(2) && proximityChk(3)) {
  //eseguo retromarcia
  while (proximityChk(2) && proximityChk(3)) {
   if(serialdbg) Serial.println("move backward");
   moveBackward();
  }
  if ((!proximityChk(2)) && (!irCheck(2))) {
  if(serialdbg) Serial.println("turn left");
  turnLeft();
 } //altrimenti controllo se a destra è libero
 else {
 //mi giro a destra
  if ((!proximityChk(3)) && (!irCheck(3))) {
   if(serialdbg) Serial.println("turn right");
   turnRight();
   }
  }
 } else { //altrimenti controllo per convenzione a sinistra
 //se a sinistra è libero mi giro a sinistra
 if (!proximityChk(2)){
  if(serialdbg) Serial.println("turn left");
  turnLeft();
 } //altrimenti controllo se a destra è libero
 else {
 //mi giro a destra
  if (!proximityChk(3)) {
   if(serialdbg) Serial.println("turn right");
   turnRight();
  }
 }
}
}

 boolean irCheck(short irSensor) {
 switch (irSensor) {
 case 1:
 if(serialdbg) Serial.println(" - fotodiodo inferiore -");
 if(digitalRead(IRRECEIVER1)==HIGH) return true;
 break;
 case 2:
 if(serialdbg) Serial.println(" - fotodiodo di sinistra -");
 if(digitalRead(IRRECEIVER2)==LOW) return true;
 break;
 case 3:
 if(serialdbg) Serial.println(" - fotodiodo di destra -");
 if(digitalRead(IRRECEIVER3)==LOW) return true;
 break;
 }
 return false;
 }

boolean proximityChk(short sensor) {
 if(serialdbg) Serial.print("controllo sensore ");
 if(serialdbg) Serial.println(sensor);
 switch (sensor) {
 case 1:
 //front sensor
 //imposto il segnale del trigger basso per 2microseconds
 digitalWrite(SENSORTRIG1, LOW);
 delayMicroseconds(2);
 //imposto il segnale del trigger alto per 10microseconds
 digitalWrite(SENSORTRIG1, HIGH);
 delayMicroseconds(10);
 //imposto il segnale del trigger basso fino a nuova condizione
 digitalWrite(SENSORTRIG1, LOW);
 duration = pulseIn(SENSORECHO1, HIGH);
 break;
 case 2:
 //left sensor
 //imposto il segnale del trigger basso per 2microseconds
 digitalWrite(SENSORTRIG2, LOW);
 delayMicroseconds(2);
 //imposto il segnale del trigger alto per 10microseconds
 digitalWrite(SENSORTRIG2, HIGH);
 delayMicroseconds(10);
 //imposto il segnale del trigger basso fino a nuova condizione
 digitalWrite(SENSORTRIG2, LOW);
 duration = pulseIn(SENSORECHO2, HIGH);
 break;
 case 3:
 //right sensor
 //imposto il segnale del trigger basso per 2microseconds
 digitalWrite(SENSORTRIG3, LOW);
 delayMicroseconds(2);
 //imposto il segnale del trigger alto per 10microseconds
 digitalWrite(SENSORTRIG3, HIGH);
 delayMicroseconds(10);
 //imposto il segnale del trigger basso fino a nuova condizione
 digitalWrite(SENSORTRIG3, LOW);
 duration = pulseIn(SENSORECHO3, HIGH);;
 break;
 }

 //Calculate the distance (in cm) based on the speed of sound.
 distance = duration / 58;
 if(serialdbg) Serial.print("distanza: ");
 if(serialdbg) Serial.println(distance);

 if((distance>=0)&&(screendbg)) {
 u8g.firstPage();
 do {
 draw(sensor,distance);
 }
 while ( u8g.nextPage() );
 }

 //se la distanza dall'ostacolo è uguale o superiore a 15cm proseguo
 if (distance >= 15) {
 return false;
 }
 else return true;
}

void moveForward() {
 if(irCheck(1)) {
  digitalWrite(MOTOROUTPUT1, LOW);
  digitalWrite(MOTOROUTPUT2, HIGH);
  digitalWrite(MOTOROUTPUT3, HIGH);
  digitalWrite(MOTOROUTPUT4, LOW);
 }
}

 void moveBackward(){
  stop();
  //startPlayback(sample1, sizeof(sample1));
  digitalWrite(MOTOROUTPUT1, HIGH);
  digitalWrite(MOTOROUTPUT2, LOW);
  digitalWrite(MOTOROUTPUT3, LOW);
  digitalWrite(MOTOROUTPUT4, HIGH);
  delay(1000);
 }

 void turnLeft() {
  moveBackward();
  //startPlayback(sample2, sizeof(sample2));
  digitalWrite(MOTOROUTPUT1, LOW);
  digitalWrite(MOTOROUTPUT2, HIGH);
  digitalWrite(MOTOROUTPUT3, LOW);
  digitalWrite(MOTOROUTPUT4, LOW);
  delay(2000);
 }

 void turnRight(){
  moveBackward();
  //startPlayback(sample3, sizeof(sample3));
  digitalWrite(MOTOROUTPUT1, LOW);
  digitalWrite(MOTOROUTPUT2, LOW);
  digitalWrite(MOTOROUTPUT3, HIGH);
  digitalWrite(MOTOROUTPUT4, LOW);
  delay(2000);
 }

 void sharpLeft(){
  //startPlayback(sample3, sizeof(sample3));
  digitalWrite(MOTOROUTPUT1, LOW);
  digitalWrite(MOTOROUTPUT2, HIGH);
  digitalWrite(MOTOROUTPUT3, LOW);
  digitalWrite(MOTOROUTPUT4, HIGH);
  delay(2000);
 }

 void sharpRight(){
  //startPlayback(sample3, sizeof(sample3));
  digitalWrite(MOTOROUTPUT1, HIGH);
  digitalWrite(MOTOROUTPUT2, LOW);
  digitalWrite(MOTOROUTPUT3, HIGH);
  digitalWrite(MOTOROUTPUT4, LOW);
  delay(2000);
 }

 void stop() {
  digitalWrite(MOTOROUTPUT1, LOW);
  digitalWrite(MOTOROUTPUT2, LOW);
  digitalWrite(MOTOROUTPUT3, LOW);
  digitalWrite(MOTOROUTPUT4, LOW);
 }

a presto!

Annunci

Rispondi

Inserisci i tuoi dati qui sotto o clicca su un'icona per effettuare l'accesso:

Logo WordPress.com

Stai commentando usando il tuo account WordPress.com. Chiudi sessione /  Modifica )

Google+ photo

Stai commentando usando il tuo account Google+. Chiudi sessione /  Modifica )

Foto Twitter

Stai commentando usando il tuo account Twitter. Chiudi sessione /  Modifica )

Foto di Facebook

Stai commentando usando il tuo account Facebook. Chiudi sessione /  Modifica )

w

Connessione a %s...