|
#include <tjpbase.h>
#include <stdio.h>
int geradeaus(int laenge);
void drehen(int grad);
void get_leuchtturm();
void main() {
int boden_flag;
init_analog();
init_motortjp();
init_clocktjp();
IRE_ON;
START; /*Press the rear bumper to start the program*/
wait(2000);
boden_flag = 0;
while(!boden_flag) {
get_leuchtturm();
boden_flag = geradeaus(50);
}
}
int geradeaus(int laenge)
{
int irl,irr,boden,n=1;
int wert=0;
if (laenge < 0) {
motorp(RIGHT_MOTOR,-MAX_SPEED);
motorp(LEFT_MOTOR,-(MAX_SPEED*0.46)); /* 46 */
laenge = laenge*(-1);
} else {
motorp(RIGHT_MOTOR,MAX_SPEED);
motorp(LEFT_MOTOR,(MAX_SPEED*0.46)); /* 46 */
}
do {
wait(400/9);
boden = analog(7);
if (boden > 90) {
wert=1;
}
} while ((!wert) && (n++ < laenge));
motorp(RIGHT_MOTOR,0);
motorp(LEFT_MOTOR,0);
return wert;
}
void drehen(int grad)
{
if (grad < 0) {
motorp(RIGHT_MOTOR,-MAX_SPEED);
motorp(LEFT_MOTOR,MAX_SPEED);
grad = -grad;
wait((int)((float)((1800/360)*grad)));
} else {
motorp(RIGHT_MOTOR,MAX_SPEED);
motorp(LEFT_MOTOR,-MAX_SPEED);
wait((int)((float)((1810/360)*grad)));
}
motorp(RIGHT_MOTOR,0);
motorp(LEFT_MOTOR,0);
}
void get_leuchtturm() {
int leucht_null,leucht,leucht_max,winkel,i;
int count=0;
leucht_null = analog(5);
leucht_max = 0;
if (leucht_null < 255) {
for (i=0; i < 36 ; i++) {
leucht = analog(5);
if(leucht > leucht_max) {
leucht_max = leucht;
winkel = i*10;
}
drehen(10);
}
wait(500);
drehen(winkel);
}
wait(500);
if ((leucht_max == 255) || (leucht_null == 255)) {
leucht = analog(5);
while (leucht == 255) {
drehen(-5);
leucht = analog(5);
}
drehen(10);
count = 2;
leucht = analog(5);
while (leucht == 255) {
drehen(5);
leucht = analog(5);
count++;
}
winkel = (-1)*(count*5)/2;
drehen(winkel);
}
}
|
|