Hi,
Die Werte für x und y stimmen schon. Die sind nur anders weil ich Die Laufrichtung der Servos geändert habe, aber deine Anregung habe ich umgesetzt und folgendes ist dabei rasgekommen:
Code:
#include "RP6RobotBaseLib.h"
volatile uint8_t rc_input_pwrr=26, rc_input_pwrl=26, rc_input_schalt;
int main(void)
{
initRobotBase();
TIMSK |= (1 << TOIE1); // Die Timer1 Overflow-ISR zur Signalauslesung
powerON();
setACSPwrHigh();
setLEDs(0b111111);
uint8_t y=21;
uint8_t x=29;
uint8_t c=22;
uint8_t v=29;
uint8_t pwr_l=0;
uint8_t pwr_r=0;
void acsStateChanged(void)
{
if(!obstacle_left && !obstacle_right)
{
move(160, FWD, DIST_MM(50), true);
}
while(true)
{
if(!obstacle_left && !obstacle_right)
{
move(160, FWD, DIST_MM(50), true);
}
else
{
if(obstacle_right)
{
rotate(160, LEFT, 2,true);
}
else
{
while((obstacle_left && obstacle_right) || obstacle_left)
{
rotate(160, RIGHT, 2,true);
}
}
}
task_Bumpers();
if(getBumperLeft() && getBumperRight()) // Beide Bumper
{
move(160, BWD, DIST_MM(30), true);
rotate(160, LEFT, 35,true);
}
if(getBumperLeft()) // Bumber Links
{
move(160, BWD, DIST_MM(30), true);
rotate(160, RIGHT, 25,true);
}
if(getBumperRight()) // Bumber Rechts
{
move(160, BWD, DIST_MM(30), true);
rotate(160, LEFT, 25,true);
}
mSleep(10);
}
}
while(1)
{
task_motionControl();
task_ACS();
if (rc_input_schalt>x) // Umschalten auf Autopilot
{
while (rc_input_schalt>x)
{
ACS_setStateChangedHandler(acsStateChanged); // ACS Event Handler registrieren
}
}
if (rc_input_pwrr<17 && rc_input_pwrl<17 && rc_input_pwrr>32 && rc_input_pwrl>32)
{
stop();
}
if (rc_input_pwrr<c && rc_input_pwrl<y)
{ while (rc_input_pwrr<c && rc_input_pwrl<y)
{
rotate(60, LEFT, 1, true);
}
}
if (rc_input_pwrr>v && rc_input_pwrl>x)
{ while (rc_input_pwrr>v && rc_input_pwrl>x)
{
rotate(60, RIGHT, 1, true);
}
}
if (rc_input_pwrr<c )
{
changeDirection(FWD);
pwr_r=60;
}
else if (rc_input_pwrr>v )
{
changeDirection(BWD);
pwr_r=60;
}
else pwr_r=0;
if (rc_input_pwrl<y )
{
changeDirection(BWD);
pwr_l=60;
}
else if (rc_input_pwrl>x )
{
changeDirection(FWD);
pwr_l=60;
}
else pwr_l=0;
if(pwr_l || pwr_r)
{
moveAtSpeed(pwr_l, pwr_r);
}
else
stop();
}
return 0;
}
ISR (TIMER1_OVF_vect) // RC-Signale an SCL und SDA messen
{
static uint16_t rc_temp_pwrl=0;
static uint16_t rc_temp_pwrr=0;
static uint16_t rc_temp_schalt=0;
if (PINC & 1) rc_temp_pwrl++; else
if (rc_temp_pwrl) { rc_input_pwrl=rc_temp_pwrl-1; rc_temp_pwrl=0; }
if (PINC & 2) rc_temp_pwrr++; else
if (rc_temp_pwrr) { rc_input_pwrr=rc_temp_pwrr-1; rc_temp_pwrr=0; }
if (PINA & 16) rc_temp_schalt++; else
if (rc_temp_schalt) { rc_input_schalt=rc_temp_schalt-1; rc_temp_schalt=0; }
}
Funktiniert ganz ausgezeichnet =D>
Lesezeichen