Problem with Rotary Encoder and Servo

Hi! I’m having problems with the direction of the rotation for the servo. We have a pully system with a modified 360 servo and a rotary encoder attached. The code is supposed to be moving up and down, starting with a clockwise rotation. I can’t find the error anywhere, please help!

#include<Servo.h>
//these pins can not be changed 2/3 are special pins
int encoderPin1 = 2;
int encoderPin2 = 3;

volatile int lastEncoded = 0;
volatile long encoderValue = 0;
volatile long int thiscall = 10;
volatile long int lastcall = 0;
volatile float timebetween = 0;
int lastMSB = 0;
int lastLSB = 0;
float angularspeed = 0;
Servo myservo;
void setup() {
  Serial.begin (9600);
  myservo.attach(9);

  pinMode(encoderPin1, INPUT);
  pinMode(encoderPin2, INPUT);
  
  digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin2, HIGH); //turn pullup resistor on

  //call updateEncoder() when any high/low changed seen
  //on interrupt 0 (pin 2), or interrupt 1 (pin 3)
  attachInterrupt(0, updateEncoder, CHANGE);
  attachInterrupt(1, updateEncoder, CHANGE);

}

void loop() {

  
  //moveroundsin(true,0,10);//wait 10 seconds
  moveroundsin(true,40,129);
  moveroundsin(false,10,17);
  moveroundsin(true,10,45);
  moveroundsin(false,8,9);
  moveroundsin(false,3,6);
  //3:26
  
  moveroundsin(true,6,11);
  moveroundsin(false,6,6);
  moveroundsin(true,6,11);
  moveroundsin(false,6,6);
  moveroundsin(true,2,4);

  
  moveroundsin(false,8,8);
  //4:20
  moveroundsin(true,10,40);


  //5:00 move down slowly

  moveroundsin(false,40,120);
  




}



/*this function will run for x seconds and try to get as close to y rounds during that time*/
void moveroundsin(bool up, float rounds, float seconds) {
  //up=!up;//uncomment this line if up and down are swapped.
  int angle=90;
  float currentdelta = 0;
  float lastdelta = 0;
  long int start = millis(); //relative starting time
  signed int startrounds = encoderValue; //position of encoder when this is called

  //define start speeds
  if (up) {
    angle = 70;
  } else {
    angle = 100;
  }
  //special case when we want no rotation
  if(rounds==0.0){
    angle=88;//start at the approximate still value so we will have less jitter
  }
  
  //loop through the feedback loop until the time has past
  while (1000.0 * seconds - (millis() - start) > 0) {
    if (up) {
      //calculate current absolute difference between wanted and actual rotational value
      currentdelta = (float)abs(-(encoderValue - startrounds) - (float)rounds * 40 * (millis() - start) / (seconds * 1000));

      //if we are behind and not closing in on the optimal value, speed up
      if ((float)-(encoderValue - startrounds) < (float)rounds * 40 * (millis() - start) / (seconds * 1000) && lastdelta > currentdelta) {
        angle--;
      }
      //if we are ahead and not closing in on the optimal value, slow down
      if ((float)-(encoderValue - startrounds) > (float)rounds * 40 * (millis() - start) / (seconds * 1000) && lastdelta < currentdelta) {
        angle++;
      }
      lastdelta = currentdelta;
    }
    else {
      //calculate current absolute difference between wanted and actual rotational value
      currentdelta = abs((encoderValue - startrounds) - rounds * 40 * (millis() - start) / (seconds * 1000));

      //if we are behind and not closing in on the optimal value, speed up
      if ((float)(encoderValue - startrounds) < (float)rounds * 40 * (millis() - start) / (seconds * 1000) && lastdelta > currentdelta) {
        angle++;
      }
      //if we are ahead and not closing in on the optimal value, slow down
      if ((float)(encoderValue - startrounds) > (float)rounds * 40 * (millis() - start) / (seconds * 1000) && lastdelta < currentdelta) {
        angle--;
      }
      lastdelta = currentdelta;
    }
    // mind the limits of the servo
    if (angle < 1)angle = 0;
    if (angle > 179)angle = 180;
    //write the new angle to 
    myservo.write(angle);

    //debug info
    Serial.print("current deviation: ");
    Serial.print(currentdelta);
    Serial.print("\t close in speed:");
    Serial.print(currentdelta-lastdelta);
    Serial.print("\t rounds: ");
    Serial.print((float)(-encoderValue + startrounds));
    Serial.print("\t should be: ");
    Serial.print((float)rounds * 40 * (millis() - start) / (seconds * 1000));
    Serial.print("\t Angle:");
    Serial.println(angle);
   

  }

}

void updateEncoder() {
  //copied this function from http://bildr.org/2012/08/rotary-encoder-arduino/http://bildr.org/2012/08/rotary-encoder-arduino/
  thiscall = millis();
  timebetween = thiscall - lastcall;
  lastcall = thiscall;

  int MSB = digitalRead(encoderPin1); //MSB = most significant bit
  int LSB = digitalRead(encoderPin2); //LSB = least significant bit

  int encoded = (MSB << 1) | LSB; //converting the 2 pin value to single number
  int sum  = (lastEncoded << 2) | encoded; //adding it to the previous encoded value

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue ++;
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue --;

  lastEncoded = encoded; //store this value for next time
}

You've told us what the code isn't doing - how about telling us what it is doing?

AWOL: You've told us what the code isn't doing - how about telling us what it is doing?

Sorry! The arudino is suspended from the ceiling together with the pully. The code is calculated to pull the body of the robot suspended by string from the bully up for 129 sec with 40 rotations, down for 17 sec with 10 rotations and so on for for 5minutes. The encoder has been programmed to carry the weight of the body of the robot so it takes a specifically calculated amount of time to get to a position in the air. Like I said before the problem is that when the servo is plugged in to power it rotates in the wrong direction. I have tried switching the wires around but that is clearly not the problem. I hope that is sufficient enough. I had a friend helping me with this project and I'm not so experienced myself. Thank you!

kaiulani: Like I said before the problem is that when the servo is plugged in to power it rotates in the wrong direction.

That sounds like you are just sending the wrong commands to the servo. Try inverting your logic. If values less than 90 make it go down change them to values greater than 90 so as to make it go up.

...R

The encoder has been programmed to carry the weight of the body of the robot

Which programmable encoder do you have? Most encoders are not programmable.

Try this out I think it is simpler and doesn't require retaining a sample of the last entry:

volatile long encoderValue = 0;  // Good you've set this as volatile Note it is required for interrupts



void setup() {

  attachInterrupt(0, updateEncoderClock, CHANGE);// Changed this Line
  attachInterrupt(1, updateEncoderData, CHANGE);// Changed this Line

}
void updateEncoderClock() {
  int Clock = digitalRead(encoderPin1); //MSB = most significant bit
  int Data = digitalRead(encoderPin2); //LSB = least significant bit
      if (Clock) {                           //Clock Pulse went HIGH
        if (Data) {         // Check if Data Pin is High at the moment of change
          encoderValue --;
        } else {
          encoderValue ++;
        }
      } else {//clock Pulse went Low
        // half step code higher resolution
        if (Data) {         // Check if Data Pin is High at the moment of change
          encoderValue ++;
        } else {
          encoderValue --;
        }
      }
}
void updateEncoderData() {
  int Clock = digitalRead(encoderPin1); //MSB = most significant bit
  int Data = digitalRead(encoderPin2); //LSB = least significant bit
      if (Data) {  //Data Pulse went HIGH
        if (Clock) {  // Check if Clock Pin is High at the moment of change
          encoderValue ++;
        } else {
          encoderValue --;
        }
      } else { //Data Pulse went LOW
        // half step code higher resolution
        if (Clock) {         // Check if Clock Pin is High at the moment of change
          encoderValue --;
        } else {
          encoderValue ++;
        }
      }
}