Arduino.ru

Двс, коллеги. Написал небольшой скетч для меги, залил, тупит. При отладке обычным сериалом понял что проблема с массивом - тупит. Обычное целочисленное вычитание дает неверный результат. Подскажите пжта. Хистори такой: скетч для машинки с колесиками и сонаром. На машинке стоит ардуина мега+сенсоршилд. К шилду цепаныл одну серву и сонар (УЗ). Сонар должен считать, поворачивая башкой (сервой) с шагом 10градусов, расстояния до всех объектов и записать в массив. Далее в массиве определяются углы возможных направлений движения. НО при обычном вычитании ардуина гонит. Подскажите пжта в чем трабл. Последний раз програмил на Паскале в школе, поэтому чз 20 лет пришлось вспоминать паскаль и С++ изучать. Вангую что беда с типом данных переменных, но лучше профессиональная оценка. Цикл из 69ой строчки гонит...

#include <EEPROM.h>
#include <Servo.h>
#include <Ultrasonic.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C Pad(0x27,16,2);
Servo servo;
Ultrasonic Sonar(48,49);

const byte Center = 90;
const byte Range = 50;
const byte Step=10;                    //Щаг перемещения головы
const int enA = 11;                   //скорость управления двигателями
const int in1 = 22;                  //Контакты ЛЕВОЙ стороны двигателей
const int in2 = 23;                  //Контакты ЛЕВОЙ стороны двигателей
const int enB = 10;                   //скорость управления двигателями
const int in3 = 24;                  //Контакты ПРАВОЙ стороны двигателей
const int in4 = 25;                  //Контакты ПРАВОЙ стороны двигателей
const int Predel=40;
int smotrim_priamo;
int Dist=0;
int KoordDist=0;

void setup()
{ servo.attach(9); // серву подключаем

  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  Serial.begin(9600);
  Pad.init();
  Pad.backlight();
}
  
void Motor_Speed(int Speed)
{analogWrite(enA, Speed);analogWrite(enB, Speed);}
void Motor_Stop(int Time)
{digitalWrite(in1, LOW);digitalWrite(in2, LOW);digitalWrite(in3, LOW);digitalWrite(in4, LOW);delay(Time);}
void Motor_Forward(int Time)
{digitalWrite(in1, LOW);digitalWrite(in2, HIGH);digitalWrite(in3, HIGH);digitalWrite(in4, LOW);Motor_Speed(130);delay(Time);}
void Motor_Backward(int Time)
{digitalWrite(in1, HIGH);digitalWrite(in2, LOW);digitalWrite(in3, LOW);digitalWrite(in4, HIGH);Motor_Speed(250);delay(Time);}
void Motor_Right(int Time)
{digitalWrite(in1, HIGH);digitalWrite(in2, LOW);digitalWrite(in3, HIGH);digitalWrite(in4, LOW);Motor_Speed(250);delay(Time);}
void Motor_Left(int Time)
{digitalWrite(in1, LOW);digitalWrite(in2, HIGH);digitalWrite(in3, LOW);digitalWrite(in4, HIGH);Motor_Speed(250);delay(Time);}


void Head_Look(int LongMas)
{ Label2:
  int MassDist[LongMas];
  servo.write(Center-Range);delay(300);
  for(int m=0;m<=LongMas;m++)
     {servo.write(Center-Range+m*Step);
     MassDist[m]=Distance(10);
     }
  for(int m=0;m<=LongMas;m++) {Serial.print("MassDist[");Serial.print(m);Serial.print("]= ");Serial.print(MassDist[m]);Serial.print("    koords = ");Serial.println(Center-Range+m*Step);}   
  Serial.println();

 // Dist - самое дальнее расстояние до препядствия; KoordDist - координата расстояния
  boolean Flag=false;
  int b[LongMas],koords[LongMas];                     //массив троек
  int j=0;
  for (int m=0;m<=LongMas;m++){b[m]=0;koords[m]=0;}     // обнуление временного массива

  for (int m=0;m<=LongMas-2;m++)
    {
      Serial.print("m = ");Serial.print(m);Serial.print("    frst = ");Serial.print(MassDist[m]-MassDist[m+1]);Serial.print("    scnd = ");Serial.print(MassDist[m]-MassDist[m+2]);Serial.print("    m = ");Serial.println(m);
      delay(500);
      if (abs(MassDist[m]-MassDist[m+1])<=5 && abs(MassDist[m]-MassDist[m+2])<=5){b[j]=MassDist[m+1];
                                        koords[j]=Center-Range+(m+1)*Step;
                                        Flag=true;
                                        j++;}
    }
  if (Flag=true){Dist=b[0];KoordDist=koords[0];
                 for (int m=0;m<=j;m++){if (Dist<b[m]){Dist=b[m];KoordDist=koords[m];}}} 
           else {Serial.println(" NO WAY TO RIDE !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
                 goto Label2;}
  if (abs(KoordDist)>200) {goto Label2;}
  //Serial.println();Serial.print("Dist = ");Serial.print(Dist);Serial.print("    KoordDist = ");Serial.println(KoordDist);
}

int Distance(int interval){ //interval - это количество замеров    //Выбор в массиве замеров самое наиболее часто встречающееся
  int L=1, K=0,Mass_d[interval]; //массив замеров расстояний
  delay(50);
  for (int i=0; i<=interval; i++){Mass_d[i]=Sonar.Ranging(CM);}
  byte a[interval];
  for (int i=0; i<interval; i++){a[i]=0;}
  for (int i=0; i<interval; i++){for (int j=0; j<interval; j++){if (Mass_d[i]==Mass_d[j]) {a[i]=a[i]+1;}}}
  for (int i=0; i<interval; i++){if (a[i]>L) {L=a[i];K=i;}}
  if (L==1){K=0;}
  return Mass_d[K]; 
}
void loop (){
smotrim_priamo=0;
KoordDist=0;
servo.write(Center);
//servo2.write(160);
smotrim_priamo=Distance(10);
Serial.print("smotrim_priamo = ");Serial.println(smotrim_priamo);
if (smotrim_priamo==1) {smotrim_priamo=300;}
if (smotrim_priamo<Predel)
 {Motor_Stop(500);
  Label1:
  Head_Look(Range*2/Step);
  servo.write(KoordDist);delay(500);
  if (KoordDist>Center){int T=(KoordDist-Center)/2;Serial.println("Turn Left");
                        for (int i=0; i<=KoordDist-Center; i++){servo.write(KoordDist-i);Motor_Left(T);}}
  if (KoordDist<Center){int T=(Center-KoordDist)/2;Serial.println("Turn Right");
                        for (int i=0; i<=Center-KoordDist; i++){servo.write(KoordDist+i);Motor_Right(T);}}
  if (KoordDist==Center){Motor_Backward(400);Motor_Stop(500);goto Label1;}
  Motor_Stop(500);
 }
Motor_Forward(1);
}