Zuckungen Servo Arduino

#include <TFT.h>

#include <SPI.h>

#include <Servo.h> 

#define echo 4

#define trig 10

#define CS A3

#define DC A2

#define RES 7

#define MOSI 6

#define SCLK 2

Adafruit_ST7735 screen = Adafruit_ST7735(CS, DC, MOSI, SCLK, RES);

Servo myservo;  // create servo object to control a servo 

                // a maximum of eight servo objects can be created 

int dis;

char disC[5];

String disS;

int pos = 0;    // variable to store the servo position 

int freeRam () {

  extern int __heap_start, *__brkval; 

  int v; 

  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 

}

void abstand(){

  digitalWrite(trig, LOW);

  delayMicroseconds(2);

  digitalWrite(trig, HIGH);

  delayMicroseconds(10);

  digitalWrite(trig, LOW);  

//dis = pulseIn(echo, HIGH);

  disS = String(dis/58);

  disS.toCharArray(disC, 5);

}

void lcdtext(char *distance, char *text){

  screen.stroke(255, 255, 255);

  screen.setTextSize(2);

  screen.text(text, 0, 0);

  screen.setTextSize(5);

  screen.text(distance, 0, 25);

  screen.stroke(0, 0, 0);

  screen.text(distance, 0, 25);

}

void setup() { 

  screen.initR(INITR_REDTAB);

  screen.setRotation(1);

  screen.background(0, 0, 0);

  screen.setTextSize(5);

  pinMode(echo, INPUT);

  pinMode(trig, OUTPUT);

  myservo.attach(5);  // attaches the servo on pin 9 to the servo object 

} 

void loop() {

  for(pos = 90; pos <180; pos += 1)  // goes from 0 degrees to 180 degrees 

  {                                  // in steps of 1 degree 

    myservo.write(pos);              // tell servo to go to position in variable 'pos' 

    abstand();

 //lcdtext(disC, "Abstand(cm):");

    delay(75);                       // waits 15ms for the servo to reach the position 

  } 

  for(pos = 180; pos>=90; pos-=1)     // goes from 180 degrees to 0 degrees 

  {                                

    myservo.write(pos);                // tell servo to go to position in variable 'pos' 

    abstand();

  //lcdtext(disC, "Abstand(cm)");

    delay(75);                       // waits 15ms for the servo to reach the position 

  } 

}