Visual Height by Velocity

//Arduino code:

//One-motor velocity control

//edit these parameters to obtain a good reponse
int posgain=2;   
int velgain=0.4;  //multiplies vest per second
long interval=10; //milliseconds between updates
long time;
long tnext;
long tlast;

int m1f = 13; //motor 1 forwards 
int m1r = 12; //motor 1 reverse 
int m1pwm = 5;
int enc1a = A0;
int enc1b = A1;
volatile int m1pos = 300;
int m1drive = 0;
int v1target = 0;
int m1mov=0;
int m1old=0;
int v1est=0;

int move[16]={0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
int change;
byte bits;
byte oldbits;
uint8_t sendchar[8];

void setup(){
  pinMode(m1f,OUTPUT);
  pinMode(m1r, OUTPUT);
  digitalWrite(m1pwm,HIGH);

oldbits=PINC;  
  cli();
  PCICR |= 0b00000010;   // turn on port C interrupts
  PCMSK1 |= 0b00000011;  // enables pins PC0 to PC1,
  sei();
  
  Serial.begin(9600);
  tnext=millis();
}

void loop() {
  time=millis();
  if (time>tnext){
    tnext=time+interval;
    v1est=(m1pos-m1old)*1000/interval;
    m1old=m1pos;
//Velocities will show rate per second
// motor velocity control
    m1drive=(v1target-v1est)*velgain;
    setdrive(m1drive,m1f,m1r,m1pwm);
  }
  if (Serial.available()>1){
// two bytes give target velocity
    tlast=time;

v1target=Serial.read();              //high byte
    if(v1target>127){v1target-=256;}     //fix sign bit
    //this restores the sign bit
    v1target=256*v1target+Serial.read(); //add low byte
 
  }  //end of command received

}    //end of loop

ISR(PCINT1_vect){ 
// interrupt routine on encoder change
  bits = PINC;

//for motor 1
  change = ((oldbits & 0b1100) | (bits & 0b11));
  m1mov = move[change]; 
  if(m1mov){  
    m1pos += m1mov;
  }

oldbits=bits<<2;
}

int lim(int x){
  if(x>255){x=255;}
  if(x<-255){x=-255;}
return x;
}

void setdrive(int m,int f, int r,int p){
  if(m>0){
    digitalWrite(f,HIGH);
    digitalWrite(r,LOW);
  }else{
    digitalWrite(f,LOW);
    digitalWrite(r,HIGH);
  }
  analogWrite(p,abs(lim(m)));
}