Öffentliche Projekte

/* NIBObee C project */
#include <nibobee/iodefs.h>
#include <nibobee/led.h>
#include <nibobee/sens.h>
#include <nibobee/delay.h>
#include <nibobee/analog.h>   
#include <nibobee/base.h>     
#include <nibobee/clock.h>    
#include <nibobee/motpwm.h>   
#include <nibobee/odometry.h> 
#include <nibobee/line.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

int main()
{
 led_init();
 sens_init();
 motpwm_init();
 motpwm_setLeft(0);
 motpwm_setRight(0);
 odometry_init();
 analog_init(); 
 line_readPersistent(); 
 set_output_group(IO_SENS); 
 activate_output_bit(IO_LINE_EN); 
 activate_output_group(IO_LEDS);  
  
 int16_t speed_flt_l=0;
 int16_t speed_flt_r=0;
 int16_t speed_r;
 int16_t speed_l;
 int16_t speed_ton;
 int i=i;
 int j=j;
 int k=k;
  
/* Start: Die LEDs blinken und die Gleichstrommotoren erzeugen durch Umpolung 
   im Millisekundenbereich Töne.*/   
  delay(8000);
  for(i=0; i<8; i++)
 {
  led_set(LED_L_RD, 1);
  led_set(LED_R_RD, 1);
  
  for  (k=0; k<1200; k++) 
  {
   speed_ton=150;
   motpwm_setRight(speed_ton);
   motpwm_setLeft(speed_ton);
   delay_us(80);
   speed_ton=-150;
   motpwm_setRight(speed_ton);
   motpwm_setLeft(speed_ton);
   delay_us(80);
  }
  
  speed_ton=0;
  motpwm_setRight(speed_ton);
  motpwm_setLeft(speed_ton); 
  led_set(LED_L_RD, 0);
  led_set(LED_R_RD, 0);
  delay_ms(10);
  led_set(LED_L_YE, 1);
  led_set(LED_R_YE, 1);
  
  for(j=0; j<3000; j++)
  {
   led_set(LED_L_RD, 0);
   led_set(LED_R_RD, 0);
   led_set(LED_L_YE, 1);
   led_set(LED_R_YE, 1);
   speed_ton=150;
   motpwm_setRight(speed_ton);
   motpwm_setLeft(speed_ton);
   delay_us(6);
   speed_ton=-150;
   motpwm_setRight(speed_ton);
   motpwm_setLeft(speed_ton);
   delay_us(6);
    
  }
  
  led_set(LED_L_YE, 0);
  led_set(LED_R_YE, 0); 
    
 
 }
 
 speed_ton=0;
 motpwm_setRight(speed_ton);
 motpwm_setLeft(speed_ton);
 led_set(LED_L_RD, 1);
 led_set(LED_R_RD, 1);
 delay_ms(10);
 led_set(LED_L_RD, 0);
 led_set(LED_R_RD, 0);
 i=j=k=0;

// Endlosschleife:
while(1) 
{
 enable_interrupts();
 
// Sensorwerte aktualisiseren
 int8_t status_R = sens_getRight();
 int8_t status_L = sens_getLeft();
 int16_t lval = line_get(LINE_L);
 int16_t cval = line_get(LINE_C);
 int16_t rval = line_get(LINE_R);

/* Bei Betätigung eines Fühlers führt der Roboter eine Kehrtwende aus,
    danach setzt er die Linienverfolgung wie gehabt fort. */
 if((status_R || status_L) == (-1 || +1))
 {
  odometry_reset();
/*Der Roboter fährt bis zu einem Odometriezählestand von minus sieben vom Hindernis zurück, somit besteht
   kein Kontakt zum Hindernis mehr und das Wendemnöver kann starten. */  
  while(odometry_getLeft(0)>-8)
   {
    speed_r=-350, speed_l=-350; 
    motpwm_setLeft(speed_l);
    motpwm_setRight(speed_r); 
   }
   
   delay(10);
   motpwm_setRight(0);
   motpwm_setLeft(0);

/* Die LEDs blinken und die Gleichstrommotoren erzeugen durch Umpolung 
   im Millisekundenbereich Töne. -> Wendemanöver wird angekündigt. */    
     for(i=0; i<8; i++)
  {
   led_set(LED_L_RD, 1);
   led_set(LED_R_RD, 1);
   
   for  (k=0; k<600; k++) 
   {
    speed_ton=150;
    motpwm_setRight(speed_ton);
    motpwm_setLeft(speed_ton);
    delay_us(80);
    speed_ton=-150;
    motpwm_setRight(speed_ton);
    motpwm_setLeft(speed_ton);
    delay_us(80);
    }
  
   speed_ton=0;
   motpwm_setRight(speed_ton);
   motpwm_setLeft(speed_ton);
   led_set(LED_L_RD, 0);
   led_set(LED_R_RD, 0);
   delay_ms(10);
   led_set(LED_L_YE, 1);
   led_set(LED_R_YE, 1);
 
   for(j=0; j<1500; j++) 
   {
    led_set(LED_L_RD, 0);
    led_set(LED_R_RD, 0);
    led_set(LED_L_YE, 1);
    led_set(LED_R_YE, 1);
    speed_ton=150;
    motpwm_setRight(speed_ton);
    motpwm_setLeft(speed_ton);
    delay_us(6);
    speed_ton=-150;
    motpwm_setRight(speed_ton);
    motpwm_setLeft(speed_ton);
    delay_us(6);
   }
  
  led_set(LED_L_YE, 0);
   led_set(LED_R_YE, 0);   
  }

speed_ton=0;
  motpwm_setRight(speed_ton);
  motpwm_setLeft(speed_ton);
  led_set(LED_L_RD, 1);
  led_set(LED_R_RD, 1);
  delay_ms(10);
  led_set(LED_L_RD, 0);
  led_set(LED_R_RD, 0);
  i=j=k=0;    
  odometry_reset();
  delay(100);

/* Wende wird ausgefürt, der Roboter dreht sich mindestens bis zu einem Odometryzählerstand von 15 bzw. ca.90°,
   dadurch wird vermiden, dass eine andere Linie registriert wird. Sobald die selbe Linie wieder erkannt ist,
   wird die Linienverfolgung fortgesetzt.*/
   while(odometry_getLeft(0)<10)
  { 
   speed_r=-425, speed_l=425; 
   motpwm_setLeft(speed_l);
   motpwm_setRight(speed_r); 
  } 
/* Der Roboter dreht sich weiter, bis er mit dem Rechten Liniensensor wieder eine schwarze Linie registriert bzw.
   der Wert des rechten Sensors kleiner als der des linken ist, da dieser noch die weiße Linie registriert. +80
   verhindert, dass der Roboter auf Schwankungen der Sensorwerte, die z.B. durch Schatten entstehen können, reagiert.*/
   while(line_get(LINE_L)<(line_get(LINE_R)+80))
  { 
   speed_r=-425, speed_l=425; 
   motpwm_setLeft(speed_l);
   motpwm_setRight(speed_r); 
  } 
  odometry_reset();
  motpwm_setLeft(0);
  motpwm_setRight(0); 
  delay(50);
 }
// Der Roboter folgt einer schwarzen Linie auf weißem Grund
 else
 {
// Die Linie ist links: Der linke Sensor hat den kleinsten Wert.  
  if ((lval<cval) || (lval<rval)) 
  {
   led_set(LED_L_RD, 1);
   led_set(LED_R_RD, 0);
// Der rechte Motor dreht schnell, der linke langsamer.
   speed_r=425, speed_l=50;
    } 
  else if ((rval<cval) && (rval<lval)) 
  {
// Linie ist rechts: Der rechte Sensor hat den kleinsten Wert.
   led_set(LED_L_RD, 0);
   led_set(LED_R_RD, 1);
// Der linke Motor dreht schnell, der rechte langsamer.
   speed_r=50, speed_l=425;
    } 
   else 
   {
// Linie ist mittig: Der mittlere Sensor hat den kleinsten Wert.
    led_set(LED_L_RD, 1);
    led_set(LED_R_RD, 1);
// Beide Motoren drehen gleich schnell
    speed_r=350, speed_l=350;
   }

// Drehzahlwerte der Motoren werden neu gestellt.
   motpwm_setLeft(speed_l);
   motpwm_setRight(speed_r);
  }
}
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

vermischt_1/main.c [read only]

Werbung

Online