File content as of revision 3:1ab2d2b1705f:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#include "HIDScope.h"

//Defining ports
DigitalOut motor1DirectionPin (D4);
PwmOut motor1MagnitudePin(D5);
DigitalIn button(D2);
AnalogIn potmeter(A0);
Serial pc(USBTX,USBRX);
QEI Encoder(D12,D13,NC,32);
//HIDScope scope(1);

// Setting tickers and printers
Ticker tick;
Ticker callMotor;
Ticker pos;
Ticker encoderTicker;

const float pi = 3.14159265359;
const float ts = 1.0/1000.0;
const int VELOCITY_CHANNEL = 0;

//Get reference velocity
float GetReferenceVelocity() {
    // Returns reference velocity in rad/s.
    // Positive value means clockwise rotation.
    const float maxVelocity = 8.4; //Als de potmeter van 0 tot 1 gaat (als die maar tot 0.25 gaat, dan max velocity 4x zo groot als motorgain maken
    volatile float referenceVelocity;  // in rad/s
    if (button) //nog even kijken voor wanneer die + en - is
    {
        // Clockwise rotation
        referenceVelocity = potmeter * maxVelocity;
    } 
    else
    {
        // Counterclockwise rotation
        referenceVelocity = -1*potmeter * maxVelocity;
    }
    return referenceVelocity;
}

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);
    }
}

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 MeasureAndControl(void) {
    // 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);
}
   
        
float getPosition() {
        //getting the position of the motor to take derivative to have a
        //different velocity signal.
        const float ts = 1.0/1000.0;
        volatile float pulses = Encoder.getPulses();
        volatile float revolutions = Encoder.getRevolutions();
        volatile float position = (pulses / 2 * 32) * 360; //formula from QEI library
    return position;
}       
 
void encoderTick() {
  int pulses = Encoder.getPulses();
   
   //calculate the total angle that is traveled so far.
    double radians = (pulses / ts) * ((2*pi)/32);

    //update lastEncoderRead such that it can be used for next time
    double lastEncoderRead = radians;
    
    //approximate the derivative for the angular velocity.
    double xdif = radians - lastEncoderRead;
    double xderiv = xdif / ts;
   
    
    pc.printf("rad=%d \n\d",radians);
/*    send velocity to HIDScope
    scope.set(VELOCITY_CHANNEL, radians);
  scope.send();
   */ 
    }

   
        
int main()
{
    encoderTicker.attach(encoderTick, 1.0/100000.0); 
    motor1MagnitudePin.period(1.0/100000.0);
    callMotor.attach(MeasureAndControl,1);
    pc.baud(115200);
    //tick.attach(print,1);
    while(true){}
}