Öffentliche Projekte

#include <nibobee/robomain.h>
#include <stdint.h>

#include "nibobee/motpwm.h"
#include "nibobee/analog.h"
#include "nibobee/line.h"
#include "nibobee/feeler.h"

#include <util/delay.h>
 
  int motor = 0;
  bool feeler = true;
  bool line = true;
  int lastAvoid = 0;

void setup() {
  motpwm_init();
  analog_init();
  line_init();
  feeler_init();

}

void lineLost() {
  motpwm_setRight(500);
  motpwm_setLeft(500);
  delay(500);
  while (line_get(LINE_C) > 600) {
    if (lastAvoid == 0) {
        motpwm_setRight(500);
        motpwm_setLeft(-500);
    }
    else {
        motpwm_setRight(500);
        motpwm_setLeft(-500);
    }
    delay(200);
  }
}

void wendeRechts();

void wendeLinks() {
  lastAvoid = 0;
  motpwm_stop();
  motpwm_setRight(-500);
  motpwm_setLeft(-500);
  delay(1000);
  motpwm_setLeft(-500);
  motpwm_setRight(500);
  delay(500);
    while (feeler_getRight() == 0 & feeler_getLeft() == 0 & line_get(LINE_L) > 600 & line_get(LINE_C) > 600 & line_get(LINE_R) > 600) {
  motpwm_setLeft(400);
  motpwm_setRight(600);
    delay(200);
  }
  motpwm_stop();
   if (feeler_getLeft()==1){wendeRechts();}
  else if (feeler_getRight()==1){wendeLinks();}
}

void wendeRechts() {
  lastAvoid = 1;
  motpwm_stop();
  motpwm_setRight(-500);
  motpwm_setLeft(-500);
  delay(1000);
  motpwm_setLeft(500);
  motpwm_setRight(-500);
  delay(500);
    while (feeler_getRight() == 0 & feeler_getLeft() == 0 & line_get(LINE_L) > 600 & line_get(LINE_C) > 600 & line_get(LINE_R) > 600) {
  motpwm_setLeft(600);
  motpwm_setRight(400);
    delay(200);
  }
  motpwm_stop();
   if (feeler_getLeft()==1){wendeRechts();}
  else if (feeler_getRight()==1){wendeLinks();}
}

void loop() {
  //Feeler
  if (feeler == true){
  if (feeler_getLeft()==1){wendeRechts();}
  else if (feeler_getRight()==1){wendeLinks();}
  else if ((feeler_getRight()==1) & (feeler_getRight()==1)) {motor = 0;}
  else {motor = 1;}
  }
  
  //Line
  if (line == true){
  if (line_get(LINE_L) < line_get(LINE_C)) {motor = 2;}
  else if (line_get(LINE_R) < line_get(LINE_C)) {motor = 3;}
  else if ((line_get(LINE_L) < line_get(LINE_C)) & (line_get(LINE_R) < line_get(LINE_C))) {motor = 1;}
  else if ((line_get(LINE_L) > line_get(LINE_C)) & (line_get(LINE_R) > line_get(LINE_C))) {motor = 0;}
  else if ((line_get(LINE_L) == line_get(LINE_C)) & (line_get(LINE_R) == line_get(LINE_C))) {lineLost();}
  }
  
  //Motor
  switch(motor) {
    case 1: motpwm_setRight(500); motpwm_setLeft(500); break;
    case 2: motpwm_setRight(600); motpwm_setLeft(400); break;
    case 3: motpwm_setRight(400); motpwm_setLeft(600); break;
  	case 0: motpwm_stop(); break;
  }
  	delay(10);
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

Orientierung/main.cpp [read only]

Werbung

Online