Das roten Kabel des RC-Akkus sind nur mit den roten Kabeln der Servos verbunden.
Hier wäre das (bestimmt unübersichtliche) Programm:
Code:
///Hindernisausweichen mit Ultraschallsensor
#include "RP6RobotBaseLib.h"
#define pos1 320
#define pos2 410
#define pos3 600
void usleep(unsigned char usec);
extern uint8_t acs_state;
uint16_t servo_pos, servo_pos2, servo_timer, servo_dummy, Zeit;
int16_t Dist, Dist_left, Dist_right, Dist_front;
int8_t x, z, a, Dist_3;
int main(void)
{
initRobotBase();
powerON();
startStopwatch3();
startStopwatch2();
servo_pos2=pos2;
a=0;
x=1;
z=0;
Dist_3=1;
servo_pos=1000;
setACSPwrLow();
while(true)
{
if ((getStopwatch2() > 20) && (acs_state < 2))
{
servo_timer = servo_pos;
extIntON();
while(servo_timer--) servo_dummy ^= servo_timer;
extIntOFF();
servo_timer = servo_pos2;
setLEDs(0b010000);
while(servo_timer--) servo_dummy ^= servo_timer;
setLEDs(0b000000);
z=z+1;
setStopwatch2(0);
}
if(z>3)
{
Dist=0;
DDRC|=(SDA);
PORTC&=~SDA;
PORTC|=SDA;
usleep(10);
PORTC&=~SDA;
DDRC&=~(SCL);
while(!(PINC& SCL))
{
setLEDs(0b000000);
}
while(PINC& SCL)
{
Dist=Dist+1;
usleep(1);
}
if(PINC&=~SCL)
{
if(Dist<23)
{
if(Dist<12)
{
servo_pos2=pos1;
startStopwatch4();
if(servo_pos<920)
{
a=0;
changeDirection(LEFT);
moveAtSpeed(50,50);
}
else
{
if(servo_pos>1150)
{
a=0;
changeDirection(RIGHT);
moveAtSpeed(50,50);
}
else
{
a=1;
stopStopwatch3();
setStopwatch3(1030);
if(adcLSL>adcLSR) {changeDirection(LEFT); moveAtSpeed(100,100);} else {changeDirection(RIGHT); moveAtSpeed(100,100);}
}
}
}
else
{
servo_pos2=pos2;
a=0;
stopStopwatch4();
setStopwatch4(0);
changeDirection(FWD);
if(servo_pos<1050)
{
moveAtSpeed(15,100);
}
else
{
moveAtSpeed(100,15);
}
}
}
else
{
servo_pos2=pos3;
a=0;
stopStopwatch4();
setStopwatch4(0);
changeDirection(FWD);
if(Dist_left-Dist_right>10)
{
moveAtSpeed(110,45);
}
else
{
if(Dist_right-Dist_left>10)
{
moveAtSpeed(45,110);
}
else
{
moveAtSpeed(100,100);
}
}
}
}
setStopwatch1(0);
z=0;
}
if(getBumperLeft()) {move(100, BWD, DIST_MM(150), true); rotate(100, RIGHT, 70, true);}
if(getBumperRight()) {move(100, BWD, DIST_MM(150), true); rotate(100, LEFT, 70, true);}
if(getStopwatch4()>2000)
{
if(adcLSL>adcLSR) {rotate(100, LEFT, 175, true);} else {rotate(100, RIGHT, 175, true);}
stopStopwatch4();
setStopwatch4(0);
}
if(servo_pos<700 && servo_pos>600) Dist_right=Dist;
if(servo_pos<1000 && servo_pos>1060) Dist_front=Dist;
if(servo_pos<1550 && servo_pos>1450) Dist_left=Dist;
if(a==0) startStopwatch3();
Zeit=getStopwatch3();
if(getStopwatch3()>1700)
{
if(x==0)
{
x=1;
setStopwatch3(0);
}
else
{
x=0;
setStopwatch3(0);
}
}
if(a==0)
{
if(x==0)
{
servo_pos=600+Zeit/2;
}
if(x==1)
{
servo_pos=1550-Zeit/2;
}
}
else
{
servo_pos=1030;
}
//task_RP6System();
task_ADC();
task_ACS();
task_Bumpers();
task_motionControl();
}
return 0;
}
void usleep(unsigned char usec)
{
for (int s=0; s<usec; s++) {
for (long int i=0; i<140; i++) {
asm volatile("nop");
asm volatile("nop");
}
}
}
Lesezeichen