Arduino nano memória hiba

Processing/Wiring (illetve C) nyelvű programozási fogások, tippek. (AVR-Duino, Arduino, EthDuino, Diecimila, Severino, Nano, LilyPad)
Válasz küldése
IzLuCSi
DrótVégénSzéndarab
Hozzászólások: 47
Csatlakozott: 2017. szeptember 26. kedd, 14:34

Arduino nano memória hiba

Hozzászólás Szerző: IzLuCSi »

Mekkora program esetén fordulhat elő olyan hogy program nem azt csinálja amit kellene neki?
Egy arduino nanora töltöttem rá egy (kb jegyzettekkel együtt 954 soros programot ) 25070 byte -os programot. A program külön külön müködött jól de egyben össze vissza. Szervokat kéne vezérelnie. Kiirattam program futás közben az értékeit és az értékek jók voltak jól is vannak bekötve de mégis össze vissza forognak a szervok.
Mekkorára kéne lecsökkentenem a program méretett hogy ne legyen baj ha ez okoza a gondot?
Vissza a tetejére
Avatar
Robert
Elektronbűvölő
Hozzászólások: 10191
Csatlakozott: 2005. december 9. péntek, 7:00

Re: Arduino nano memória hiba

Hozzászólás Szerző: Robert »

Kód nélkül macerás.... :)

Nem a flash, hanem a SRAM szokott általában elfogyni.
De lehet HW hiva is (ennyi szervo nagy áramot vesz/venne fel egyszerre mozgatva).
Van még 10-15 lehetőség biztosan (táp, RF zavar, SW/HW hiba...)
http://www.tavir.hu - a gazda :)
Vissza a tetejére
Avatar
Robert
Elektronbűvölő
Hozzászólások: 10191
Csatlakozott: 2005. december 9. péntek, 7:00

Re: Arduino nano memória hiba

Hozzászólás Szerző: Robert »

Memfree() parancs megmondja hogy mennyi szabad helyed van....
http://www.tavir.hu - a gazda :)
Vissza a tetejére
vargham
Chipgyilok
Hozzászólások: 308
Csatlakozott: 2014. január 8. szerda, 8:32

Re: Arduino nano memória hiba

Hozzászólás Szerző: vargham »

1. Kód.
2. Ahogy Róbert mondja, nem a flash szokott elfogyni, hanem a RAM. Ha a túl nagy lenne a programkódod, akkor nem is engedné feltölteni.
Továbbiakat lásd itt: viewtopic.php?f=23&t=2126
Vissza a tetejére
IzLuCSi
DrótVégénSzéndarab
Hozzászólások: 47
Csatlakozott: 2017. szeptember 26. kedd, 14:34

Re: Arduino nano memória hiba

Hozzászólás Szerző: IzLuCSi »

Ez volt az eredeti kód:

Kód: Egész kijelölése


#define  emel  2000
#include <Servo.h> 
#include <SoftwareSerial.h>
#include <PID_v1.h>



SoftwareSerial HC12(A2, A3); // HC-12 TX Pin, HC-12 RX Pin


 #include <Wire.h>
  #include <Kalman.h> 
  #define RESTRICT_PITCH 
  Kalman kalmanX; 
  Kalman kalmanY;
  
  double accX, accY, accZ;
  double gyroX, gyroY, gyroZ;
  
  double kalAngleX, kalAngleY; 
  uint32_t timer;
  uint8_t i2cData[14]; 


String nul;
String tobi;
int sor;
int hos;
int ertek;
int tomb[] = {};

Servo ESC1;   
Servo ESC2;

Servo moz1;        //JE1
Servo moz2;        //JE2
Servo moz3;        //JE3
Servo moz4;        //BE1
Servo moz5;        //BE2
Servo moz6;        //BE3

Servo moz7;       //JH1
Servo moz8;       //JH2
Servo moz9;       //JH3
Servo moz10;      //BH1
Servo moz11;      //BH2
Servo moz12;      //BH3

//mászkálós mód
bool  szak1;
bool  szak2;
bool  szak3;
bool  szakREP;
bool  szakSTOP;

byte  elo;       //érték
bool irany;     //előjel
bool oldal;     //előjel
byte  old;       //érték
byte  k;          //oldal alapján 1 vagy -1
//változók definiálása

double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;
//paraméterek

PID myPID1(&Input1, &Output1, &Setpoint1, 2, 5, 1, DIRECT);
PID myPID2(&Input2, &Output2, &Setpoint2, 2, 5, 1, DIRECT);

//MOON mód



//mód váltó
bool OV;
//----------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//void setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setup
void setup() { 
   HC12.begin(9600); 
  
    ESC1.attach(A0,1000,2000); 
    ESC2.attach(A1,1000,2000);


    Input1 =kalAngleX;
    Input2 =kalAngleY;
 myPID1.SetMode(AUTOMATIC);
  myPID2.SetMode(AUTOMATIC);
  
  //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    
 
     Wire.begin();
  #if ARDUINO >= 157
    Wire.setClock(400000UL); // Set I2C frequency to 400kHz
  #else
    TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
  #endif
  
    i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
    i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
    i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
    i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
    while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
    while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
  
    while (i2cRead(0x75, i2cData, 1));
    delay(100); // Wait for sensor to stabilize
  
    /* Set kalman and gyro starting angle */
    while (i2cRead(0x3B, i2cData, 6));
    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  
   //#ifdef RESTRICT_PITCH // Eq. 25 and 26
    double roll  = atan2(accY, accZ) * RAD_TO_DEG;
    double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
   
    kalmanX.setAngle(roll); // Set starting angle
    kalmanY.setAngle(pitch);
  
    timer = micros();
 
  
 //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    
  
 
 
 
 
  
  
moz1.attach(2); 
moz2.attach(3); 
moz3.attach(4); 
moz4.attach(5); 
moz5.attach(6); 
moz6.attach(7); 

moz7.attach(8); 
moz8.attach(9); 
moz9.attach(10); 
moz10.attach(11); 
moz11.attach(12); 
moz12.attach(13); 
//----------------------------------------------------------------------------------------------------------------------------

 //alap állapot
 moz1.write(0);
 moz2.write(90);
 moz3.write(180);

 moz4.write(180);
 moz5.write(90);
 moz6.write(0);
 
 moz7.write(135);
 moz8.write(90);
 moz9.write(0);
 
 moz10.write(45);
 moz11.write(90);
 moz12.write(180);
//----------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------




} 
void loop() {// elvileg bal első jobb hátsó láb

  //FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    
   while (HC12.available()) {        // ha HC12 ben van adatt
      String bej=String(HC12.read());      //bejövő adat string konvertáltja beküldése a bej változóba
      nul =String(bej.substring(0));        //string első karakterét a nul értékbe visszük
      sor = nul.toInt();                    //nul konvertálása stringből inté

      hos = bej.length();                    
      tobi  =String(bej.substring(1,hos));
      ertek = tobi.toInt();
      ertek = tomb[sor];
      }

  //FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    

 //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    
 
     while (i2cRead(0x3B, i2cData, 14));
    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  
    gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
    gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
    gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
  
    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
    timer = micros();
  
     double roll  = atan2(accY, accZ) * RAD_TO_DEG;
    double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
     double gyroXrate = gyroX / 131.0; // Convert to deg/s
    double gyroYrate = gyroY / 131.0; // Convert to deg/s

     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
      kalmanX.setAngle(roll);
  
      kalAngleX = roll;
  
    } else
      kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  
    if (abs(kalAngleX) > 90)
      gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);

 
 
 
 
 //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    






  if(oldal==true){
    k=1;
  }
  else{
    k=-1;}

//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------------
//INDUL

for(int i=1; i<11&& elo!=0&&moz1.read()<10&&OV==false&&szak2==false; i++){
  if(i==10){ 
    szak1=true;}
     
  moz11.write(90+i);
  moz2.write(90+i);
 moz1.write(0+i);
 moz10.write(45-i);
 delay (elo);
  }
 
 
 
 for(int i=1; i<26&& elo!=0&&moz1.read()<35&& szak1==true&&OV==false; i++){
  if(i==10){ 
    szak2=true;}
  
 moz2.write(100);   
 moz11.write(100);

 moz1.write(10+i);
 moz10.write(35-i);
  delay (elo);
 }


for(int i=1; i<11&& elo!=0&&moz1.read()<45&& szak2==true&&OV==false; i++){
  if(i==10){ 
    szak3=true;}
     
  moz11.write(100-i);
  moz2.write(100-i);
 moz1.write(35+i);
 moz10.write(10-i);
  }

  
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ 
//----------------------------------------------------------------------------------------------------------------------------
//FUT

for(;elo!=0&& szak3==true&&OV==false;){
//for 1 végez aztán for 2 végez aztán for 3 végez ....
// vagy
//for 1 akkor van ha i=1 for2 ha i=2 ... végén i=0nulláz
//****************************
//irány meg adja hogy melyik jöjjön melyik után és hogy + vagy - 1 el nöjjön
// feltétel is változik az iránnyal


int j;
if(irany= true){
  j=1;}
  else{
    j=-1;}

for(int i=1;35<=moz1.read()<45&&moz2.read()==90&&OV==false; i=i+j){

//JE1
  moz1.write(45-i);
 moz2.write(90);
  //BH
 moz10.write(0+i);
 moz11.write(90);
//BE
 moz4.write(180-i);
 moz5.write(90-i);
 //JH
 moz7.write(135+i);
 moz8.write(90-i);}

//************************************************************************************************************************************************************************************

if(k!=0&&old!=0){
//********************************************************************
//FÖLD LÁB FORDUL
for(;moz4.read()==(170)&&moz1.read()!=(35+k*old)&&OV==false;){
//JEf1.1
  moz1.write(35+k);
 moz2.write(90);
  //BH
 moz10.write(10+k);
 moz11.write(90);
//BE
 moz4.write(170);
 moz5.write(80);
 //JH                         
 moz7.write(145);
 moz8.write(80);
}
//*******************************************************************
// LEVEGŐ LÁB LETESZ // FÖLD LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf1.2
  moz1.write(moz1.read());
 moz2.write(90+i);
  //BH
 moz10.write(moz10.read());
 moz11.write(90+i);
//BE
 moz4.write(170);
 moz5.write(80+i);
 //JH                         
 moz7.write(145);
 moz8.write(80+i);
}
//*******************************************************************
//FÖLD LÁB FORDUL
for(;moz4.read()==(170)&&moz1.read()!=(35)&&OV==false;){
//JEf1.3
  moz1.write(moz1.read()-k);
 moz2.write(100);
  //BH
 moz10.write(moz10.read()-k);
 moz11.write(100);
//BE
 moz4.write(170);
 moz5.write(90);
 //JH                         
 moz7.write(145);
 moz8.write(90);
}
//*******************************************************************
// FÖLD LÁB LETESZ // LEVEGŐ LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf1
  moz1.write(35);
 moz2.write(100-i);
  //BH
 moz10.write(10);
 moz11.write(100-i);
//BE
 moz4.write(170);
 moz5.write(90-i);
 //JH                         
 moz7.write(145);
 moz8.write(90-i);
}
//*******************************************************************  
}

//************************************************************************************************************************************************************************************
//**************************************************************************************************************************

 for(int i=1;10<=moz1.read()<35&&moz2.read()==90&&OV==false;  i=i+j){
//JE2
  moz1.write(35-i);
 moz2.write(90);
  //BH
 moz10.write(10+i);
 moz11.write(90);
//BE
 moz4.write(170-i);
 moz5.write(80);
 //JH
 moz7.write(145+i);
 moz8.write(80);}
//************************************************************************************************************************************************************************************

if(k!=0&&old!=0){
//********************************************************************
//FÖLD LÁB FORDUL
for(;moz4.read()==(145)&&moz1.read()!=(10+k*old)&&OV==false;){
//JEf2.1
  moz1.write(10+k);
 moz2.write(90);
  //BH
 moz10.write(35+k);
 moz11.write(90);
//BE
 moz4.write(145);
 moz5.write(80);
 //JH                         
 moz7.write(170);
 moz8.write(80);
}
//*******************************************************************
// LEVEGŐ LÁB LETESZ // FÖLD LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf2.2
  moz1.write(moz1.read());
 moz2.write(90+i);
  //BH
 moz10.write(moz10.read());
 moz11.write(90+i);
//BE
 moz4.write(145);
 moz5.write(80+i);
 //JH                         
 moz7.write(170);
 moz8.write(80+i);
}
//*******************************************************************
//FÖLD LÁB FORDUL
for(;moz4.read()==(145)&&moz1.read()!=(10)&&OV==false;){
//JEf2
  moz1.write( moz1.read()-k);
 moz2.write(100);
  //BH
 moz10.write(moz10.read()-k);
 moz11.write(100);
//BE
 moz4.write(145);
 moz5.write(90);
 //JH                         
 moz7.write(170);
 moz8.write(90);
}
//*******************************************************************
// FÖLD LÁB LETESZ // LEVEGŐ LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf2
  moz1.write(10);
 moz2.write(100-i);
  //BH
 moz10.write(35);
 moz11.write(100-i);
//BE
 moz4.write(145);
 moz5.write(90-i);
 //JH                         
 moz7.write(170);
 moz8.write(90-i);
}
//*******************************************************************  
}

//************************************************************************************************************************************************************************************
//**************************************************************************************************************************

 for(int i=1;0<=moz1.read()<10&&moz2.read()==90&&OV==false; i=i+j){
//JE3
  moz1.write(10-i);
 moz2.write(90);
  //BH
 moz10.write(35+i);
 moz11.write(90);
//BE
 moz4.write(145-i);
 moz5.write(80+i);
 //JH
 moz7.write(170+i);
 moz8.write(80+i);}
//**************************************************************************************************************************

 for(int i=1;10>=moz1.read()>0&&moz5.read()==90&&OV==false;  i=i+j){
//JE4
  moz1.write(0+i);
 moz2.write(90+i);
  //BH
 moz10.write(45-i);
 moz11.write(90+i);
//BE
 moz4.write(135+i);
 moz5.write(90);
 //JH
 moz7.write(180-i);
 moz8.write(90);}
//************************************************************************************************************************************************************************************

if(k!=0&&old!=0){
//********************************************************************
//FÖLD LÁB FORDUL
for(;moz1.read()==(10)&&moz4.read()!=(145+k*old)&&OV==false;){
//JEf4.1
  moz1.write(10);
 moz2.write(100);
  //BH
 moz10.write(35);
 moz11.write(100);
//BE
 moz4.write(145+k);
 moz5.write(90);
 //JH                         
 moz7.write(170+k);
 moz8.write(90);
}
//*******************************************************************
// LEVEGŐ LÁB LETESZ // FÖLD LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf4.2
  moz1.write(10);
 moz2.write(100-i);
  //BH
 moz10.write(35);
 moz11.write(100-i);
//BE
 moz4.write(moz4.read());
 moz5.write(90-i);
 //JH                         
 moz7.write(moz7.read());
 moz8.write(90-i);
}
//*******************************************************************
//FÖLD LÁB FORDUL
for(;moz1.read()==(10)&&moz4.read()!=(145)&&OV==false;){
//JEf4
  moz1.write(10);
 moz2.write(90);
  //BH
 moz10.write(35);
 moz11.write(90);
//BE
 moz4.write(moz4.read()-k);
 moz5.write(80);
 //JH                         
 moz7.write(moz7.read()-k);
 moz8.write(80);
}
//*******************************************************************
// FÖLD LÁB LETESZ // LEVEGŐ LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf4
  moz1.write(10);
 moz2.write(90+i);
  //BH
 moz10.write(35);
 moz11.write(90+i);
//BE
 moz4.write(145);
 moz5.write(80+i);
 //JH                         
 moz7.write(170);
 moz8.write(80+i);
}
//*******************************************************************  
}

//************************************************************************************************************************************************************************************
//**************************************************************************************************************************

 for(int i=1;35>=moz1.read()>10&&moz5.read()==90&&OV==false;  i=i+j){
//JE5
  moz1.write(10+i);
 moz2.write(100);
  //BH
 moz10.write(35-i);
 moz11.write(100);
//BE
 moz4.write(145+i);
 moz5.write(90);
 //JH
 moz7.write(170-i);
 moz8.write(90);}
//************************************************************************************************************************************************************************************

if(k!=0&&old!=0){
//********************************************************************
//FÖLD LÁB FORDUL
for(;moz1.read()==(35)&&moz4.read()!=(170+k*old)&&OV==false;){
//JEf4.1
  moz1.write(35);
 moz2.write(100);
  //BH
 moz10.write(10);
 moz11.write(100);
//BE
 moz4.write(170+k);
 moz5.write(90);
 //JH                         
 moz7.write(145+k);
 moz8.write(90);
}
//*******************************************************************
// LEVEGŐ LÁB LETESZ // FÖLD LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf4.2
  moz1.write(35);
 moz2.write(100-i);
  //BH
 moz10.write(10);
 moz11.write(100-i);
//BE
 moz4.write(moz4.read());
 moz5.write(90-i);
 //JH                         
 moz7.write(moz7.read());
 moz8.write(90-i);
}
//*******************************************************************
//FÖLD LÁB FORDUL
for(;moz1.read()==(35)&&moz4.read()!=(170)&&OV==false;){
//JEf4
  moz1.write(35);
 moz2.write(90);
  //BH
 moz10.write(10);
 moz11.write(90);
//BE
 moz4.write(moz4.read()-k);
 moz5.write(80);
 //JH                         
 moz7.write(moz7.read()-k);
 moz8.write(80);
}
//*******************************************************************
// FÖLD LÁB LETESZ // LEVEGŐ LÁB EMEL
for(int i=1;i<11&&OV==false;i++){
//JEf4
  moz1.write(35);
 moz2.write(90+i);
  //BH
 moz10.write(10);
 moz11.write(90+i);
//BE
 moz4.write(170);
 moz5.write(80+i);
 //JH                         
 moz7.write(145);
 moz8.write(80+i);
}
//*******************************************************************  
}

//************************************************************************************************************************************************************************************
//**************************************************************************************************************************

 for(int i=1;45>=moz1.read()>35&&moz5.read()==90&&OV==false;  i=i+j){
//JE6
  moz1.write(35+i);
 moz2.write(100-i);
  //BH
 moz10.write(10-i);
 moz11.write(100-i);
//BE
 moz4.write(170+i);
 moz5.write(90);
 //JH
 moz7.write(145-i);
 moz8.write(90);}

}
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

//MEG ÁLL
if(elo==0&& szak3==true){

//BLOKOLNI AZ INDITÁST MÍG VISSZA ÁLL VAGY INDITANI
//********************************************************************************************************************************
    if(90!=moz2.read()){
for(;moz1.read()>=10;){
//JE
  moz1.write(moz1.read()-1);
 moz2.write(moz2.read());
  //BH
 moz10.write(moz10.read()+1);
 moz11.write(moz11.read());}
for(;moz1.read()<180&&moz1.read()>170;){
//JE
  moz1.write(moz1.read()-1);
 moz2.write(moz2.read()-1);
  //BH
 moz10.write(moz10.read()+1);
 moz11.write(moz11.read()-1);}
 if(moz1.read()==0&&moz2.read()==90){


//HA A LEVEGŐ LÁB VISSZA ÉRT

if(moz4.read()!=180){
for(;moz5.read()<=80;){
//BE
 moz4.write(moz4.read());
 moz5.write(moz5.read()-1);
 //JH
 moz7.write(moz7.read());
 moz8.write(moz8.read()-1);}
 for(;moz4.read()!=0;){
 //BE
 moz4.write(moz4.read()+1);
 moz5.write(moz5.read());
 //JH
 moz7.write(moz7.read()-1);
 moz8.write(moz8.read()); }
 for(;moz4.read()==180&&moz5.read()!=90;){
 //BE
 moz4.write(moz4.read());
 moz5.write(moz5.read()+1);
 //JH
 moz7.write(moz7.read());
 moz8.write(moz8.read()+1); }
 }

  }
 }
//*********************************************************************************************************************************************
//HA A MÁSIK LÁB VAN A LEVEGŐBEN


      if(90!=moz5.read()){
     
for(;moz4.read()<=170;){
//JE
  moz4.write(moz4.read()+1);
 moz5.write(moz5.read());
  //BH
 moz7.write(moz7.read()-1);
 moz8.write(moz8.read());}
for(;moz4.read()<180&&moz4.read()<=10;){
//JE
  moz4.write(moz4.read()+1);
 moz5.write(moz5.read()+1);
  //BH
 moz7.write(moz7.read()-1);
 moz8.write(moz8.read()+1);}
 if(moz4.read()==180&&moz5.read()==90){


//HA A LEVEGŐ LÁB VISSZA ÉRT

if(moz1.read()!=0){
for(;moz2.read()>=100;){
//BE
 moz1.write(moz1.read());
 moz2.write(moz2.read()+1);
 //JH
 moz10.write(moz10.read());
 moz11.write(moz11.read()+1);}
 for(;moz1.read()!=0;){
 //BE
 moz1.write(moz1.read()-1);
 moz2.write(moz2.read());
 //JH
 moz10.write(moz10.read()+1);
 moz11.write(moz11.read()); }
 for(;moz1.read()==0&&moz2.read()!=90;){
 //BE
 moz1.write(moz1.read());
 moz2.write(moz2.read()-1);
 //JH
 moz10.write(moz10.read());
 moz11.write(moz11.read()-1); }
 }}}


//*************************************************************************************************************************************

//egyik láb sincs levegőben AKKOR AZ EGYIK A HELYÉN 
//VAN MÁSIK MEG VÉG ÁLLÁSBAN VISSZA FELÉ LEFUTATNI A
//KIMENÉST
if(90==moz5.read()&&90==moz2.read()){

  if(moz1.read()==0){
     for(int i=1;i<11&&moz4.read()>145; i++){
//BE
 moz4.write(135+i);
 moz5.write(90-i);
 //JH
 moz7.write(180-i);
 moz8.write(90-i);}
//****************************
 for(int i=1;i<26&&moz4.read()>170; i++){
//BE
 moz4.write(145+i);
 moz5.write(80);
 //JH
 moz7.write(170-i);
 moz8.write(80);}
//****************************
 for(int i=1;i<11&&moz4.read()>180; i++){
//BE
 moz4.write(170+i);
 moz5.write(80+i);
 //JH
 moz7.write(145-i);
 moz8.write(80+i);}

  }

//******************************************************************************************************************************
  else{
       
  
    
     for(int i=1;i<11&&moz1.read()>35; i++){
//JE6
  moz1.write(45-i);
 moz2.write(90+i);
  //BH
 moz10.write(0+i);
 moz11.write(90+i);
    
    }
//****************************
 for(int i=1;i<26&&moz1.read()>10; i++){
//JE5
  moz1.write(35-i);
 moz2.write(100);
  //BH
 moz10.write(10+i);
 moz11.write(100);
//****************************
}

    for(int i=1;i<11&&moz1.read()>0; i++){
//JE4
  moz1.write(10-i);
 moz2.write(100-i);
  //BH
 moz10.write(35+i);
 moz11.write(100-i);}
    

 }  }
//-------------------------------------------------------------------------------------------------
 if(moz4.read()==180&&moz1.read()==0){ 
    szak1=false;
    szak2=false;
    szak3=false;
    szakSTOP=true;}
}
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//ÁLLÓ FORDULÁS
for(;elo==0&&old!=0&&OV==false;){
 if(k=-1){
 for(int i=1;i<11;i++){
  //JE1
  moz1.write(0);
 moz2.write(90);
  //BH
 moz10.write(45);
 moz11.write(90);
//BE
 moz4.write(180);
 moz5.write(90-i);
 //JH                         
 moz7.write(135);
 moz8.write(90-i);}
   if(k!=0&&old!=0){
//********************************************************************
//FÖLD LÁB FORDUL
for(;moz4.read()==170&&moz1.read()!=(35+k*old);){
  }
 for(;moz1.read()<0+k*old&&moz2.read()>90;){
  //JE2
  moz1.write(0+k);
 moz2.write(90);
  //BH
 moz10.write(45+k);
 moz11.write(90);
//BE
 moz4.write(180);
 moz5.write(80);
 //JH                         
 moz7.write(135);
 moz8.write(80);
  
  }
  for(int i=1;i<11;i++){
//JE3
  moz1.write(moz1.read());
 moz2.write(90+i);
  //BH
 moz10.write(moz10.read());
 moz11.write(90+i);
//BE
 moz4.write(180);
 moz5.write(80+i);
 //JH                         
 moz7.write(135);
 moz8.write(80+i);
}

  for(;moz1.read()>0;){
//JE4
  moz1.write(moz1.read()-k);
 moz2.write(100);
  //BH
 moz10.write(moz10.read()-k);
 moz11.write(100);
//BE
 moz4.write(180);
 moz5.write(90);
 //JH                         
 moz7.write(135);
 moz8.write(90);
}
  
  for(int i=1;i<11;i++){
  //JE1
  moz1.write(0);
 moz2.write(100-i);
  //BH
 moz10.write(45);
 moz11.write(100-i);
//BE
 moz4.write(180);
 moz5.write(90);
 //JH                         
 moz7.write(135);
 moz8.write(90);}
  }}

  
//**************************************************************
 
 else{
 for(int i=1;i<11;i++){
  //JE
  moz1.write(0);
 moz2.write(90+i);
  //BH
 moz10.write(45);
 moz11.write(90+i);
//BE
 moz4.write(180);
 moz5.write(90);
 //JH                         
 moz7.write(135);
 moz8.write(90);
  
  }
   for(;moz4.read()>180+k*old&&moz4.read()>90;){
      //JE
  moz1.write(0);
 moz2.write(100);
  //BH
 moz10.write(45);
 moz11.write(100);
//BE
 moz4.write(180+k);
 moz5.write(90);
 //JH                         
 moz7.write(135+k);
 moz8.write(90); }

 for(int i=1;i<11;i++){
//JE3
  moz1.write(0);
 moz2.write(100-i);
  //BH
 moz10.write(45);
 moz11.write(100-i);
//BE
 moz4.write(moz4.read());
 moz5.write(90-i);
 //JH                         
 moz7.write(moz7.read());
 moz8.write(90-i);
}

  for(;moz4.read()>180;){
//JE4
  moz1.write(0);
 moz2.write(90);
  //BH
 moz10.write(45);
 moz11.write(90);
//BE
 moz4.write(moz4.read()-k);
 moz5.write(80);
 //JH                         
 moz7.write(moz7.read()-k);
 moz8.write(80);
}
 for(int i=1;i<11;i++){
  //JE5
  moz1.write(0);
 moz2.write(90);
  //BH
 moz10.write(45);
 moz11.write(90);
//BE
 moz4.write(180);
 moz5.write(80+i);
 //JH                         
 moz7.write(135);
 moz8.write(80+i);}

}} 


  //REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET



for(int i=1; i<11&& elo!=0&&moz1.read()<10&&OV==false&&szak2==false; i++){

  moz2.write(90+i);
 moz1.write(0+i);

 delay (elo);
  }

 for(int i=1; i<26&& elo!=0&&moz1.read()<35&& szak1==true&&OV==false; i++){
  
 moz2.write(100);   
 moz1.write(10+i);

  delay (elo);
 }


for(int i=1; i<11&& elo!=0&&moz1.read()<45&& szak2==true&&OV==false; i++){
    moz2.write(100-i);
 moz1.write(35+i);

  }





for(int i=1;45>=moz1.read()>35&&moz2.read()==90&&OV==false; i=i++){
//BE
 moz4.write(180-i);
 moz5.write(90-i);}
 for(int i=1;35>=moz1.read()>10&&moz2.read()==90&&OV==false;  i=i++){
//BE
 moz4.write(170-i);
 moz5.write(80);}
 for(int i=1;10>=moz1.read()>0&&moz2.read()==90&&OV==false; i=i++){
//BE
 moz4.write(145-i);
 moz5.write(80+i);}





ESC1.write(emel);
ESC2.write(emel);
   //REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET

                                               //láb behajlít
for(int i=0;moz2.read()!=0&&moz8.read()!=180;i++){
//JE
 moz1.write(45);   //bemozgatva
 moz2.write(90-i);   //MOZGAT
 moz3.write(180);    //alap
//BH
 moz4.write(135);   //bemozgatva
 moz5.write(90+i);   //MOZGAT
 moz6.write(0);    //alap

 //BE
 moz7.write(135);   //bemozgatva
 moz8.write(90+i);   //MOZGAT
 moz9.write(0);    //alap
 //JH
 moz10.write(45);   //bemozgatva
 moz11.write(90-i);   //MOZGAT
 moz12.write(180);    //alap
 }
 if(OV==true){
 szakREP=true;
 }
  
   

  
  // PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  
  
for(  szakSTOP=false;OV==true&&szakREP==true;){  
    Setpoint1 = tomb[0];
    // outputot max minnel be állítani a megfelelőre;
    Setpoint2 =tomb[1];
    Input1 =  kalAngleX;
    Input2 =  kalAngleY;

  myPID1.Compute();
  myPID2.Compute();
  int  gelo = map(Output2, 0, 90, 0, 45);
  int  ghat = map(Output2, 0, -90, 0, 45);
  int  gjob = map(Output1, 0, 90, 0, 45);
  int  gbal = map(Output1, 0, -90, 0, 45);
  gelo =constrain(gelo, 0, 45);
  ghat =constrain(ghat, 0, 45);
  gjob =constrain(gjob, 0, 45);   
  gbal =constrain(gbal, 0, 45);    
 //az értékek össze adódnak
     //JE
     moz2.write(gelo+gjob);   //
     moz3.write(gelo+gjob);    //
     //JH
     moz8.write(ghat+gjob);   //
     moz9.write(ghat+gjob);    //
     //BH
     moz11.write(ghat+gbal);   //
     moz12.write(ghat+gbal);    //
     //BE
     moz5.write(gelo+gbal);   //
     moz6.write(gelo+gbal);    //

 }
 if (OV==false)
 {
 
  szakREP=false;
 
 }
}
Itt meg próbáltam függvényekkel de itt sem müködött.

Kód: Egész kijelölése

#define  emel  2000
#include <Servo.h> 
#include <SoftwareSerial.h>
#include <PID_v1.h>
#include <Wire.h>
#include <Kalman.h> 
#define RESTRICT_PITCH 
SoftwareSerial HC12(A2, A3); // HC-12 TX Pin, HC-12 RX Pin
  Kalman kalmanX; 
  Kalman kalmanY;
  
  double accX, accY, accZ;
  double gyroX, gyroY, gyroZ;
  
  double kalAngleX, kalAngleY; 
  uint32_t timer;
  uint8_t i2cData[14]; 
int i;

String nul;
String tobi;
int sor;
int hos;
int ertek;
int tomb[] = {};

Servo ESC1;   
Servo ESC2;

Servo moz1;        //JE1
Servo moz2;        //JE2
Servo mozf;        //JE3
Servo moz4;        //BE1
Servo moz5;        //BE2
Servo mozl;        //BE3

Servo moz7;       //JH1
Servo moz8;       //JH2
Servo moz10;      //BH1
Servo moz11;      //BH2

//mászkálós mód
bool  szak1;
bool  szak2;
bool  szak3;
bool  szakREP;
bool  szakSTOP;

int  elo;       //érték
bool irany;     //előjel
int oldal;     //előjel
int  old;       //érték
int  k;          //oldal alapján 1 vagy -1
//változók definiálása

double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;
//paraméterek

PID myPID1(&Input1, &Output1, &Setpoint1, 2, 5, 1, DIRECT);
PID myPID2(&Input2, &Output2, &Setpoint2, 2, 5, 1, DIRECT);

//MOON mód



//mód váltó
bool OV;
//----------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//void setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setupvoid setup
void setup() { 
   HC12.begin(9600); 
  
    ESC1.attach(A0,1000,2000); 
    ESC2.attach(A1,1000,2000);


    Input1 =kalAngleX;
    Input2 =kalAngleY;
 myPID1.SetMode(AUTOMATIC);
  myPID2.SetMode(AUTOMATIC);
  
  //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    
 
     Wire.begin();
  #if ARDUINO >= 157
    Wire.setClock(400000UL); // Set I2C frequency to 400kHz
  #else
    TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
  #endif
  
    i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
    i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
    i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
    i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
    while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
    while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
  
    while (i2cRead(0x75, i2cData, 1));
    delay(100); // Wait for sensor to stabilize
  
    /* Set kalman and gyro starting angle */
    while (i2cRead(0x3B, i2cData, 6));
    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  
   //#ifdef RESTRICT_PITCH // Eq. 25 and 26
    double roll  = atan2(accY, accZ) * RAD_TO_DEG;
    double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
   
    kalmanX.setAngle(roll); // Set starting angle
    kalmanY.setAngle(pitch);
  
    timer = micros();
 
  
 //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    
  
 
 
 
 
  
  
moz1.attach(2); 
moz2.attach(3); 
mozf.attach(4); 
moz4.attach(5); 
moz5.attach(6); 
mozl.attach(7); 

moz7.attach(8); 
moz8.attach(9); 

moz10.attach(11); 
moz11.attach(12); 

//----------------------------------------------------------------------------------------------------------------------------

 //alap állapot
 moz1.write(0);
 moz2.write(90);
 mozf.write(180);

 moz4.write(180);
 moz5.write(90);
 mozl.write(0);

 moz7.write(135);
 moz8.write(90);
 moz10.write(45);
 moz11.write(90);

//----------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------




} 
void loop() {// elvileg bal első jobb hátsó láb

  //FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    
   while (HC12.available()) {        // ha HC12 ben van adatt
      String bej=String(HC12.read());      //bejövő adat string konvertáltja beküldése a bej változóba
      nul =String(bej.substring(0));        //string első karakterét a nul értékbe visszük
      sor = nul.toInt();                    //nul konvertálása stringből inté

      hos = bej.length();                    
      tobi  =String(bej.substring(1,hos));
      ertek = tobi.toInt();
      ertek = tomb[sor];
      }

  //FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    FOGAD    
 //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    
     while (i2cRead(0x3B, i2cData, 14));
    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  
    gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
    gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
    gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
  
    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
    timer = micros();
  
     double roll  = atan2(accY, accZ) * RAD_TO_DEG;
    double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
     double gyroXrate = gyroX / 131.0; // Convert to deg/s
    double gyroYrate = gyroY / 131.0; // Convert to deg/s

     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
      kalmanX.setAngle(roll);
  
      kalAngleX = roll;
  
    } else
      kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  
    if (abs(kalAngleX) > 90)
      gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
 //KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    KÁLMÁN    


  if(oldal==true){
    k=1;
  }
  else{
    k=-1;}

//----------------------------------------------------------------------------------------------------------------------------
//INDUL

for(int i=1; i<11&& elo!=0&&moz1.read()<10&&OV==false&&szak2==false; i++){

     
  moz11.write(90+i);
  moz2.write(90+i);
 moz1.write(0+i);
 moz10.write(45-i);
   if(i==10){ 
    szak1=true;}
 delay (elo);
  }
 
 
 
 for(int i=1; i<26&& elo!=0&&moz1.read()<35&& szak1==true&&OV==false; i++){


 moz1.write(10+i);
 moz10.write(35-i);
  if(i==10){ 
    szak2=true;}
  delay (elo);
 }


for(int i=1; i<11&& elo!=0&&moz1.read()<45&& szak2==true&&OV==false; i++){

     
  moz11.write(100-i);
  moz2.write(100-i);
 moz1.write(35+i);
 moz10.write(10-i);
   if(i==10){ 
    szak3=true;}
  }

  
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ 
//----------------------------------------------------------------------------------------------------------------------------
//FUT

for(;elo!=0&& szak3==true&&OV==false;){
int j;
if(irany= true){
  j=1;}
  else{
    j=0;}

for(int i=0;i<=10&&i>=-10&&35<=moz1.read()&&moz1.read()<=45&&OV==false; i=i++){

//JE1
  moz1.write(45-i);
 moz2.write(moz2.read()+(1-j));
  //BH
 moz10.write(0+i);
 moz11.write(moz11.read()+(1-j));
//BE
 moz4.write(180-i);
 moz5.write(moz5.read()-j);
 //JH
 moz7.write(135+i);
 moz8.write(moz8.read()-j);}

//************************************************************************************************************************************************************************************

if(k!=0&&old!=0){
folfor(moz4.read()==170&&moz1.read()!=(35+k*old),(0+k),0,0);
levle (i);
folle(moz4.read()==170&&moz1.read()!=35,1);
levle((10-i));
}

//************************************************************************************************************************************************************************************
//**************************************************************************************************************************

 for(int i=1;i<=25&&i>=-25&&10<=moz1.read()&&moz1.read()<=35&&OV==false;  i=i++){
//JE2
  moz1.write(35-i);
  //BH
 moz10.write(10+i);
//BE
 moz4.write(170-i);
 //JH
 moz7.write(145+i);}
//************************************************************************************************************************************************************************************

if(k!=0&&old!=0){
folfor(moz4.read()==145&&moz1.read()!=(10+k*old),(25+k),25,0);
levle(i);
folle(moz4.read()==145&&moz1.read()!=10,1);
levle((10-i));
}

//************************************************************************************************************************************************************************************
//**************************************************************************************************************************

 for(int i=1;i<=10&&i>=-10&&0<=moz1.read()&&moz1.read()<=10&&OV==false; i=i++){
//JE3
  moz1.write(10-i);
 moz2.write(moz2.read()-(1-j));
  //BH
 moz10.write(35+i);
 moz11.write(moz11.read()-(1-j));
//BE
 moz4.write(145-i);
 moz5.write(moz5.read()+j);
 //JH
 moz7.write(170+i);
 moz8.write(moz8.read()+j);}
//**************************************************************************************************************************

 for(int i=1;i<=10&&i>=-10&&10>=moz1.read()&&moz1.read()>=0&&OV==false;  i=i++){
//JE4
  moz1.write(0+i);
 moz2.write(moz2.read()+j);
  //BH
 moz10.write(45-i);
 moz11.write(moz11.read()+j);
//BE
 moz4.write(135+i);
 moz5.write(moz5.read()-(1-j));
 //JH
 moz7.write(180-i);
 moz8.write(moz8.read()-(1-j));}
//************************************************************************************************************************************************************************************
if(k!=0&&old!=0){
folfor(moz1.read()==10&&moz4.read()!=(145+k*old),25,(25+k),10);
levle(i);
folle(moz1.read()==10&&moz4.read()!=145,0);
levle((10-i));
}
//**************************************************************************************************************************

 for(int i=1;i<=25&&i>=-25&&35>=moz1.read()&&moz1.read()>=10&&OV==false;  i=i+j){
//JE5
  moz1.write(10+i);
  //BH
 moz10.write(35-i);
//BE
 moz4.write(145+i);
 //JH
 moz7.write(170-i);}
//************************************************************************************************************************************************************************************

if(k!=0&&old!=0){
folfor(moz1.read()==35&&moz4.read()!=(170+k*old),0,(0+k),10);
levle(i);
folle(moz1.read()==35&&moz4.read()!=170,0);
levle((10-i));
}

//************************************************************************************************************************************************************************************
//**************************************************************************************************************************

 for(int i=1;i<=10&&i>=-10&&45>=moz1.read()&&moz1.read()>=35&&OV==false;  i=i++){
//JE6
  moz1.write(35+i);
 moz2.write(moz2.read()-j);
  //BH
 moz10.write(10-i);
 moz11.write(moz11.read()-j);
//BE
 moz4.write(170+i);
 moz5.write(moz5.read()+(1-j));
 //JH
 moz7.write(145-i);
 moz8.write(moz8.read()+(1-j));}

}
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//MEG ÁLL
if(elo==0&& szak1==true){

//BLOKOLNI AZ INDITÁST MÍG VISSZA ÁLL VAGY INDITANI
//********************************************************************************************************************************
   //egyből bal láb letesz ha lenn van ->másik emel(ha nincs a helyén) ->helyére ->letesz->másik emel(ha nincs a helyén)->helyére ->letesz
    for(;moz5.read()!=90;){      //letesz
//BE
  moz5.write(moz5.read()+1);
 //JH
  moz8.write(moz8.read()+1);}  
    
      for(;moz1.read()!=0&&moz2.read()!=100;){ //emel
//JE
  moz2.write(moz2.read()+1);
  //BH
 moz11.write(moz11.read()+1);}

      
      
      for(;moz1.read()!=0;){    // a helyére
//JE
  moz1.write(moz1.read()-1);
  //BH
 moz10.write(moz10.read()+1);}


levle(10-i);    //lábcsere

 for(;moz5.read()==80&&moz4.read()!=180;){      //helyére
 //BE
 moz4.write(moz4.read()+1);
 //JH
 moz7.write(moz7.read()-1); }
 for(int i=0;i<=10&&moz4.read()==180&&moz5.read()!=90;i=i++){    //letesz
 //BE
 moz5.write(80+1);
 //JH
 moz8.write(80+1); }
 
   if(moz4.read()==180&&moz1.read()==0){ 
    szak1=false;
    szak2=false;
    szak3=false;
    szakSTOP=true;}
 }

//-------------------------------------------------------------------------------------------------


//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//ÁLLÓ FORDULÁS
for(;elo==0&&old!=0&&OV==false&& szak1==false;szakSTOP=false){
kerel();
 
 //bool oldal;     //előjel
//byte  old;       //érték
 for(;elo==0&&old!=0&&OV==false;){
 for(int i=0;i<=old;i++){
    moz1.write(moz1.read()+oldal);   //
//BH
 moz4.write(moz4.read()+oldal);   //
 //BE
 moz7.write(moz7.read()+oldal);   //
 //JH
 moz10.write(moz10.read()+oldal);  } 
 
 
 for(int i=1;i<=10;i++){
 moz2.write(90+i);
 moz11.write(90+i);}
 
 for(int i=0;i<=old;i++){    
 moz1.write(moz1.read()-oldal);  
 moz10.write(moz10.read()-oldal); }

levle((10-i));

 for(int i=0;i<=old;i++){ 
  moz4.write(moz4.read()-oldal);   //
 moz7.write(moz7.read()-oldal);   }
  
  for(int i=1;i<=10;i++){
     moz5.write(80+i);                       
 moz8.write(80+i);}
 }
 
} 


  //REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET


for(;OV==true;){
kerel();

ESC1.write(emel);
ESC2.write(emel);
   //REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET                  REPALAP HELYZET

for(int i=0; i<=90&&moz2.read()!=0&&moz8.read()!=180;i++){
//JE
 moz2.write(90-i);   //MOZGAT
//BH
 moz5.write(90+i);   //MOZGAT
 //BE
 moz8.write(90+i);   //MOZGAT
 //JH
 moz11.write(90-i);   //MOZGAT
 }
 if(OV==true){
 szakREP=true;
 }
  
   

  
  // PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  PID  
  
for(  szakSTOP=false;szakREP==true;){  
    Setpoint1 = tomb[0];
    Setpoint2 =tomb[1];
    Input1 =  kalAngleX;
    Input2 =  kalAngleY;

  myPID1.Compute();
  myPID2.Compute();
  int  gelo = map(Output2, 0, 90, 0, 45);
  int  ghat = map(Output2, 0, -90, 0, 45);
  int  gjob = map(Output1, 0, 90, 0, 45);
  int  gbal = map(Output1, 0, -90, 0, 45);
  gelo =constrain(gelo, 0, 45);
  ghat =constrain(ghat, 0, 45);
  gjob =constrain(gjob, 0, 45);   
  gbal =constrain(gbal, 0, 45);    
 //az értékek össze adódnak
     //JE
     moz2.write(gelo+gjob);   //
     mozf.write(90);  
     //JH
     moz8.write(ghat+gjob);   //
     //BH
     moz11.write(ghat+gbal);   //
     //BE
     moz5.write(gelo+gbal);   //
     mozl.write(90);   
}}
 
 if (OV==false){
 
  szakREP=false;
 
 }
}

void levle(int is){
// LEVEGŐ LÁB LETESZ // FÖLD LÁB EMEL
for( i=1;i<=10&&OV==false;i++){
//JE
 moz2.write(90+is);
  //BH
 moz11.write(90+is);
//BE
 moz5.write(80+is);
 //JH                         
 moz8.write(80+is);
}
return;
}
void folfor(boolean a, int ho, int ho2,int plus ){
//FÖLD LÁB FORDUL
for(;a&&OV==false;){
//JEf1.1
  moz1.write(35-ho);
 moz2.write(90+plus);
  //BH
 moz10.write(35-(25-ho));
 moz11.write(90+plus);
//BE
 moz4.write(170-ho2);
 moz5.write(80+plus);
 //JH                         
 moz7.write(170-(25-ho2));
 moz8.write(80+plus);
}
return;
}

void folle (boolean x,byte z){
for(;x&&OV==false;){
  moz1.write( moz1.read()-(k*z));
moz10.write(moz10.read()-(k*z));
moz4.write(moz4.read()-(k*(1-z)));
moz7.write(moz7.read()-(k*(1-z)));

}
return;
}

void kerel (){
for(int i=1; i<=10&&moz1.read()<=10&&moz1.read()>=0; i++){
  moz2.write(90+i);
 moz1.write(0+i);
 delay (elo);  }
 for(int i=1; i<=25&&moz1.read()<=35&&moz1.read()>=10; i++){   
 moz1.write(10+i);
  delay (elo); }
for(int i=1; i<=10&&moz1.read()<=45&&moz1.read()>=35; i++){
    moz2.write(100-i);
 moz1.write(35+i);  }

for(int i=1; i<=10&&180>=moz4.read()&&moz4.read()>=170&&moz5.read()!=80; i++){
//BE
 moz4.write(180-i);
 moz5.write(90-i);}
 for(int i=1; i<=25&&170>=moz4.read()&&moz4.read()>=145&&moz5.read()==80;  i++){
//BE
 moz4.write(170-i);}
 for(int i=1; i<=10&&145>=moz4.read()&&moz4.read()>=135&&moz5.read()!=90; i++){
//BE
 moz4.write(145-i);
 moz5.write(80+i);}

return;
}
Ez pedig az I2C hez egy másik fülön.

Kód: Egész kijelölése

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication

uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
  return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data, length);
  uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
  if (rcode) {
    Serial.print(F("i2cWrite failed: "));
    Serial.println(rcode);
  }
  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
  uint32_t timeOutTimer;
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
  if (rcode) {
    Serial.print(F("i2cRead failed: "));
    Serial.println(rcode);
    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
  }
  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
  for (uint8_t i = 0; i < nbytes; i++) {
    if (Wire.available())
      data[i] = Wire.read();
    else {
      timeOutTimer = micros();
      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
      if (Wire.available())
        data[i] = Wire.read();
      else {
        Serial.println(F("i2cRead timeout"));
        return 5; // This error value is not already taken by endTransmission
      }
    }
  }
  return 0; // Success
}
Vissza a tetejére
IzLuCSi
DrótVégénSzéndarab
Hozzászólások: 47
Csatlakozott: 2017. szeptember 26. kedd, 14:34

Re: Arduino nano memória hiba

Hozzászólás Szerző: IzLuCSi »

A szervokat egy külső aksiról táplálom meg aminek a feszültségét két ESC csökkenti le 5V -ra.
Vissza a tetejére
Avatar
kapu48
Elektronbűvölő
Hozzászólások: 3375
Csatlakozott: 2008. augusztus 29. péntek, 6:00

Re: Arduino nano memória hiba

Hozzászólás Szerző: kapu48 »

A for ciklust én még nem láttam így feltételezni!
Nem ismered a While-t? https://www.arduino.cc/reference/en/lan ... ure/while/
Vissza a tetejére
vargham
Chipgyilok
Hozzászólások: 308
Csatlakozott: 2014. január 8. szerda, 8:32

Re: Arduino nano memória hiba

Hozzászólás Szerző: vargham »

Nekem ez a kód közel olvashatatlan. Egy tömbnek hogy lehet tomb nevet adni? Legyen inkább valami beszédes, ami utal a tömb tartalmára. A többi báltozónév is lehetne hosszabb és beszédesebb, javítaná az olvashatóságot.
Vissza a tetejére
Válasz küldése

Vissza: “Arduino / C-nyelvű programozás (AVR-Duino, EthDuino, Arduino, Diecimila)”