Need Help

Projects Programming
5 
#include <Wire.h>
#include <Zumo32U4.h>
#include "TurnSensor.h"

Zumo32U4Motors motors;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4LCD lcd;
L3G gyro;

  int speedleft=250;
  int speedright=speedleft*1;

void setup()
{
  lineSensors.initFiveSensors();
  lineSensors.calibrate();
  proxSensors.initFrontSensor();

  turnSensorSetup();
  delay(500);
  turnSensorReset();
  
  lcd.clear();
 // lcd.print(F("Press A"));
 // buttonA.waitForButton();
  delay(1000);

  motors.setLeftSpeed(speedleft);
  motors.setRightSpeed(speedright);
}


void loop()
{
    int counter = 2;
    while (1==1)
    {
    
    ledRed(0);
    #define NUM_SENSORS 5
    uint16_t lineSensorValues[NUM_SENSORS];
    lineSensors.read(lineSensorValues);
    proxSensors.read();
    int leftValue = proxSensors.countsFrontWithLeftLeds();
    int rightValue = proxSensors.countsFrontWithRightLeds();
   
    lcd.clear();
    lcd.gotoXY(0, 0);
    lcd.print(lineSensorValues[2]);
    lcd.gotoXY(0, 1);
    lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));

 
    turnSensorUpdate();    
 
       
    if ((lineSensorValues[2] < 300 ||  lineSensorValues[0] < 300 || lineSensorValues[4] < 300) && counter % 2 == 0)
    {
      motors.setSpeeds(-100,-100);
      delay(700);  

      while (((((int32_t)turnAngle >> 16) * 360) >> 16) != -180)
      {    
      motors.setLeftSpeed(200);
      motors.setRightSpeed(0);
      turnSensorUpdate(); 
      
      lcd.gotoXY(0, 1);
      lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
      }
      counter = counter+1;
      lineSensors.read(lineSensorValues);
    }
    proxSensors.read();
    leftValue = proxSensors.countsFrontWithLeftLeds();
    rightValue = proxSensors.countsFrontWithRightLeds();
    
    if ((lineSensorValues[2] < 300 ||  lineSensorValues[0] < 300 || lineSensorValues[4] < 300) && counter % 2 == 1)
    {
      motors.setSpeeds(-100,-100);
      delay(700);  
      
      motors.setLeftSpeed(0);
      motors.setRightSpeed(200);
      delay(20);

      while (((((int32_t)turnAngle >> 16) * 360) >> 16) != 0){    
      motors.setLeftSpeed(0);
      motors.setRightSpeed(200);
      turnSensorUpdate(); 
      lcd.gotoXY(0, 1);
      lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
      }
      counter = counter+1;
      lineSensors.read(lineSensorValues);
    }
    proxSensors.read();
    leftValue = proxSensors.countsFrontWithLeftLeds();
    rightValue = proxSensors.countsFrontWithRightLeds();


    if (((leftValue >= 5.9) || (rightValue >=5.9)) && (((((int32_t)turnAngle >> 16) * 360) > -100) || ((((int32_t)turnAngle >> 16) * 360) < 100 )))
    // if-Abfrage bei Gegenstand mit 0 Grad Ausrichtung
    {
        while (((((int32_t)turnAngle >> 16) * 360) >> 16) != 90)
        {    
          motors.setLeftSpeed(0);
          motors.setRightSpeed(200);
          turnSensorUpdate(); 
          lcd.gotoXY(0, 1);
          lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
        }
        
  
        motors.setSpeeds(speedleft,speedright);
        delay(300);
        motors.setSpeeds(0,0);
  
        while (((((int32_t)turnAngle >> 16) * 360) >> 16) != 0)
        {    
          motors.setLeftSpeed(200);
          motors.setRightSpeed(0);
          turnSensorUpdate(); 
          lcd.gotoXY(0, 1);
          lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
        }  
  
        motors.setSpeeds(speedleft,speedright);
        delay(1000);
        motors.setSpeeds(0,0);
  
        while (((((int32_t)turnAngle >> 16) * 360) >> 16) != -90)
        {    
          motors.setLeftSpeed(200);
          motors.setRightSpeed(0);
          turnSensorUpdate(); 
          lcd.gotoXY(0, 1);
          lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
        }  
        
        motors.setSpeeds(speedleft,speedright);
        delay(300);
        motors.setSpeeds(0,0);
  
        while (((((int32_t)turnAngle >> 16) * 360) >> 16) != 0)
        {    
          motors.setLeftSpeed(00);
          motors.setRightSpeed(200);
          turnSensorUpdate(); 
          lcd.gotoXY(0, 1);
          lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
        }               
      }

    proxSensors.read();
    leftValue = proxSensors.countsFrontWithLeftLeds();
    rightValue = proxSensors.countsFrontWithRightLeds();
      
      if (((leftValue >= 5.9) || (rightValue >=5.9)) && (((((int32_t)turnAngle >> 16) * 360) < -100) || ((((int32_t)turnAngle >> 16) * 360) > 100 )))
      {
      // if-Abfrage bei Gegenstand mit 179 Grad Ausrichtung
      ledGreen(1);
      while (((((int32_t)turnAngle >> 16) * 360) >> 16) != -90){    
        motors.setLeftSpeed(0);
        motors.setRightSpeed(200);
        turnSensorUpdate(); 
        lcd.gotoXY(0, 1);
       // lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
       lcd.print('HILFE');
      }
      
      motors.setSpeeds(speedleft,speedright);
      delay(300);
      motors.setSpeeds(0,0);

      while (((((int32_t)turnAngle >> 16) * 360) >> 16) != -180)
      {    
        motors.setLeftSpeed(200);
        motors.setRightSpeed(0);
        turnSensorUpdate(); 
        lcd.gotoXY(0, 1);
        lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
      }  

      motors.setSpeeds(speedleft,speedright);
      delay(1000);
      motors.setSpeeds(0,0);

      motors.setLeftSpeed(200);
      motors.setRightSpeed(0);
      delay(50);
      turnSensorUpdate(); 

      while (((((int32_t)turnAngle >> 16) * 360) >> 16) != 90)
      {    
        motors.setLeftSpeed(200);
        motors.setRightSpeed(0);
        turnSensorUpdate(); 
        lcd.gotoXY(0, 1);
        lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
      }  
      
      motors.setSpeeds(speedleft,speedright);
      delay(300);
      motors.setSpeeds(0,0);

      while (((((int32_t)turnAngle >> 16) * 360) >> 16) != 179)
      {    
        motors.setLeftSpeed(0);
        motors.setRightSpeed(200);
        turnSensorUpdate(); 
        lcd.gotoXY(0, 1);
        lcd.print(((((int32_t)turnAngle >> 16) * 360) >> 16));
      } 
      } 
      ledGreen(0);
  
    motors.setLeftSpeed(speedleft);
    motors.setRightSpeed(speedright);
    }
}

UKHeliBob:
So what did you try ?
What is the specification for the bot ?

As you can see I have a "decent" code but.... The Problem ist that every-time the robot tries to curve a obstacle it looses control over the line sensor and it leaves the battleground.

Is there any prioritization?

I don't know the specifications - it is a normal Zumo 32U4

Thank You!