DC servo motor with optical encoder

I’m about to replace a stepper motor with a DC servo motor with optical encoder. With the help of the PID-library from br3ttb (http://www.arduino.cc/playground/Code/PIDLibrary) I am able to control either the velocity or the position of the motor.

The real challenge at the moment, for me, is to control both velocity and position. To be more specifically I want to:

1.) ramp up velocity at start;
2.) keep the velocity constant;
3.) ramp down velocity before a given position;
4.) and hold given position whatever happens.

If I understand correctly I should use ‘Cascade control’ ( http://en.wikipedia.org/wiki/PID_controller#Cascade_control); using two PIDs arranged with one PID controlling the set point of another. However, my interpretation is not working as it should. Any ideas?

#include <avr/interrupt.h>
#include <PID_Beta6.h>

#define EncoderPinA 3    //Quadrature Track A (interrupt 1)
#define EncoderPinB 4    //Quadrature Track B
#define OutputPin 11

volatile long EncoderPos = 0;
long EncoderPos0 = 0;
unsigned long Time = 0;
unsigned long Time0 = 0;
int RPM = 0;
int Error = 0;

double DisplacementSetpoint, DisplacementInput, DisplacementOutput;
PID DisplacementPID(&DisplacementInput, &DisplacementOutput, &DisplacementSetpoint, 2,3,0);

double VelocitySetpoint, VelocityInput, VelocityOutput;
PID VelocityPID(&VelocityInput, &VelocityOutput, &VelocitySetpoint, 2,3,0);


void setup(){ 
  pinMode(EncoderPinA, INPUT); 
  digitalWrite(EncoderPinA, HIGH);    // turn on pullup resistor
  pinMode(EncoderPinB, INPUT); 
  digitalWrite(EncoderPinB, HIGH);    // turn on pullup resistor

  attachInterrupt(1, doEncoder, CHANGE);    // encoder track A on interrupt 1 (pin 3)

  DisplacementPID.SetMode(AUTO);
  VelocityPID.SetMode(AUTO);

  beginSerial(115200);
} 

void loop(){
  Time = micros();

  RPM = 60 * 1000000 * abs(EncoderPos - EncoderPos0) / ((Time - Time0) * 2000);
  Error = 100 * (RPM - VelocitySetpoint) / VelocitySetpoint;

  EncoderPos0 = EncoderPos;
  Time0 = Time;
  
  DisplacementSetpoint = 30000;                        // just a position
  DisplacementInput = EncoderPos;                  // read input from interrupt
  DisplacementPID.Compute();                        // calculate the magic number
  VelocitySetpoint = DisplacementOutput;      // use this output as set point for velocity (?)
  VelocityInput = RPM;                                    // input from RPM calculation
  VelocityPID.Compute();                              // calculate output
  analogWrite(OutputPin,VelocityOutput);      // use VelocityOutput
}

void doEncoder(){    //every time a change happens on encoder pin A doEncoder will run.
  if (digitalRead(EncoderPinA) == HIGH) {    // found a low-to-high on channel A
    if (digitalRead(EncoderPinB) == LOW) {    //CCW
      EncoderPos = (EncoderPos - 1);
    } 
    else {    //CW
      EncoderPos = (EncoderPos + 1);
    }
  } 
  else {    //found a high-to-low on channel A
    if (digitalRead(EncoderPinB) == LOW) {    //CW
      EncoderPos = (EncoderPos + 1);
    } 
    else {    //CCW
      EncoderPos = (EncoderPos - 1);
    }
  }
}

hi, im quite interested to know if you got this to work and what the problem was?

thanks