Mein Programm kann genau dass was du willst:
An Tischkanten/Hindernissen anhalten,zurückfahren und drehen.
Ausserdem sucht es einen Weißen Punkt und hält da an.
Das Programm ist komplett selbst geschrieben weil mir das Beispielprogramm in dem Tuorial einfach zu kompliziert und undurchsihchtig war.
Jedoch habe ich einen weiteren Testsensot eingebaut weil meine Biene sich immer an Tischbeinen etc festgefahren hat (keine auslösung der Taster).
Dieser neue Taster wurde in die Libaries eingearbeitet wie die anderen nur als sens_getCenter().
Jedoch hab ich damit noch einige Probleme weil ich den nicht genauso ansprechen kann wie die anderen. In meinem Code geht es nur mit der if anweisung
Jedoch nicht mit so wie zB bei sens_getCenter() in der check() Funktion.
Vieleicht kann sich das ja jemand mal anschauen. Und mir erklären was ich falsch gemacht habe, bzw wie es besser geht (immerhin funzt es ja^^)
Programm:
Code:
#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/delay.h>
#include <nibobee/sens.h>
#include <nibobee/led.h>
#include <nibobee/odometry.h>
#include <nibobee/line.h>
#define speed 500
uint8_t check()
{
if ((sens_getLeft()==0) && (sens_getRight()==0))
{
return (0);
}
if ((sens_getLeft()==-1) && (sens_getRight()==-1))
{
return (1);
}
if ((sens_getLeft()==-1) && (sens_getRight()==0))
{
return (2);
}
if ((sens_getLeft()==0) && (sens_getRight()==-1))
{
return (3);
}
if ((sens_getLeft()==1) && (sens_getRight()==0))
{
return (4);
}
if ((sens_getLeft()==0) && (sens_getRight()==1))
{
return (5);
}
return(0);
}
int right(int time,int backtime)
{
motpwm_setLeft(0);
motpwm_setRight(0);
delay(200);
motpwm_setLeft(-400);
motpwm_setRight(-400);
delay(backtime);
motpwm_setLeft(-300);
motpwm_setRight(300);
delay(time);
motpwm_setLeft(0);
motpwm_setRight(0);
return(0);
}
int left(int time,int backtime)
{
motpwm_setLeft(0);
motpwm_setRight(0);
delay(200);
motpwm_setLeft(-400);
motpwm_setRight(-400);
delay(backtime);
motpwm_setLeft(300);
motpwm_setRight(-300);
delay(time);
motpwm_setLeft(0);
motpwm_setRight(0);
return(0);
}
void end()
{
motpwm_stop();
while(1==1)
{
led_set(LED_L_RD,1);
led_set(LED_R_RD,1);
led_set(LED_L_YE,1);
led_set(LED_R_YE,1);
delay(300);
led_set(LED_L_RD,0);
led_set(LED_R_RD,0);
led_set(LED_L_YE,0);
led_set(LED_R_YE,0);
delay(300);
}
}
int main()
{
motpwm_init();
sens_init();
led_init();
odometry_init();
enable_interrupts();
line_init();
uint8_t mode;
uint16_t step;
uint16_t moto=speed;
int countdown;
for(countdown=2000;countdown>10; countdown/=1.5)
{
led_set(LED_L_RD,1);
delay(countdown/2);
led_set(LED_L_RD,0);
led_set(LED_R_RD,1);
delay(countdown/2);
led_set(LED_R_RD,0);
}
odometry_reset();
while(1==1)
{
for(step=0;step<5000;step++)
{
mode=check();
switch (mode)
{
case 1: right(600, 600); break;
case 2: left (600, 600); break;
case 3: left (1200,600); break;
case 4: left (200, 100); break;
case 5: right(200, 100); break;
case 0: motpwm_setRight(speed);motpwm_setLeft(moto);break;
}
if (sens_getCenter()==1) {right(600, 1000);}
if ((line_get(LINE_L)<50)||(line_get(LINE_C)<50)||(line_get(LINE_R)<50)){right(1200,600);}
if ((line_get(LINE_L)>900)||(line_get(LINE_C)>900)||(line_get(LINE_R)>900)){end(0);}
}
if((odometry_getLeft(0)) < (odometry_getRight(0))) {moto=moto+10;}
if((odometry_getLeft(0)) > (odometry_getRight(0))) {moto=moto-10;}
if((odometry_getLeft(0)==0)||(odometry_getRight(0)==0))
{
motpwm_setRight(-800);motpwm_setLeft(-800);
delay(2000);
odometry_reset();
}
else
{
odometry_reset();
}
}
return(0);
}
sens.c
Code:
#include "nibobee/iodefs.h"
#include "nibobee/sens.h"
#include "nibobee/extern.h"
#ifdef __cplusplus
extern "C" {
#endif
void sens_init() {
set_output_group(IO_SENS);
set_output_groupbit(IO_EXT,IO_EXT_BIT_0);
extern_init();
}
int8_t sens_getLeft () {
uint8_t val = IO_SENS_PIN & (_BV(IO_SENS_BIT_LEFT1) | _BV(IO_SENS_BIT_LEFT2));
if (val==_BV(IO_SENS_BIT_LEFT1)) return -1;
if (val==_BV(IO_SENS_BIT_LEFT2)) return 1;
return 0;
}
int8_t sens_getRight ()
{
uint8_t val = IO_SENS_PIN & (_BV(IO_SENS_BIT_RIGHT1) | _BV(IO_SENS_BIT_RIGHT2));
if (val==_BV(IO_SENS_BIT_RIGHT1)) return -1;
if (val==_BV(IO_SENS_BIT_RIGHT2)) return 1;
return 0;
}
int8_t sens_getCenter()
{
if (PINA & (1<<1))
{
return(1);
}
else
{
return(0);
}
}
extern.h
Code:
void extern_init()
{
DDRA = 0x01000000;
PORTA= 0x01000000;
}
Zitieren
Zitat von gagu1
Lesezeichen