File content as of revision 0:43160ef59f9f:

#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(A0);
AnalogIn potMeter2(A1);
DigitalOut motor1DirectionPin(D7);
PwmOut motor1MagnitudePin(D6);
DigitalIn button1(D5);

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

//set datatypes
int counts;
double DerivativeCounts;
int countsPrev = 0;
float referenceVelocity = 0;
double bqcDerivativeCounts = 0;
const double PI = 3.141592653589793;
double Potmeter1 = potMeter1.read();
double Potmeter2 = potMeter2.read();
const int cw = 1;
const int ccw = 0;

//define encoder counts and degrees
QEI Encoder(D12, D13, NC, 32); // turns on encoder
int counts_per_revolution = 4200;
const double gear_ratio = 131;
const double resolution = counts_per_revolution/(2*PI/gear_ratio);  //counts per radian

//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 MeasurePTicker_go=false, PTicker_go=false, MotorControllerTicker_go=false;// TimeTracker_go=false, sampleT_go=false;
void MeasurePTicker_act(){MeasurePTicker_go=true;}; // Activates go-flags
void PTicker_act(){PTicker_go=true;};
void MotorControllerTicker_act(){MotorControllerTicker_go=true;};
//void TimeTracker_act(){TimeTracker_go=true;};
//void sampleT_act(){sampleT_go=true;};

/*
float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    const float maxVelocity = 8.4; // in rad/s of course!
    if (button1 == 0){
        led1=1;
        led2=0;
        // Counterclockwise rotation
        referenceVelocity = potMeterIn * maxVelocity;  
    } 
    else {   
        led1=0;
        led2=1;
        // Clockwise rotation
        referenceVelocity = -1*potMeterIn * maxVelocity;
    }
    return referenceVelocity;
}

float FeedForwardControl(float referenceVelocity)
{
    // very simple linear feed-forward control
    const float MotorGain=8.4; // unit: (rad/s) / PWM
    float motorValue = referenceVelocity / MotorGain;
    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=1;
        else motor1DirectionPin=0;
    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motorValue);
}

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

void TimeTrackerF(){
     wait(1);   
     float Potmeter = potMeterIn.read();
     pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
     pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
     //pc.printf("Counts: %i rad/s \r\n", counts);
     //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts);
}

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=0;
 led2=0;
 float Potmeter = potMeterIn.read();
 MeasureP.attach(&MeasureP_act, 0.01f); 
 P.attach(&P_act, 0.01f);
 MotorController.attach(&MotorController_act, 0.01f);
 pc.baud(115200);  
 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 (MeasureP_go){
            MeasureP_go=false;
            MeasureP();
            // 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 (PTicker_go){
            PTicker_go=false;
            P();
        }
        if (MotorControllerTicker_go){
            MotorControllerTicker_go=false;
            MotorController();
        /*if (TimeTracker_go){
            TimeTracker_go=false;
            TimeTrackerF();
        }
        if (sampleT_go){
            sampleT_go=false;
            sample();
        }*/
    }
}