Wegspeicherung

[SH-Robotics]

Ich versuche jetzt mit hilfe einer RC5 Fernbedienung den Roboter zu steuern, was auch funktioniert aber die Wegspeicherung will einfach nicht.
Wenn ich die Rewind taste drücke führt er nur den letzten befähl aus, ich vermute das der Microprozessor das array schneller abarbeitet, als das Fahrwerk den Befehlen nachkommt. Mit der sleep-funktion hab ich wenig erfahrung und sie funktioniert auch nicht so wie ich will.
Erkennt jemand den fehler im Code?




#include "RP6RobotBaseLib.h"

#define KEY_LEFT 4
#define KEY_RIGHT 6
#define KEY_FORWARDS 2
#define KEY_BACKWARDS 8
#define KEY_STOP 5
#define KEY_LEFTCURVE 1
#define KEY_RIGHTCURVE 3
#define KEY_LEFTCURVEBACK 7
#define KEY_RIGHTCURVEBACK 9
#define KEY_REWIND 60


#define MOVE_FAST 200
#define MOVE_SLOW 100

uint8_t speedvar_left;
uint8_t speedvar_right;
uint8_t array[50];
uint8_t pos = 0;
uint8_t tmp = 0;
uint8_t unstopable = false;


void receiveRC5Data(RC5data_t rc5data)
{
// Output the received data:
writeString_P("Toggle Bit:");
writeChar(rc5data.toggle_bit + '0');
writeString_P(" | Device Address:");
writeInteger(rc5data.device, DEC);
writeString_P(" | Key Code:");
writeInteger(rc5data.key_code, DEC);
writeChar('\n');

uint8_t movement_command = false;

switch(rc5data.key_code)
{
case KEY_LEFT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 4;
break;
case KEY_RIGHT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(FWD);
movement_command = true;
tmp = 6;
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 2;
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 8;
break;
case KEY_STOP:
speedvar_left = 0;
speedvar_right = 0;
movement_command = true;
tmp = 5;
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 1;
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
movement_command = true;
tmp = 3;
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 7;
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
movement_command = true;
tmp = 9;
break;
case KEY_REWIND:
unstopable = true;
pos = pos - 1;
do
{
sleep(500);
switch(array[pos])
{
case KEY_LEFT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(FWD);
break;
case KEY_RIGHT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
case KEY_STOP:
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
}
moveAtSpeed(speedvar_left, speedvar_right);
pos = pos - 1;
}while(pos > 0);
movement_command = false;
unstopable = false;
break;

}

if(movement_command)
{
moveAtSpeed(speedvar_left, speedvar_right);
array[pos] = tmp;
pos = pos + 1;

}
setStopwatch1(0);
startStopwatch1();
}


void stopfunction(void)
{
if(getStopwatch1() > 500 && unstopable == false)
{
moveAtSpeed(0, 0);
stopStopwatch1();
setStopwatch1(0);
}
}

int main(void)
{
initRobotBase();

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);
powerON();

// Set the RC5 Receive Handler:
IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

writeString_P("\n * Turn Left: "); writeInteger(KEY_LEFT, DEC);
writeString_P("\n * Turn Right: "); writeInteger(KEY_RIGHT, DEC);
writeString_P("\n * Move Forwards: "); writeInteger(KEY_FORWARDS, DEC);
writeString_P("\n * Move Backwards: "); writeInteger(KEY_BACKWARDS, DEC);
writeString_P("\n * Stop: "); writeInteger(KEY_STOP, DEC);
writeString_P("\n * Move curve left forwards: "); writeInteger(KEY_LEFTCURVE, DEC);
writeString_P("\n * Move curve right forwards: "); writeInteger(KEY_RIGHTCURVE, DEC);
writeString_P("\n * Move curve left backwards: "); writeInteger(KEY_LEFTCURVEBACK, DEC);
writeString_P("\n * Move curve right backwards: "); writeInteger(KEY_RIGHTCURVEBACK, DEC);

while(true)
{
stopfunction();
task_RP6System(); // Motion Control tasks etc.
}
return 0;
}

[Pr0gm4n]

Hi,

also schon mal ein recht fetter code, aber poste nächstes mal doch bitte einfach in dem bereits offenen Thread, in dem genau das GLEICHE Thema diskutiert wird....


MfG Pr0gm4n

[Roboman93]

mit der sleep-funktion macht der controller eine pause, das heißt, er fährt auch nicht mehr. vielleicht kannst du die sleepaufrufe durch stopwatches ersetzen.

[SH-Robotics]

ok hier mit Stopwatches, funktioniert aber trotzdem noch nicht.

@Pr0gm4n: Welchen Thread meinst du denn?


#include "RP6RobotBaseLib.h"

#define KEY_LEFT 4
#define KEY_RIGHT 6
#define KEY_FORWARDS 2
#define KEY_BACKWARDS 8
#define KEY_STOP 5
#define KEY_LEFTCURVE 1
#define KEY_RIGHTCURVE 3
#define KEY_LEFTCURVEBACK 7
#define KEY_RIGHTCURVEBACK 9
#define KEY_REWIND 60


#define MOVE_FAST 200
#define MOVE_SLOW 100

uint8_t speedvar_left;
uint8_t speedvar_right;
uint8_t array[10];
uint8_t pos = 0;
uint8_t tmp = 0;
uint8_t unstopable = false;


void receiveRC5Data(RC5data_t rc5data)
{
// Output the received data:
writeString_P("Toggle Bit:");
writeChar(rc5data.toggle_bit + '0');
writeString_P(" | Key Code:");
writeInteger(rc5data.key_code, DEC);
writeChar('\n');

uint8_t movement_command = false;

switch(rc5data.key_code)
{
case KEY_LEFT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 4;
break;
case KEY_RIGHT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(FWD);
movement_command = true;
tmp = 6;
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 2;
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 8;
break;
case KEY_STOP:
speedvar_left = 0;
speedvar_right = 0;
movement_command = true;
tmp = 5;
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 1;
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
movement_command = true;
tmp = 3;
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 7;
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
movement_command = true;
tmp = 9;
break;
case KEY_REWIND:
unstopable = true;
pos = pos - 1;
setStopwatch2(0);
startStopwatch2();
do
{
if(getStopwatch2() > 500)
{
switch(array[pos])
{
case KEY_LEFT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(FWD);
break;
case KEY_RIGHT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
case KEY_STOP:
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
}
moveAtSpeed(speedvar_left, speedvar_right);
pos = pos - 1;

stopStopwatch2();
setStopwatch2(0);
startStopwatch2();
}

}while(pos > 0);
movement_command = false;
unstopable = false;
break;

}

if(movement_command)
{
moveAtSpeed(speedvar_left, speedvar_right);
array[pos] = tmp;
pos = pos + 1;

}
setStopwatch1(0);
startStopwatch1();
}


void stopfunction(void)
{
if(getStopwatch1() > 500 && unstopable == false)
{
moveAtSpeed(0, 0);
stopStopwatch1();
setStopwatch1(0);
}
}

int main(void)
{
initRobotBase();

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);
powerON();

// Set the RC5 Receive Handler:
IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

writeString_P("\n * Turn Left: "); writeInteger(KEY_LEFT, DEC);
writeString_P("\n * Turn Right: "); writeInteger(KEY_RIGHT, DEC);
writeString_P("\n * Move Forwards: "); writeInteger(KEY_FORWARDS, DEC);
writeString_P("\n * Move Backwards: "); writeInteger(KEY_BACKWARDS, DEC);
writeString_P("\n * Stop: "); writeInteger(KEY_STOP, DEC);
writeString_P("\n * Move curve left forwards: "); writeInteger(KEY_LEFTCURVE, DEC);
writeString_P("\n * Move curve right forwards: "); writeInteger(KEY_RIGHTCURVE, DEC);
writeString_P("\n * Move curve left backwards: "); writeInteger(KEY_LEFTCURVEBACK, DEC);
writeString_P("\n * Move curve right backwards: "); writeInteger(KEY_RIGHTCURVEBACK, DEC);

while(true)
{
stopfunction();
task_RP6System(); // Motion Control tasks etc.
}
return 0;
}

[Pr0gm4n]

Hi,

ich mein den Thread, den du sogar selbst aufgemacht hast...

https://www.roboternetz.de/phpBB2/zeigebeitrag.php?t=38668&highlight=weg+speichern


MfG Pr0gm4n

[SlyD]

Hallo,

wenn Du "moveAtSpeed" verwendest musst Du auch IMMER task_motionControl ( bzw. besser task_RP6System ) in kurzen Intervallen aufrufen (da passiert die eigentliche Geschwindigkeitsregelung - in der moveAtSpeed Funktion wird nur gesetzt das man gerne mit Geschwindigkeit x fahren würde... ).

Da Du hier eine Schleife (do... while) bei der rewind geschichte verwendest - kann das nicht klappen da task_motionControl nicht mehr aufgerufen wird (das Programm ist ja dann nicht mehr in der Hauptschleife).


Es würde aber dennoch nicht immer so funktionieren wie Du es willst wenn Du jetzt einfach nur task_motionControl mit in die Schleife packst.

Es wäre besser in dem IR Event Handler einfach nur ein flag zu setzen (rewind=true o.ä. also ein globale Variable) und das dann in der Hauptschleife abzufragen und den rewind Code da auszuführen. In einem Event Handler sollte so wenig wie möglich an blockierenden Dingen verwendet werden - vor allem wenn Du dann nämlich task_RP6System mit da reinschreiben würdest und dann während der abarbeitung von rewind weitere IR Codes empfangen werden...
Event Handler wie "receiveRC5Data" sollten nur einmal schnell durchlaufen und ein paar Flags oder andere Werte setzen - man sollte keine Schleifen die länger als 100ms brauchen da einbauen. Alles was länger als 100ms dauert: in andere Programmteile auslagern!

Hoffe das hilft Dir weiter.

MfG,
SlyD

[SH-Robotics]

Danke für die Tipps SlyD, werds dann gleich mal so umsetzen.

@Pr0gm4n: Naja im anderen Thread gings Darum dem Licht zu folgen, dort musste ich Zeit usw. speichern, hier nur welche taste gedrückt wurde

EDIT:

#include "RP6RobotBaseLib.h"

#define KEY_LEFT 4
#define KEY_RIGHT 6
#define KEY_FORWARDS 2
#define KEY_BACKWARDS 8
#define KEY_STOP 5
#define KEY_LEFTCURVE 1
#define KEY_RIGHTCURVE 3
#define KEY_LEFTCURVEBACK 7
#define KEY_RIGHTCURVEBACK 9
#define KEY_REWIND 60


#define MOVE_FAST 200
#define MOVE_SLOW 100

uint8_t speedvar_left;
uint8_t speedvar_right;
uint8_t array[10];
uint8_t pos = 0;
uint8_t tmp = 0;
uint8_t rewind = false;


void receiveRC5Data(RC5data_t rc5data)
{
// Output the received data:
writeString_P("Toggle Bit:");
writeChar(rc5data.toggle_bit + '0');
writeString_P(" | Key Code:");
writeInteger(rc5data.key_code, DEC);
writeChar('\n');

uint8_t movement_command = false;

switch(rc5data.key_code)
{
case KEY_LEFT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 4;
break;
case KEY_RIGHT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(FWD);
movement_command = true;
tmp = 6;
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 2;
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 8;
break;
case KEY_STOP:
speedvar_left = 0;
speedvar_right = 0;
movement_command = true;
tmp = 5;
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 1;
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
movement_command = true;
tmp = 3;
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 7;
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
movement_command = true;
tmp = 9;
break;
case KEY_REWIND:
pos = pos - 1;
rewind = true;
movement_command = false;
break;

}

if(movement_command)
{
moveAtSpeed(speedvar_left, speedvar_right);
array[pos] = tmp;
pos = pos + 1;

}
setStopwatch1(0);
startStopwatch1();
}


void stopfunction(void)
{
if(getStopwatch1() > 500)
{
moveAtSpeed(0, 0);
stopStopwatch1();
setStopwatch1(0);
}
}

int main(void)
{
initRobotBase();

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);
powerON();

// Set the RC5 Receive Handler:
IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

writeString_P("\n * Turn Left: "); writeInteger(KEY_LEFT, DEC);
writeString_P("\n * Turn Right: "); writeInteger(KEY_RIGHT, DEC);
writeString_P("\n * Move Forwards: "); writeInteger(KEY_FORWARDS, DEC);
writeString_P("\n * Move Backwards: "); writeInteger(KEY_BACKWARDS, DEC);
writeString_P("\n * Stop: "); writeInteger(KEY_STOP, DEC);
writeString_P("\n * Move curve left forwards: "); writeInteger(KEY_LEFTCURVE, DEC);
writeString_P("\n * Move curve right forwards: "); writeInteger(KEY_RIGHTCURVE, DEC);
writeString_P("\n * Move curve left backwards: "); writeInteger(KEY_LEFTCURVEBACK, DEC);
writeString_P("\n * Move curve right backwards: "); writeInteger(KEY_RIGHTCURVEBACK, DEC);

while(true)
{
setStopwatch2(0);
startStopwatch2();
do
{
if(getStopwatch2() > 500)
{
switch(array[pos])
{
case KEY_LEFT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(FWD);
break;
case KEY_RIGHT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
case KEY_STOP:
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
}
moveAtSpeed(speedvar_left, speedvar_right);
pos = pos - 1;
task_RP6System();
stopStopwatch2();
setStopwatch2(0);
startStopwatch2();
}

}while(pos > 0 && rewind == true);
stopfunction();
task_RP6System(); // Motion Control tasks etc.
}
return 0;
}


Funktioniert auf den ersten Blick ganz gut, danke SlyD. Aber es sieht so aus als ob die Zeitabstände noch nicht ganz hinhaun. Ich werd mir das Fahrverhalten später nocheinmal ansehen, wenn ich mehr Zeit habe.
Nochmals danke SlyD.

[Pr0gm4n]

Hi,

eigentlich ist das mit dem Licht doch nichts anderes:

du empfängst über einen Sensor irgendwelche Daten und speicherst diese in einem Array oder so. dann soll er die Daten noch zusätzlich verarbeiten und bei der Repeat-Taste die gespeicherten Befehle ausführen. Also irgendwie ist der einzige Code-Unterschied, dass du andere Signale empfangen musst und auch bei if-Bedingungen oder so auch andere Sachen abfragen musst, aber im entefekt isses doch alles das gleiche, oder?

MfG Pr0gm4n

[SH-Robotics]

Nein, denn beim Licht nachfahren muss ich auch die Zeit speichern, die der Roboter dem Licht nachgefahren ist. Deswegen war das größte Problem der Speicherplatz, dieses existiert hier nicht.

Also hör bitte auf diesen Thread vollzuspammen...

[SH-Robotics]

Hatte jetzt mal wieder Zeit den Code auszuprobieren, aber es gibt ein paar kleine Fehler bei der Ausführung. Manchmal fährt er länger manchmal kürzer(liegt warscheinlich an der stopfunktion, aber wenn ich die in die obere funktion verschiebe geht garnichts mehr(warscheinlich wegen task_RP6System)), machmal fährt er aber auch in eine andere richtung als er eigentlich sollte(sehr selten) und manchmal hört er nichtmehr auf in eine richtung zu fahren...

könnte mir bitte jemand helfen?



#include "RP6RobotBaseLib.h"

#define KEY_LEFT 4
#define KEY_RIGHT 6
#define KEY_FORWARDS 2
#define KEY_BACKWARDS 8
#define KEY_STOP 5
#define KEY_LEFTCURVE 1
#define KEY_RIGHTCURVE 3
#define KEY_LEFTCURVEBACK 7
#define KEY_RIGHTCURVEBACK 9
#define KEY_REWIND 60


#define MOVE_FAST 200
#define MOVE_SLOW 100

uint8_t speedvar_left;
uint8_t speedvar_right;
uint8_t array[50];
uint8_t pos = 0;
uint8_t tmp = 0;
uint8_t rewind = false;


void receiveRC5Data(RC5data_t rc5data)
{
// Output the received data:
writeString_P("Toggle Bit:");
writeChar(rc5data.toggle_bit + '0');
writeString_P(" | Key Code:");
writeInteger(rc5data.key_code, DEC);
writeChar('\n');

uint8_t movement_command = false;

switch(rc5data.key_code)
{
case KEY_LEFT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 4;
break;
case KEY_RIGHT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(FWD);
movement_command = true;
tmp = 6;
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 2;
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 8;
break;
case KEY_STOP:
speedvar_left = 0;
speedvar_right = 0;
movement_command = true;
tmp = 5;
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
movement_command = true;
tmp = 1;
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
movement_command = true;
tmp = 3;
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
movement_command = true;
tmp = 7;
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
movement_command = true;
tmp = 9;
break;
case KEY_REWIND:
pos = pos - 1;
rewind = true;
movement_command = false;
break;

}

if(movement_command)
{
moveAtSpeed(speedvar_left, speedvar_right);
array[pos] = tmp;
pos = pos + 1;

}
setStopwatch1(0);
startStopwatch1();
}


void stopfunction(void)
{
if(getStopwatch1() > 500)
{
moveAtSpeed(0, 0);
stopStopwatch1();
setStopwatch1(0);
}
}

int main(void)
{
initRobotBase();

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);
powerON();

// Set the RC5 Receive Handler:
IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

writeString_P("\n * Turn Left: "); writeInteger(KEY_LEFT, DEC);
writeString_P("\n * Turn Right: "); writeInteger(KEY_RIGHT, DEC);
writeString_P("\n * Move Forwards: "); writeInteger(KEY_FORWARDS, DEC);
writeString_P("\n * Move Backwards: "); writeInteger(KEY_BACKWARDS, DEC);
writeString_P("\n * Stop: "); writeInteger(KEY_STOP, DEC);
writeString_P("\n * Move curve left forwards: "); writeInteger(KEY_LEFTCURVE, DEC);
writeString_P("\n * Move curve right forwards: "); writeInteger(KEY_RIGHTCURVE, DEC);
writeString_P("\n * Move curve left backwards: "); writeInteger(KEY_LEFTCURVEBACK, DEC);
writeString_P("\n * Move curve right backwards: "); writeInteger(KEY_RIGHTCURVEBACK, DEC);

while(true)
{
setStopwatch2(0);
startStopwatch2();
while(pos > 0 && rewind == true)
{
if(getStopwatch2() > 500)
{
switch(array[pos])
{
case KEY_LEFT:
speedvar_left = MOVE_FAST;
speedvar_right = 0;
changeDirection(BWD);
break;
case KEY_RIGHT:
speedvar_left = 0;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_FORWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_BACKWARDS:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
case KEY_STOP:
break;
case KEY_LEFTCURVE:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(BWD);
break;
case KEY_RIGHTCURVE:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(BWD);
break;
case KEY_LEFTCURVEBACK:
speedvar_left = MOVE_FAST;
speedvar_right = MOVE_SLOW;
changeDirection(FWD);
break;
case KEY_RIGHTCURVEBACK:
speedvar_left = MOVE_SLOW;
speedvar_right = MOVE_FAST;
changeDirection(FWD);
break;
}
moveAtSpeed(speedvar_left, speedvar_right);
pos = pos - 1;
task_RP6System();
stopStopwatch2();
setStopwatch2(0);
startStopwatch2();
}

}
rewind = false;
stopfunction();
task_RP6System();
}
return 0;
}