|
|
Source Code:
#include <stdio.h>
#include <regc515c.h>
#include <stub.h>
#define MOTOR_LINKS 0
#define MOTOR_RECHTS 1
#define MOTOR_HINTEN 2
#define SCHNELL 10
#define MITTEL 7
#define STOP 0
#define UZS 0
#define GUZS 1
#define WAND_SENSOR_RECHTS 14
#define WAND_SENSOR_LINKS 13
#define WAND_SENSOR_VORNE 11
#define IR_VL 1
#define IR_VR 0
#define IR_HL 3
#define IR_HR 2
#define IR_LEISTE 4
#define IR_BALL 7
#define SERVO_KICKER 0
#define BUMPER_LEFT 8
#define BUMPER_RIGHT 9
#define CENTER 0
#define LEFT 1
#define RIGHT 2
#define CLEAR 3
#define FALSE 0
#define TRUE 1
#define IR_PROC_HALF_PERIOD_100 5
#define IR_PROC_HALF_PERIOD_125 4
#define IR_PROC_PERIOD_FOR_OK 3
unsigned char bumped=CLEAR;
char direction = CENTER;
char lastSeen = LEFT;
unsigned char vl_t0,hr_t0,vr_t0,hl_t0=0;
unsigned char vl_t1,hr_t1,vr_t1,hl_t1=0;
unsigned char vl_t2,hr_t2,vr_t2,hl_t2=0;
unsigned char vl_t3,hr_t3,vr_t3,hl_t3=0;
unsigned char vl,vr,hl,hr=0;
unsigned char ir_leiste;
int deadlock_counter=0;
unsigned long ecke;
unsigned long habeBall;
struct hind{
char position;
unsigned long time;
} hindernis;
struct bl{
char direction;
unsigned long time;
} ball;
void messen();
void bumper_check();
void hindernisBehandlung();
void turn (char direction){
if (direction == LEFT){
motor_richtung(MOTOR_RECHTS,GUZS);
motor_pwm(MOTOR_RECHTS,SCHNELL);
motor_richtung(MOTOR_LINKS,GUZS);
motor_pwm(MOTOR_LINKS,SCHNELL);
motor_richtung(MOTOR_HINTEN,GUZS);
motor_pwm(MOTOR_HINTEN,SCHNELL);
}
else if(direction == RIGHT){
motor_richtung(MOTOR_RECHTS,UZS);
motor_pwm(MOTOR_RECHTS,SCHNELL);
motor_richtung(MOTOR_LINKS,UZS);
motor_pwm(MOTOR_LINKS,SCHNELL);
motor_richtung(MOTOR_HINTEN,UZS);
motor_pwm(MOTOR_HINTEN,SCHNELL);
}
}
void slide(char direction){
if (direction == RIGHT){
motor_pwm(MOTOR_RECHTS,STOP);
motor_pwm(MOTOR_LINKS,STOP);
motor_richtung(MOTOR_HINTEN,GUZS);
motor_pwm(MOTOR_HINTEN,SCHNELL);
}
else{
motor_pwm(MOTOR_RECHTS,STOP);
motor_pwm(MOTOR_LINKS,STOP);
motor_richtung(MOTOR_HINTEN,UZS);
motor_pwm(MOTOR_HINTEN,SCHNELL);
}
}
void slide_gross(char direction){
if (direction == LEFT){
motor_pwm(MOTOR_RECHTS,STOP);
motor_richtung(MOTOR_LINKS,GUZS);
motor_pwm(MOTOR_LINKS,MITTEL);
motor_richtung(MOTOR_HINTEN,UZS);
motor_pwm(MOTOR_HINTEN,SCHNELL);
}
else if(direction == RIGHT){
motor_richtung(MOTOR_RECHTS,UZS);
motor_pwm(MOTOR_RECHTS,MITTEL);
motor_pwm(MOTOR_LINKS,STOP);
motor_richtung(MOTOR_HINTEN,GUZS);
motor_pwm(MOTOR_HINTEN,SCHNELL);
}
}
void go_ahead(){
if (akt_time() < hindernis.time && analog(IR_LEISTE)>240){
slide_gross(hindernis.position);
}
else{
motor_richtung(MOTOR_RECHTS,GUZS);
motor_pwm(MOTOR_RECHTS,SCHNELL);
motor_richtung(MOTOR_LINKS,UZS);
motor_pwm(MOTOR_LINKS,SCHNELL);
motor_pwm(MOTOR_HINTEN,STOP);
}
}
void go(char direction){
if (direction == RIGHT){
motor_pwm(MOTOR_RECHTS,STOP);
motor_richtung(MOTOR_LINKS,UZS);
motor_pwm(MOTOR_LINKS,SCHNELL);
motor_richtung(MOTOR_HINTEN,GUZS);
motor_pwm(MOTOR_HINTEN,MITTEL);
}
else{
motor_richtung(MOTOR_RECHTS,GUZS);
motor_pwm(MOTOR_RECHTS,MITTEL);
motor_pwm(MOTOR_LINKS,STOP);
motor_richtung(MOTOR_HINTEN,UZS);
motor_pwm(MOTOR_HINTEN,SCHNELL);
}
}
void search_ball(char direction) {
if(direction == LEFT){
motor_richtung(MOTOR_RECHTS,GUZS);
motor_pwm(MOTOR_RECHTS,SCHNELL);
motor_richtung(MOTOR_LINKS,GUZS);
motor_pwm(MOTOR_LINKS,SCHNELL);
motor_pwm(MOTOR_HINTEN,STOP);
}
else{
motor_richtung(MOTOR_RECHTS,UZS);
motor_pwm(MOTOR_RECHTS,SCHNELL);
motor_richtung(MOTOR_LINKS,UZS);
motor_pwm(MOTOR_LINKS,SCHNELL);
motor_pwm(MOTOR_HINTEN,STOP);
}
}
void kick( int sensitivity){
if (analog(IR_BALL) > sensitivity){
servo_arc(SERVO_KICKER, 0);
}
else{
servo_arc(SERVO_KICKER, 35);
sleep(100);
}
}
void messen(){
vl_t3=vl_t2; hl_t3=hl_t2; vr_t3=vr_t2; hr_t3=hr_t2;
vl_t2=vl_t1; hl_t2=hl_t1; vr_t2=vr_t1; hr_t2=hr_t1;
vl_t1=vl_t0; hl_t1=hl_t0; vr_t1=vr_t0; hr_t1=hr_t0;
vl_t0=analog(IR_VL);
vr_t0=analog(IR_VR);
hl_t0=analog(IR_HL);
hr_t0=analog(IR_HR);
ir_leiste=analog(IR_LEISTE);
vl=(unsigned char)((vl_t0+(vl_t1>>1)+(vl_t2>>2)+(vl_t3>>3))>>1);
vr=(unsigned char)((vr_t0+(vr_t1>>1)+(vr_t2>>2)+(vr_t3>>3))>>1);
hl=(unsigned char)((hl_t0+(hl_t1>>1)+(hl_t2>>2)+(hl_t3>>3))>>1);
hr=(unsigned char)((hr_t0+(hr_t1>>1)+(hr_t2>>2)+(hr_t3>>3))>>1);
hindernisBehandlung();
if (analog (IR_BALL)<120)
habeBall = akt_time()+500;
if ((vl+hl)-(vr+hr)>5){
ball.direction=RIGHT;
ball.time=akt_time()+2000;
}
else if((vr+hr)-(vl+hl)>5){
ball.direction=LEFT;
ball.time=akt_time()+2000;
}
else if(ir_leiste<240){
ball.direction=CENTER;
ball.time=akt_time()+2000;
}
}
void bumper_check(){
if(digital_in(BUMPER_LEFT)){
go(RIGHT);
sleep(500);
}
else if(digital_in(BUMPER_RIGHT)){
go(LEFT);
sleep(500);
}
}
void hindernisBehandlung(){
unsigned char vorne = analog(WAND_SENSOR_VORNE);
unsigned char links = analog(WAND_SENSOR_LINKS);
unsigned char rechts = analog(WAND_SENSOR_RECHTS);
if(vorne >150 && links >150 && rechts >150)
ecke=akt_time()+1000;
else if(vorne >180 || links >195 || rechts >195){
if (vorne> links && vorne > rechts) {
if(links>rechts)
hindernis.position = LEFT;
else
hindernis.position = RIGHT;
hindernis.time = akt_time()+1000;
}
else if (links>vorne && links >rechts){
hindernis.position = LEFT;
hindernis.time = akt_time()+1000;
}
else{
hindernis.position = RIGHT;
hindernis.time = akt_time()+1000;
}
}
}
void init_Tor(char tor){
if(tor==1){
mod_ir0_takt(IR_PROC_HALF_PERIOD_100);
mod_ir1_takt(IR_PROC_HALF_PERIOD_100);
mod_ir2_takt(IR_PROC_HALF_PERIOD_100);
}
else{
mod_ir0_takt(IR_PROC_HALF_PERIOD_125);
mod_ir1_takt(IR_PROC_HALF_PERIOD_125);
mod_ir2_takt(IR_PROC_HALF_PERIOD_125);
}
}
void AksenMain(void)
{
char messergebnis = CENTER;
init_Tor(dip_pin(0));
while(1){
if (mod_ir1_status() >= IR_PROC_PERIOD_FOR_OK)
lastSeen = RIGHT;
else if (mod_ir0_status() >= IR_PROC_PERIOD_FOR_OK)
lastSeen = LEFT;
if(mod_ir2_status() >= IR_PROC_PERIOD_FOR_OK){
go_ahead();
kick(50);
}
bumper_check();
messen();
if(akt_time() < habeBall)
slide (lastSeen);
else if(akt_time() < ecke)
turn(lastSeen);
else{
if(ball.direction == CENTER)
go_ahead();
else
turn(ball.direction);
}
}
}
|