Code:
#include <asuro.h>
unsigned char sw_data, i, tmp_char, step, speed_min, speed_max, rampe_v, rampe_r;
unsigned int data[2], j, tmp_int;
unsigned long loop_count, rampe_delay;
unsigned char odo_bit_l, odo_bit_r;
unsigned int odo_min_l, odo_max_l, odo_low_l, odo_high_l;
unsigned int odo_min_r, odo_max_r, odo_low_r, odo_high_r;
unsigned int count_l, count_r, count_soll_l, count_soll_r;
unsigned int speed_l, speed_r, speed_max_l, speed_max_r, speed_soll_l, speed_soll_r;
void hochheben() { // blinken: gelb=hochheben green=hinsetzen dauergreen=los
LineData(data); tmp_char=data[0]+data[1]+20; // Schwelle für aufheben vom Boden
do {
LineData(data); StatusLED(OFF);Msleep(300); StatusLED(YELLOW); Msleep(150);
} while ((data[0]+data[1]) < tmp_char);
do {
LineData(data); StatusLED(OFF);Msleep(300); StatusLED(GREEN); Msleep(150);
} while ((data[0]+data[1]) > tmp_char);
StatusLED(GREEN); Msleep(2000);
}
int init_odo() {
StatusLED(YELLOW); MotorDir(RWD,FWD);
odo_min_l=odo_max_l=odo_low_l=odo_high_l=count_l=0;
odo_min_r=odo_max_r=odo_low_r=odo_high_r=count_r=0;
MotorSpeed(150,150);
for (j==0; j<5000; j++) {
OdometrieData(data);
if (data[0] < odo_min_l) odo_min_l=data[0];
if (data[0] > odo_max_l) odo_max_l=data[0];
if (data[1] < odo_min_r) odo_min_r=data[1];
if (data[1] > odo_max_r) odo_max_r=data[1];
for (i=0; i<200; i++);
}
MotorSpeed(0,0);Msleep(100);
OdometrieData(data);
odo_low_l=((odo_min_l + odo_max_l)/3); // je ein drittel high-nix-low
odo_high_l=((odo_min_l + odo_max_l)/3)*2;
if (data[0] >= ((odo_min_l + odo_max_l)/2)) odo_bit_l=1; else odo_bit_l=0;
odo_low_r=((odo_min_r + odo_max_r)/3); // je ein drittel high-nix-low
odo_high_r=((odo_min_r + odo_max_r)/3)*2;
if (data[1] >= ((odo_min_r + odo_max_r)/2)) odo_bit_r=1; else odo_bit_r=0;
MotorDir(BREAK,BREAK);
}
void Count(){
OdometrieData(data);
if (!odo_bit_l & (data[0] > odo_high_l)) {
odo_bit_l=1; count_l ++; StatusLED(YELLOW);
}
if (odo_bit_l & (data[0] < odo_low_l)) {
odo_bit_l=0; count_l ++; StatusLED(OFF);
}
if (!odo_bit_r & (data[1] > odo_high_r)) {
odo_bit_r=1; count_r ++; StatusLED(RED);
}
if (odo_bit_r & (data[1] < odo_low_r)) {
odo_bit_r=0; count_r ++; StatusLED(OFF);
}
}
int MotorRampe(unsigned char sp_l, unsigned char sp_r, unsigned int r) {
if (loop_count > rampe_delay) {
if (sp_l != speed_l) {
if (sp_l > speed_l) speed_l ++;
if (sp_l < speed_l) speed_l --;
rampe_delay=loop_count+r;
}
if (sp_r != speed_r) {
if (sp_r > speed_r) speed_r ++;
if (sp_r < speed_r) speed_r --;
rampe_delay=loop_count+r;
}
MotorSpeed(speed_l,speed_r);
}
return ((sp_l == speed_l) && (sp_r == speed_r));
}
void run(void) {
}
int main(void) {
Init();
SerWrite("\nHallo\nBatterie: ",17);
PrintInt(Batterie());
init_odo();// hochheben();
step=loop_count=rampe_delay=speed_l=speed_r=count_l=count_r=0;
speed_min=0; speed_max=250; rampe_v=15; rampe_r=15;
do {
loop_count ++;
switch (step) {
case (0): MotorDir(FWD,FWD); FrontLED(ON); BackLED(OFF,OFF);
count_l=count_r=0; step++; break;
case (1): if (MotorRampe(speed_max,speed_max,rampe_v)) step++; break;
case (2): if (MotorRampe(speed_min,speed_min,rampe_r)) step++; break;
case (3): MotorDir(RWD,RWD); FrontLED(OFF); BackLED(ON,ON);
SerWrite("\n",1);PrintInt(count_l); SerWrite("\n",1); PrintInt(count_r);
step++; break;
case (4): if (MotorRampe(speed_max,speed_max,rampe_v)) step++; break;
case (5): if (MotorRampe(speed_min,speed_min,rampe_r)) step=0; break;
case (98): MotorDir(BREAK,BREAK); step++; break;
case (99): FrontLED(OFF); BackLED(OFF,OFF); step=99; break;
}
Count();
} while (1);
return 0;
}
Lesezeichen