ich hab befürchtet dass jemand das sehen will
sieht leider ziemlich chaotisch aus, da ich ja nur am rumtesten bin..
is eigentlich nur ein "multitasking" system von sternthaler, kombiniert mit dem geradeausfahr algorithmus von waste. dazu soll er noch ne led blinken lassen und eben mit den tastern einem hindernis ausweichen, code wie gesagt abgeleitet von der asuro lib (kollisionstest)
Code:
#include "asuro.h"
int main (void)
{
unsigned long v_time = 0;
unsigned long v_P1_lasttime = 0; // PROZESS: Tasten auswerten
#define d_P1_wartezeit 50; // Zeit in ms
unsigned long v_P2_lasttime = 0; // PROZESS: Status-LEDs blinken
#define d_P2_wartezeit 200;// Zeit in ms
unsigned char v_P2_LEDonoff = 0;
unsigned long v_P3_lasttime = 0; // PROZESS: Linien-Sensoren
#define d_P3_wartezeit 5; // Zeit in ms
unsigned long v_P4_lasttime = 0; // PROZESS: PID-REGLER
#define d_P4_wartezeit 400; // Zeit in ms
unsigned char flagl=FALSE, flagr=FALSE;
unsigned int data[2];
int wegl, wegr, diff;
int speed = 160;
int speedLeft = 160;
int speedRight = 160;
wegl=0; wegr=0;
MotorDir(FWD,FWD);
Init();
unsigned char t1;
unsigned char t2;
// Multitasking teil von Sternthaler
//
while (1)
{
/*----------------------------------------------------------------------
Zeitbasis fuer die Prozesse aktualisieren
*/
v_time = Gettime ();
/*----------------------------------------------------------------------
PROZESS: Tasten auswerten.
*/
if (v_time > v_P1_lasttime)
{
v_P1_lasttime = v_time + d_P1_wartezeit;
t1 = PollSwitch();
t2 = PollSwitch();
if (t1 == 0 && t2 == 0) /* keine Taste */
{
}
else if (t1 && t2 && t1 == t2)
{
MotorDir(BREAK,BREAK);
MotorDir(RWD,RWD);
MotorSpeed(150,150);
Msleep(200);
//Hier fängt das problem an
//
//
if (t1 & 0x07) /* Tasten links gedrückt? */
{
MotorDir(FWD,RWD);
MotorSpeed(150,150);
Msleep(550); //ca 90 Grad
MotorDir(FWD,FWD);
}
if (t1 & 0x38) /* Tasten rechts gedrückt? */
{
MotorDir(RWD,FWD);
MotorSpeed(150,150);
Msleep(550); //ca 90 Grad
MotorDir(FWD,FWD);
}
/* 1 Sekunde fahren */
}
}
/*----------------------------------------------------------------------
PROZESS: Status-LED blinken lassen
*/
if (v_time > v_P2_lasttime)
{
v_P2_lasttime = v_time + d_P2_wartezeit;
if (v_P2_LEDonoff)
StatusLED(YELLOW);
else
StatusLED(OFF);
v_P2_LEDonoff ^= 1;
}
/*----------------------------------------------------------------------
PROZESS: test
*/
if (v_time > v_P3_lasttime)
{
v_P3_lasttime = v_time + d_P3_wartezeit;
//leer
}
/*----------------------------------------------------------------------
PROZESS:
geradeausfahren von waste
*/
if (v_time > v_P4_lasttime)
{
OdometrieData(data);
if ((data[0] < 550) && (flagl == TRUE)) {flagl = FALSE; wegl++;}
if ((data[0] > 650) && (flagl == FALSE)) {flagl = TRUE; wegl++;}
if ((data[1] < 550) && (flagr == TRUE)) {flagr = FALSE; wegr++;}
if ((data[1] > 650) && (flagr == FALSE)) {flagr = TRUE; wegr++;}
diff=wegr-wegl;
if (diff>0) speedRight--; // fahre geradeaus
else if (diff<0) speedLeft--;
else {speedRight=speed; speedLeft=speed;}
if (speedRight<0) {speedRight=0;}
if (speedLeft<0) {speedLeft=0;}
MotorSpeed(speedLeft,speedRight);
}
/*----------------------------------------------------------------------
PERMANTER RECHENPROZESS
*/
// Volle Rechenleistung ist hier verfügbar, ohne sich um ein
// 'Timing' zu kümmern.
// Natürlich darf hier keine Endlosschleife sein.
}
}
das ist die version, in der er nur in eine seite fährt, in die andere seite dreht er sich einmal und dann sofort wieder zurück..
julian
Lesezeichen