Arduino.ru

 Собственно не могу понять почему при не соблюдении условий не выходит  из цикла While, собственно есть значение с компаса  и значение в котором машина дожна быть ( значение с компаса + 40) 

Вот код: может кто знает

#define Trig 2
#define Echo 3
#define Trigg 6
#define Echoo 7      
#define D1 8          
#define M1 9          
#define D2 10          
#define M2 11
int ledPin = 5; 
char incomingByte;
#include "Wire.h"
#include "HMC5883L.h"
HMC5883L compass;
float first_angle;
float second_angle=0;
byte second_angle_accuired = 0;

void setup() 
{ 
  Wire.begin();
  compass = HMC5883L();  
  setupHMC5883L();    
  pinMode(D1, OUTPUT);
  pinMode(D2, OUTPUT);  
  pinMode(Trig, OUTPUT); 
  pinMode(Echo, INPUT); 
  pinMode(Trigg, OUTPUT); 
  pinMode(Echoo, INPUT);
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600); 
} 
    
unsigned int impulseTime=0; 
unsigned int distance_sm=0; 
unsigned int impulseTimee=0; 
unsigned int distance_cm=0; 

void loop() 
{ 
//-------------------------------------!!!!! 
  float heading = getHeading();
  Serial.println(heading);
  delay(250);
//-------------------------------------!!!!!  
  digitalWrite(Trig, HIGH); 
  delayMicroseconds(5000);
  digitalWrite(Trig, LOW); 
  impulseTime=pulseIn(Echo, HIGH);  
  distance_sm=impulseTime/58; 
  if ( distance_sm <= 7 )
  {
    distance_sm = 21 ;
  }
  Serial.println(distance_sm);
//-------------------------------------!!!!! 
  
  digitalWrite(Trigg, HIGH); 
  delayMicroseconds(5000); 
  digitalWrite(Trigg, LOW); 
  impulseTimee=pulseIn(Echoo, HIGH); 
  distance_cm=impulseTimee/58; 
  if ( distance_cm <=7)
  {
  distance_cm = 22 ;  
  }
  Serial.println(distance_cm);
//-------------------------------------!!!!! 

if (distance_sm > 20)
{
//-------------------------------------!!!!! 
if(distance_cm <= 25)
{
int timeposition=1;
 Serial.println(timeposition);
    digitalWrite(D1, LOW);  
    digitalWrite(D2, LOW);
    analogWrite(M1, 205	);       
    analogWrite(M2, 205 );
}
//-------------------------------------!!!!! 
else if (distance_cm > 25)
{
delay (250);
    first_angle = 40.00;
        if(!second_angle_accuired)
     {
         second_angle = first_angle + heading;
        second_angle_accuired = 1;
     }
    if ( heading <= second_angle )
{  
  Serial.println( second_angle);
    digitalWrite(D1, LOW);  
    digitalWrite(D2, LOW);
    analogWrite(M1, 150	);       
    analogWrite(M2, 0 );
}
//-------------------------------------!!!!! 
}
}
//-------------------------------------!!!!! 
else if (distance_sm <= 20)
{
 int timeposition=2;
 Serial.println(timeposition);

    delay (250);
    first_angle = 10.00;
        if(!second_angle_accuired)
     { 
         second_angle = heading + first_angle;
        second_angle_accuired = 1;
     }
    while ( heading <= second_angle )
{  
  float heading = getHeading();
  first_angle = 10.00;
        if(!second_angle_accuired)
     { 
         second_angle = heading + first_angle;
        second_angle_accuired = 1;
     }
    Serial.println( heading);
  Serial.println( second_angle);
    digitalWrite(D1, LOW);  
    digitalWrite(D2, LOW);
    analogWrite( M1,  0 );       
    analogWrite(M2, 150 );
}
//-------------------------------------!!!!! 
}
}
void setupHMC5883L(){
  int error;  
  error = compass.SetScale(0.88); 
  if(error != 0) Serial.println(compass.GetErrorText(error)); 
  error = compass.SetMeasurementMode(Measurement_Continuous); 
  if(error != 0) Serial.println(compass.GetErrorText(error)); 
}
//-------------------------------------!!!!! 
 
float getHeading(){
  MagnetometerScaled scaled = compass.ReadScaledAxis(); 
  float heading = atan2(scaled.YAxis, scaled.XAxis);  
  if(heading < 0) heading += 2*PI;
  if(heading > 2*PI) heading -= 2*PI;
  return heading * RAD_TO_DEG; 
}
//-------------------------------------!!!!!