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"); 
    } 
  } 
}