Vielen Dank, das mit den 500 ms Pause scheint mein Problem behoben zu haben. Zumindest funktioniert es nun einigermassen richtig.
Nun ist nur noch das Problem, das er irgendwie immer vorwärts fährt und nur nach rechts dreht. Egal wie ich steuer.
Hier is nun eben dochmal das ganze Programm, ich hoffe ihr könnt es lesen.
Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_I2CMasterLib.h"
void I2C_transmissionError(uint8_t errorState)
{
writeString_P("\nI2C ERROR - TWI STATE: 0x");
writeInteger(errorState, HEX);
writeChar('\n');
}
extern uint8_t rc_input_right_y, rc_input_right_x, rc_input_active;
/*
Aufnahme der Servowerte des Empfaengers erfolgt in der ISR
in der Datei RP6ControlLib.c ab Zeile 752
*/
uint8_t speed_cruising, speed_rotating, speed_left, speed_right;
uint8_t dir_cruising, dir_rotating, dir_cruising_high, dir_cruising_low, dir_rotating_high, dir_rotating_low;
/*
speed_cruising := zu fahrende geschwindigkeit
speed_rotating := drehgeschwindigkeitsoffset
dir_cruising := zu fahrende richtung
dir_rotating := drehrichtung
dir_cruising_high := fahrrichtung mit hohen empfaengerwerten
dir_cruising_low := fahrrichtung mit niedrigen empfaengerwerten
dir_rotating_low := drehrichtung mit niedrigen empfaengerwerten
dir_rotating_high := drehrichtung mit hohen empfaengerwerten
speed_left := effektive geschwindigkeit links aus speed und speed_rotating
speed_right := effektive geschwindigkeit rechts aus speed und speed_rotating
*/
static uint8_t speed_cruising_max = 100;
static uint8_t speed_cruising_step = 30;
/*
speed_cruising_max := maximale fahrgeschwindigkeit
speed_cruising_step := geschwindigkeitsaenderung je schritt des empfaengers
*/
static uint8_t speed_rotating_max = 50;
static uint8_t speed_rotating_step = 20;
/*
speed_rotating_max := maximaler offset zum drehen
speed_rotating_step := drehgeschwindigkeitsaenderung je schritt des empfaengers
*/
static uint8_t speed_mid = 0;
/*
speed_mid := geschwindigkeitswerte bei mittelposition
*/
int main(void)
{
initRP6Control();
I2CTWI_initMaster(100);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(1);
mSleep(500);
setLEDs(0);
while(1)
{
task_checkINT0();
task_I2CTWI();
/*
writeString_P("RC active :");
writeInteger(rc_input_active, 10);
writeString_P(" - ");
writeString_P("RC vorwaerts :");
writeInteger(rc_input_right_y, 10);
writeString_P(" - ");
writeString_P("RC quer :");
writeInteger(rc_input_right_x, 10);
writeString_P("\n\r");
*/
//ist die fernsteuerung aktiviert
if ( rc_input_active >= 16 && rc_input_active <=18 )
{
//writeString_P(" RC aktiviert ");
//fahrgeschwindigkeit auswerten
switch( rc_input_right_y )
{
//minimale werte
case 8:
writeString_P("y 8");
dir_cruising = dir_cruising_low;
speed_cruising = 4 * speed_cruising_step;
break;
case 9:
writeString_P("y 9");
dir_cruising = dir_cruising_low;
speed_cruising = 3 * speed_cruising_step;
break;
case 10:
writeString_P("y 10");
dir_cruising = dir_cruising_low;
speed_cruising = 2 * speed_cruising_step;
break;
case 11:
writeString_P("y 11");
dir_cruising = dir_cruising_low;
speed_cruising = 1 * speed_cruising_step;
break;
//mittlere werte
case 12:
writeString_P("y 12");
dir_cruising = dir_cruising_low;
speed_cruising = speed_mid;
break;
case 13:
writeString_P("y 13");
dir_cruising = dir_cruising_low;
speed_cruising = speed_mid;
break;
case 14:
writeString_P("y 14");
dir_cruising = dir_cruising_low;
speed_cruising = speed_mid;
break;
case 15:
writeString_P("y 15");
dir_cruising = dir_cruising_low;
speed_cruising = speed_mid;
break;
//maximale werte
case 16:
writeString_P("y 16");
dir_cruising = dir_cruising_high;
speed_cruising = 1 * speed_cruising_step;
break;
case 17:
writeString_P("y 17");
dir_cruising = dir_cruising_high;
speed_cruising = 2 * speed_cruising_step;
break;
case 18:
writeString_P("y 18");
dir_cruising = dir_cruising_high;
speed_cruising = 3 * speed_cruising_step;
break;
case 19:
writeString_P("y 19");
dir_cruising = dir_cruising_high;
speed_cruising = 4 * speed_cruising_step;
break;
//fuer alle anderen werte
default:
writeString_P("y default");
dir_cruising = dir_cruising_high;
speed_cruising = speed_mid;
break;
}
writeString_P("\n\r");
//drehgeschwindigkeit auswerten
switch ( rc_input_right_x)
{
//niedrige werte
case 9:
writeString_P("x 9");
dir_rotating = dir_rotating_low;
speed_rotating = 5 * speed_rotating_step;
break;
case 10:
writeString_P("x 10");
dir_rotating = dir_rotating_low;
speed_rotating = 4 * speed_rotating_step;
break;
case 11:
writeString_P("x 11");
dir_rotating = dir_rotating_low;
speed_rotating = 3 * speed_rotating_step;
break;
case 12:
writeString_P("x 12");
dir_rotating = dir_rotating_low;
speed_rotating = 2 * speed_rotating_step;
break;
case 13:
writeString_P("x 13");
dir_rotating = dir_rotating_low;
speed_rotating = 1 * speed_rotating_step;
break;
//mittlere werte
case 14:
writeString_P("x 14");
dir_rotating = dir_rotating_low;
speed_rotating = speed_mid;
break;
case 15:
writeString_P("x 15");
dir_rotating = dir_rotating_low;
speed_rotating = speed_mid;
break;
case 16:
writeString_P("x 16");
dir_rotating = dir_rotating_low;
speed_rotating = speed_mid;
break;
case 17:
writeString_P("x 17");
dir_rotating = dir_rotating_low;
speed_rotating = speed_mid;
break;
//hohe werte
case 18:
writeString_P("x 18");
dir_rotating = dir_rotating_high;
speed_rotating = 1 * speed_rotating_step;
break;
case 19:
writeString_P("x 19");
dir_rotating = dir_rotating_high;
speed_rotating = 2 * speed_rotating_step;
break;
case 20:
writeString_P("x 20");
dir_rotating = dir_rotating_high;
speed_rotating = 3 * speed_rotating_step;
break;
case 21:
writeString_P("x 21");
dir_rotating = dir_rotating_high;
speed_rotating = 4 * speed_rotating_step;
break;
case 22:
writeString_P("x 22");
dir_rotating = dir_rotating_high;
speed_rotating = 5 * speed_rotating_step;
break;
//fuer alle anderen werte
default:
writeString_P("x default");
dir_rotating = dir_rotating_high;
speed_rotating = speed_mid;
break;
}
writeString_P("\n\r");
}
else
{
speed_cruising = 0;
speed_rotating = 0;
}
/*
writeString_P("X. ");
writeInteger(rc_input_right_x, 10);
writeString_P(" speed rot ");
writeInteger(speed_rotating, 10);
writeString_P(" Y. ");
writeInteger(rc_input_right_y, 10);
writeString_P(" speed crus ");
writeInteger(speed_cruising, 10);
writeString_P("\n\r");
*/
//beschraenkung der maximalen werte
if (speed_cruising > speed_cruising_max)
{
speed_cruising = speed_cruising_max;
}
if (speed_rotating > speed_rotating_max)
{
speed_rotating = speed_rotating_max;
}
//er soll fahren und sich evtl drehen
if ( speed_cruising != 0 )
{
writeString_P("1 ");
//vorwaerts fahren
if ( dir_cruising == dir_cruising_high )
{
writeString_P("1.1 ");
changeDirection(FWD);
speed_left = speed_cruising;
speed_right = speed_cruising;
//zusaetzlich drehen nach rechts
if ( dir_rotating == dir_rotating_high )
{
writeString_P("1.1.1 ");
speed_left += speed_rotating;
speed_right -= speed_rotating;
if ( speed_right < 0)
{
speed_right = 0;
}
}
//zusaetzlich drehen nach links
else if ( dir_rotating == dir_rotating_low )
{
writeString_P("1.1.2 ");
speed_left -= speed_rotating;
speed_right += speed_rotating;
if ( speed_left < 0)
{
speed_left = 0;
}
}
moveAtSpeed(speed_left,speed_right);
}
//rueckwarts fahren
else if ( dir_cruising == dir_cruising_low )
{
writeString_P("1.2 ");
changeDirection(BWD);
speed_left = speed_cruising;
speed_right = speed_cruising;
//zusaetzlich drehen nach rechts
if ( dir_rotating == dir_rotating_high )
{
writeString_P("1.2.1 ");
speed_left += speed_rotating;
speed_right -= speed_rotating;
if ( speed_right < 0)
{
speed_right = 0;
}
}
//zusaetzlich drehen nach links
else if ( dir_rotating == dir_rotating_low )
{
writeString_P("1.2.2 ");
speed_left -= speed_rotating;
speed_right += speed_rotating;
if ( speed_left < 0)
{
speed_left = 0;
}
}
moveAtSpeed(speed_left,speed_right);
}
}
//er soll auf der stelle drehen ohne zu fahren
else if ( speed_cruising == 0 )
{
writeString_P("2 ");
//auf der stelle nach rechts drehen
if ( dir_rotating == dir_rotating_high )
{
writeString_P("2.1 ");
rotate(speed_rotating,RIGHT,1,false);
}
//auf der stelle nach links drehen
else if ( dir_rotating == dir_rotating_low )
{
writeString_P("2.2 ");
rotate(speed_rotating,LEFT,1,false);
}
}
mSleep(200);
writeString_P("\n\r");
}
return 0;
}
Die Werte rc_input_right_y und rc_input_right_x werden wie in den Kommentaren angegeben durch eine ISR von einem Empfänger aufgenommen. Das klappt auch so wie es soll, aber irgendwie fährt er immer nur vorwärts, bzw landet immer an der Stelle wo es heisst vorwärts fahren und wenn er auf der Stelle drehen soll dreht er sich auch nuch nach rechts.
Werd mir das morgen nochmal anschaun. Evtl sehe ich dann klarer.
Lesezeichen