Hi I' had been working to correct it for couple hours and becouse of lack of skills I gave up.
#include <Servo.h>
#include <AFMotor.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
AF_DCMotor motor(4);
AF_DCMotor motor1(3);
//-------------------------------------------------
//dotyczy serva z czujnikiem do pomiaru odleg?o?ci
//-------------------------------------------------
Servo myservo;
//------------------------------------------------
//dotyczy czujnika pomiaru odleg?o?ci
//------------------------------------------------
const int trigPin = 42;
const int echoPin = 43;
//----------------------------------------------
int tab [168][2];
float kalibracjaAX, kalibracjaAY, kalibracjaAZ, kalibracjaGX, kalibracjaGY, kalibracjaGZ;
MPU6050 accelgyro;
void setup()
{
Serial.begin(9600);
myservo.attach(10);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
motor.setSpeed(200);
motor.run(RELEASE);
motor1.setSpeed(200);
motor1.run(RELEASE);
Wire.begin();
accelgyro.initialize();
delay(9000);//czekaj 9 sekund na poprawny pomiar akcelerometru
// join I2C bus (I2Cdev library doesn't do this automatically)
int czas_calkowity = millis();
}
void loop()
{
procedura_pomiar_wys?anie_detekcja();
procedura_akcelerometr_ruch(); // CZY
// TO
// DZIEJE
motor.run(FORWARD); // SIE
motor1.run(FORWARD); // JEDNOCZE?NIE
motor.setSpeed(255); // ?
motor1.setSpeed(255); //
delay(2000); //
motor.run(RELEASE); //
motor1.run(RELEASE); //
procedura_ponowny_pomiar();
}
//pierwsza procedura
bool procedura_pomiar_wys?anie_detekcja()
{
int pos;
//------------------------------------------------
//dotyczy serva z czujnikiem do pomiaru odleg?o?ci
//------------------------------------------------
//obrót w lewo
for(pos = 10; pos < 170; pos ++)
{
pomiar_odleglosci_serwo_1(pos);
}
wyznaczanie_min_odl();
}
//-----------------------------------------------
//druga procedura
bool procedura_akcelerometr_ruch()
{
kalibracja();
akcelerometr_zyro();
}
// trzecia procedura
bool procedura_ponowny_pomiar()
{
int pos;
//------------------------------------------------
//dotyczy serva z czujnikiem do pomiaru odleg?o?ci
//------------------------------------------------
//obrót w lewo
for(pos = 10; pos < 170; pos ++)
{
pomiar_odleglosci_serwo_1(pos);
}
wyznaczanie_min_odl();
}
bool kalibracja()
{
float cal_ax = 0;
float cal_ay = 0;
float cal_az = 0;
float cal_gx = 0;
float cal_gy = 0;
float cal_gz = 0;
for (int i = 0; i <= 1000; i++) //
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
cal_ax += ax;
cal_ay += ay;
cal_az += az;
cal_gx += gx;
cal_gy += gy;
cal_gz += gz;
delay(50);
cal_ax /= 1000;
cal_ay /= 1000;
cal_az /= 1000;
cal_gx /= 1000;
cal_gy /= 1000;
cal_gz /= 1000;
kalibracjaAX = cal_ax;
kalibracjaAY = cal_ax;
kalibracjaAZ = cal_ax;
kalibracjaGX = cal_ax;
kalibracjaGY = cal_ax;
kalibracjaGZ = cal_ax;
Serial.println("kalibracja zako?czona sukcesem");
}
//funkcja obrotu serwa o k?t pos, pomiaru odleg?o?ci i wys?ania danych na port szeregowy
bool pomiar_odleglosci_serwo_1(int pos)
{
int l_p=7;
int odl_s=0;
int i_p=0;
int T_odl[l_p];
//zmiana k?ta
myservo.write(pos);
delay(100);
//pomiar odleg?o?ci
//z rozdzielczo?ci? do 0.1 cm
for(int i=0; i<l_p; i++)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
int duration = pulseIn(echoPin, HIGH);
T_odl[i]= microsecondsToCentimeters(duration);
delay(40);
}
//wyznaczanie warto?ci ?rodkowej
for(int j=0; j<(l_p/2); j++)
{
odl_s=-1;
for(int i=0; i<l_p; i++)
{
if(T_odl[i]>odl_s)
{
odl_s=T_odl[i];
i_p=i;
}
}
T_odl[i_p]=-1;
}
tab [pos][1]=pos;
tab [pos][0]=odl_s;
/* format depeszy: $,czas trwania programu, odleg?o?? z czujnika, wychylenie serwa, min odl z 1 sektora, namiar na t? odleg?o?? z pierwszego sektora, min odl z 2 sektora, namiar z 2 sektora,
przy?pieszenie po osi x, przy?pieszenie po osi y, warto?? ?yro po osi z, droga po osi x, droga po y)
*/
Serial.print("$,"); Serial.print(czas_ca?kowity);Serial.print(","); Serial.print(odl_s); Serial.print(","); Serial.print(pos); Serial.print(", 0, 0, 0, 0, 0, 0, 0, 0, 0")
// S? dwa formaty bo nie wiem czy ten pierwszy jest poprawny, ale wydaje mi sie ?e nie.
}
//funkcja obliczania odleg?o?ci na podstawie pomiaru czasu
float microsecondsToCentimeters(int microseconds)
{
return microseconds / 58.0 ;
}
// funkcja odszukiwanie war minimalenej w pomiarze
bool wyznaczanie_min_odl()
{
float minimum = 700;
float minimum1 = 700;
int z;
int z1;
int i;
for ( i =10; i <90; i++) //SEKTOR PIERWSZY
{
if (tab[i][0]< minimum)
{
minimum = tab[i][0];
z=i;
}
for ( i= 89; i <170; i++) //SEKTOR DRUGI
(
if (tab[i][0] < minimum1)
(minimum1 = tab[i][0];
z1=i;
)
)
}
Serial.print("$,"); Serial.print(czas_ca?kowity); Serial.print("0, 0,"); Serial.print(minimum); Serial.print(","); Serial.print(z); Serial.print(","); Serial.print(z1); Serial.Print(","); Serial.print(" 0, 0, 0, 0, 0");
}
bool akcelerometr_zyro()
{
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
//////////////zmienne do akcelerometru i zyroskopu/////////////////
float ax1, ay1, az1;
float predkoscx, predkoscy;
float drogax, drogay;
unsigned long czas_nowy, czas_stary;
float dt;// delta czasu
float dgx, dgy, dgz;// zmiana k?towa w czasie dt
float gx1, gy1, gz1;// warto?ci k?towe
/////////////////////////////////////////////////////////////////
#define OUTPUT_READABLE_ACCELGYRO
czas_nowy = millis();
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
////////////////////////////////////// akcelerometr///////////////////////////////////////////////
dt = (czas_nowy - czas_stary) * 0.001;
ax1 = (ax -= kalibracjaAX) /= 16384 * 9.81;
ay1 = (ay -= kalibracjaAY) /= 16384 * 9.81;
az1 = (az -= kalibracjaAZ) /= 16384 * 9.81;
float sx = ax1 * dt * dt * 0.5 + predkoscx * dt;
float sy = ay1 * dt * dt * 0.5 + predkoscy * dt;
predkoscx = ax1 * dt;
predkoscy = ay1 * dt;
drogax += sx;// warto?ci zmian drogi po czasie do podania na port szeregowy OSTATECZNE
drogay += sy;
/////////////////////////////////?yro////////////////////////////////////////////////////////////
dgx = (gx -= kalibracjaGX) /= 131 * dt;// 131 bo gx to surowa warto?? dla zmiany o 1 stopie? w czasie jednej sekundy i przez podzielenie jej przez 131 odtrzymujemy warto?? zmiany w stopniach w jednostce czasu
dgy = (gy -= kalibracjaGY) /= 131 * dt;
dgz = (gz -= kalibracjaGZ) /= 131 * dt;
gx1 += dgx;//warto?ci zmian k?tów do podania na port szeregowy OSTATECZNE
gy1 += dgy;
gz1 += dgz;
/////////////////////////////////////////////////////////////////////////////////////////////////
Serial.print("$,"); Serial.print(czas_ca?kowity); Serial.print(", 0, 0, 0, 0, 0, 0,"); Serial.print(ax1); Serial.print(","); Serial.print(ay1); Serial.print(","); Serial.print(gz1); Serial.Print(",");
Serial.print(drogax); Serial.print(","); Serial.print(drogay);
czas_stary = czas_nowy;
}