File content as of revision 5:37e230689418:

#include "mbed.h"
#include <math.h>
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"

//set pins
DigitalIn encoder1A (D13); //Channel A van Encoder 1
DigitalIn encoder1B (D12); //Channel B van Encoder 1
DigitalOut led1 (D11); 
DigitalOut led2 (D10);
AnalogIn potMeter1(A2);
AnalogIn potMeter2(A1);
DigitalOut motor1DirectionPin(D7);
PwmOut motor1MagnitudePin(D6);
DigitalIn button1(D5);

//set settings
Serial pc(USBTX,USBRX);
Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
HIDScope    scope(2);

//set datatypes
int counts = 0;
double DerivativeCounts;

float error_prev = 0;
float IntError = 0;
float t_sample = 0.01; //seconds

int countsPrev = 0;
float referenceVelocity = 0;
float bqcDerivativeCounts = 0;
const float PI = 3.141592653589793;
//float Potmeter1 = potMeter1.read();
//float Potmeter2 = potMeter2.read();
const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
const int ccw = 1;

//set BiQuad
BiQuadChain bqc;
BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);

//set go-Ticker settings
volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
void BiQuadTicker_act(){BiQuadTicker_go=true;};
void FeedbackTicker_act(){FeedbackTicker_go=true;};
void TimeTracker_act(){TimeTracker_go=true;};
//void sampleT_act(){sampleT_go=true;};

//define encoder counts and degrees
QEI Encoder(D12, D13, NC, 32); // turns on encoder
const int counts_per_revolution = 4200; //counts per motor axis revolution
const int inverse_gear_ratio = 131;
//const float motor_axial_resolution = counts_per_revolution/(2*PI);
const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);  //87567.0496892 counts per radian, encoder axis

float GetReferencePosition()
{
    // Returns reference position in rad. 
    // Positive value means clockwise rotation.
    const float maxPosition = 2*PI; //6.283185307179586; // in radians
    float Potmeter1 = potMeter1.read();
    float referencePosition = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians 
    pc.printf("Max Position: %f rad \r\n", maxPosition);
    pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
    pc.printf("Motor Axis Ref Position: %f rad \r\n", referencePosition);
    return referencePosition;
}

float FeedForwardControl(float referencePosition)
{
    float EncoderPosition = counts/resolution;         //position in radians, encoder axis
    float Position = EncoderPosition*inverse_gear_ratio;        //position in radians, motor axis
    // linear feedback control
    scope.set(0,referencePosition);
    scope.set(1,Position);
    scope.send();
    float error = referencePosition - Position; // 'error' in radians
    float Kp = 1; //potMeter2.read();
    
    float IntError = IntError + error*t_sample;
    float maxKi = 0.2;
    float Ki = potMeter2.read()*maxKi;
    
    //float DerivativeError = (error_prev + error)/t_sample;
    float maxKd = 0.2;
    //float Kd = potMeter2.read()*maxKd;
    
    float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd;
    pc.printf("Motor Axis Position: %f rad \r\n", Position);
    pc.printf("Counts encoder: %i rad \r\n", counts);
    pc.printf("Kp: %f \r\n", Kp);
    pc.printf("MotorValue: %f \r\n", motorValue);
    
    error_prev = error;
    return motorValue;
}

void SetMotor1(float motorValue)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (motorValue >=0) 
        {motor1DirectionPin=cw;
        led1=1;
        led2=0;
        }
    else {motor1DirectionPin=ccw;
        led1=0;
        led2=1;
        }
    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motorValue);
}

void MeasureAndControl()
{
    // This function measures the potmeter position, extracts a
    // reference position from it, and controls the motor with 
    // a Feedback controller. Call this from a Ticker.
    float referencePosition = GetReferencePosition();
    float motorValue = FeedForwardControl(referencePosition);
    SetMotor1(motorValue);
}

void TimeTrackerF(){
     //wait(1);   
     //float Potmeter1 = potMeter1.read();
     float referencePosition = GetReferencePosition();
     pc.printf("TTReference Position: %d rad \r\n", referencePosition);
     //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
     //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
     pc.printf("TTCounts: %i \r\n", counts);
}
/*
void sample()
{
    int countsPrev = 0;
    QEI Encoder(D12, D13, NC, 32);
    counts = Encoder.getPulses();  // gives position
    //scope.set(0,counts);
    DerivativeCounts = (counts-countsPrev)/0.001;
    //scope.set(1,DerivativeCounts);
    countsPrev = counts; 
    //scope.send();
    pc.printf("Counts: %i rad/s \r\n", counts);
    pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
}

void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
    //double in=DerivativeCounts();
    bqcDerivativeCounts=bqc.step(DerivativeCounts);
    //return(bqcDerivativeCounts);
    }
    
void MeasureP(){
    double ref_position = Potmeter1;                //reference position from potmeter
    int counts = Encoder.getPulses();           // gives position    
    double position = counts/resolution;         //position in radians
    double rotation = ref_position-position;     //rotation is 'position error' in radians
    double movement = rotation/(2*PI);  //movement in rotations
    double Kp = Potmeter2;
    }
    
double P(double rotation, double Kp){
    double P_output = Kp*movement;
    return P_output;
    }
    
void MotorController(){
    double output = P(rotation, Kp);
    
    if(rotation>0){
        motor1DirectionPin.write(cw);
        motor1MagnitudePin.write(output);
        }
    if(rotation<0){
        motor1DirectionPin.write(ccw);
        motor1MagnitudePin.write(-output);
        }
    }
*/
int main()
{
 //Initialize
 led1=1;
 led2=1;
 pc.baud(115200);
 pc.printf("Test putty");
 //float Potmeter = potMeterIn.read();
 MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
 bqc.add(&bq1).add(&bq2);
 //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz
 //TimeTracker.attach(&TimeTracker_act, 1.0f);
 QEI Encoder(D12, D13, NC, 32); // turns on encoder
 //sampleT.attach(&sampleT_act, 0.1f);
 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
 
 while(1)
    {
        if (MeasureTicker_go){
            MeasureTicker_go=false;
            MeasureAndControl();
            counts = Encoder.getPulses();           // gives position of encoder 
            pc.printf("Resolution: %f pulses/rad \r\n",resolution);
            }
      /*
            // Encoder part
            counts = Encoder.getPulses();  // gives position
            DerivativeCounts = ((double) counts-countsPrev)/0.01;  
            
            scope.set(0,counts);
            scope.set(1,DerivativeCounts);
            //scope.set(1,bqcDerivativeCounts);
            scope.send();
            countsPrev = counts;
            //pc.printf("Counts: %i rad/s \r\n", counts);
            //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
        }
        
        if (BiQuadTicker_go){
            BiQuadTicker_go=false;
            BiQuadFilter();
        }
    
        if (FeedbackTicker_go){
            FeedbackTicker_go=false;
            Feedback();
        
        if (TimeTracker_go){
            TimeTracker_go=false;
            TimeTrackerF();
        }
        
        if (sampleT_go){
            sampleT_go=false;
            sample();
        }*/
    }
}