
#include <Servo.h>
char serialData;
Servo myservo;
int zerop=90; 
int servpos=zerop;
int maxservpos=zerop;
int minservpos=zerop;
int i=zerop;

void setup() {
  
  myservo.attach(9);
  Serial.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
  
  
  
}

void loop() {
  while (Serial.available()>0){
    serialData=Serial.read();
    delay(2);
    Serial.print(serialData);

  
  // go to zero   
    if(serialData == 'z'){ //if python sent 'z'
      if (servpos<zerop){ //Case - current pos is smaller than zero
        for (i=servpos; i<zerop; i+=1){ //go up to zero with 30 milisec delay between each move
          myservo.write(i);
          delay(30);
        }
        servpos=i;//update new pos
      }
      if (servpos>zerop){ //Case - current pos is larger than zero
        for (i=servpos; i>zerop; i-=1){ //go down to zero with 30 milisec delay between each move
          myservo.write(i);
          delay(30);
        }
        servpos=i;//update new pos
      }
    }
      
  // plus 1 degree  
    if(serialData=='u'){//if python sent 'u'
      if (servpos<180){ //Case - pos didnt reach maximum turn of 180 degree
        servpos++; //pos 1Up
        myservo.write(servpos);
        delay(15);
        }
      else { //Case - pos reach maximum
        myservo.write(180); //dont go over more than 180
        servpos=180;
      }
    }
  



  // plus 5 degree    
    if(serialData=='U'){ //if python sent 'U'
      if (servpos<=175){ //Case - pos didnt reach step before maximum turn of 175 degree
        for (i=servpos; i<servpos+5; i+=1){ //go 5 degrees up, with 30 milisec between each degree
          myservo.write(i);
          delay(30);
         }
         servpos=i; //update new pos
      }
      else {  //Case - jumping 5 will bring you over 180
        myservo.write(180); 
        servpos=180; //update new pos
      }
      
    }
    
  // minus 1 degree  
    if(serialData=='d'){ //if python sent 'd'
        if (servpos>0){ //Case - pos didnt reach minimum turn of 0 degree
          servpos--; //pos 1Down
          myservo.write(servpos);
          delay(15);
        }
        else {  //Case - pos reach minimum
          servpos=0; 
          myservo.write(0);//dont go lower than 0
        }
    }
  
  // minus 5 degree
      if(serialData=='D'){//if python sent 'D'
        if (servpos>=5){ //Case - pos didnt reach step before minimum turn of 5 degree
          for (i=servpos; i>servpos-5; i-=1){ //go 5 degrees down, with 30 milisec between each degree
            myservo.write(i);
            delay(30);
          }
          servpos=i;//update new pos
        }
        else { //Case - jumping down 5 will bring you under 0
          servpos=0;//update new pos
          myservo.write(0);
        }
    }
  
    
    
  //set min   
    if(serialData=='n'){ //if python sent 'n'
      minservpos=servpos; //the value of current pos saved in minservpos
      }
      
  //set max  
    if(serialData=='x'){ //if python sent 'x'
      maxservpos=servpos; //the value of current pos saved in maxservpos
      }
    
  //go to min  
    if(serialData=='g'){ //if python sent 'g'
      if (servpos<minservpos){ //Case - pos is under the value saved in minservpos
        for (i=servpos; i<minservpos; i+=1){ //go up to minservpos with 30 milisec between each degree
          myservo.write(i);
          delay(30);
        }
        servpos=i;
      }
      if (servpos>minservpos){ //Case - pos is over the value saved in minservpos
        for (i=servpos; i>minservpos; i-=1){ //go down to minservpos with 30 milisec between each degree
          myservo.write(i);
          delay(30);
        }
        servpos=i; //update new pos
      }
    }
    
  //go to max   
    if(serialData=='G'){ //if python sent 'G'
      if (servpos<maxservpos){ //Case - pos is under the value saved in maxservpos
        for (i=servpos; i<maxservpos; i+=1){ //go up to maxservpos with 30 milisec between each degree
          myservo.write(i);
          delay(30);
        }
        servpos=i;
      }
      if (servpos>maxservpos){ //Case - pos is over the value saved in maxservpos
        for (i=servpos; i>maxservpos; i-=1){ //go down to maxservpos with 30 milisec between each degree
          myservo.write(i);
          delay(30);
        }
        servpos=i;
      }
    }
    
  // reset all parameters  
    if(serialData=='r'){ //if python sent 'r'
      servpos=zerop; //go to zero
      maxservpos=zerop; 
      minservpos=zerop;
      myservo.write(zerop);
    }
  
  // cyclic 1 hz between max pos and min pos
    if(serialData=='1'){ //if python sent '1'
      //1st - go to min
      if (servpos<minservpos){ //Case - pos is under the value saved in minservpos
        for (i=servpos; i<minservpos; i+=1){
          myservo.write(i);
          delay(30);
        }
        servpos=i;
      }
      if (servpos>minservpos){ //Case - pos is over the value saved in minservpos
        for (i=servpos; i>minservpos; i-=1){
          myservo.write(i);
          delay(30);
        }
        servpos=i;
      }
      
      //now Cycle
      delay(100);
      bool f=false; //stopping condition
      while(f==false){
        //Going up with constant delay- sawtooth profile
        for (servpos=minservpos; servpos <=maxservpos; servpos +=1){
          myservo.write(servpos);
          if ((maxservpos-minservpos)>150){ //if the distance between max and min is too great, the servo used wont be able to make it in time, so the rate is not readly 1htz.
            delay(3.333); //the fastest delaypossible between each degree
          }
          else{
            delay(500/(maxservpos-minservpos)); //the distance between max and min is ok for the servo to handle. "going up" will last 500 milisec
          }
        }
        delay(20); //hold before changing direction
        //Going down with constant delay- sawtooth profile
        for (servpos=maxservpos; servpos >=minservpos; servpos -=1){
          myservo.write(servpos);
          if ((maxservpos-minservpos)>150){ //if the distance between max and min is too great, the servo used wont be able to make it in time, so the rate is not readly 1htz.
            delay(3.333);//the fastest delaypossible between each degree
          } 
          else{
            delay(500/(maxservpos-minservpos));//the distance between max and min is ok for the servo to handle. "going down" will last 500 milisec
          }
        }
        delay(20);//hold before changing direction
        if(Serial.available()>0){
          serialData=Serial.read();
          if (serialData=='s'){ //if python sent 's' while looping
            f=true;
          }
        }
      }
    } 
  
  // cyclic 0.5 hz
    if(serialData=='2'){ //if python sent '2'
      //1st - go to min
      if (servpos<minservpos){  //Case - pos is under the value saved in minservpos
        for (i=servpos; i<minservpos; i+=1){
          myservo.write(i);
          delay(30);
        }
        servpos=i;
      }
      if (servpos>=minservpos){//Case - pos is over the value saved in minservpos
        for (i=servpos; i>minservpos; i-=1){
          myservo.write(i);
          delay(30);
        }
        servpos=i;
      }
      
      //now Cycle
      delay(100);
      bool f=false; //stopping condition
      while(f==false){
        //Going up with constant delay- sawtooth profile
        for (servpos=minservpos; servpos <=maxservpos; servpos +=1){
          myservo.write(servpos);
          delay(1000/(maxservpos-minservpos)); //delay so the "going up" will take 1 sec
        }
        delay(20);//hold before changing direction
        //Going down with constant delay- sawtooth profile
        for (servpos=maxservpos; servpos >=minservpos; servpos -=1){
          myservo.write(servpos);
          delay(1000/(maxservpos-minservpos));//delay so the "going down" will take 1 sec
        }
        delay(20);//hold before changing direction
        if(Serial.available()>0){
          serialData=Serial.read();
          if (serialData=='s'){//if python sent 's' while looping
            f=true;
          }
        }
      }
  } 

  }
}
