Witam wszystkich ,
Otoz naszym projektem jest zrobienie platformy ktora rekompensuje roznice w wysokosci. Nie jest to akurat tutaj zbyt wazne, chodzi bardziej o kod.
Otoz ultrasensor jest na tyle nie dokladny ze mierzone wartosci w cm skacza kiedy sensor stoi w miejscu miedzy nawet czasami 1 cm a czasami 3 mm. W mierzeniach nie ma zadnej spojnosci jest to na tyle losowe ze nie mam pojecia jak usunac te nie rowne mierzenia. Filtra nie zbudujemy bo nie jest analogowy sensor. Do tej pory udalo mi sie tyle napisac , moze macie pomysl jakis lepszy.
Hardware wyglada tak ze sensor stoi w miejscu a przed nim rusza sie przeszkoda.
#include <Stepper.h>
#define TRIGGER_PIN 4
#define ECHO_PIN 5
#define MAX_DISTANCE 200
double aantalstappen; //ilosc krokow uzywamy stepper motor.
double startPositie; // pozycja startowa
double newPositie; // nowa pozycja
double verschilPositie; // roznica
double verschilError; //roznica roznicy
double verschilErrornow,verschilErrorvorige; //roznica roznicy teraz, roznica roznicy poprzednia
int standingstil = 0; // stoi w miejscu
int moving = 0;
void setup() {
pinMode(TRIGGER_PIN,OUTPUT);
pinMode(ECHO_PIN,INPUT);
Serial.begin(9600);
verschilErrorvorige = 0;
double uS = avarage();
startPositie = uS;
}
//LOOP
void loop() {
newPositie = avarage(); //zo bereken je het afstand in CM avarage functie oproepen.
verschilPositie = startPositie - newPositie;
verschilErrornow = verschilPositie;
verschilError = verschilErrornow + verschilErrorvorige;
/*
Serial.println("verschilPositie");
Serial.println(verschilPositie);
Serial.println("verschilError");
Serial.println(verschilError);
*/
if(verschilPositie > 0.02){
if((verschilError == verschilPositie) || (verschilError == (verschilPositie + 0.01)) || (verschilError == (verschilPositie - 0.01))){
Serial.println("STAAT STILL");
// standingstil++;
// if(standingstil >= 4){
// verschilError = verschilPositie;
// standingstil =0;
// moving--;
}
else{
//moving++;
//if(moving > 4){
Serial.println("BEWEEGT ");
aantalstappen = (verschilPositie * 25 );
Serial.println(aantalstappen);
// standingstil--;
// }
}
}
delay(20);
startPositie = newPositie;
verschilErrorvorige = verschilPositie;
}
//FUNCITES ONDERAAN
double avarage(){
double duration, distance1,total;
int test = 0;
digitalWrite(TRIGGER_PIN,HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN,LOW);
duration = pulseIn(ECHO_PIN,HIGH);
distance1 = (duration/2)/ 29.1;
if(distance1 > 3 && distance1 < 125){
for(int i =0;i <= 9;i++){
total += distance1;
test++;
//Serial.println(distance1);
}
}
return (total/test);
total = 0;
}