File content as of revision 7:080805fc3cf0:

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



DigitalOut gpo(D0);
DigitalOut ledb(LED_BLUE);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut motor1DC(D7);
PwmOut motor1PWM(D6);
DigitalOut motor2DC(D4);
PwmOut motor2PWM(D5);

AnalogIn   potMeterIn1(A0);
AnalogIn   potMeterIn2(A1);

DigitalIn   button1(D11);

MODSERIAL pc(USBTX,USBRX);
QEI Encoder(D12,D13,NC,32);
Ticker controller;



float GetReferencePosition()
{
    int potmultiplier = 8000; // constant to multiply the pot 2 value with to get a reference position
    
    float referencePosition;
    referencePosition = potMeterIn2 * potmultiplier;
    return referencePosition;
}

float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    
    int referencePosition = GetReferencePosition();
    int counts =  Encoder.getPulses();
    
    const float maxVelocity=8.4; // in rad/s of course!
    
    float referenceVelocity;  // in rad/s
    
    if (counts > referencePosition)   {
        // Clockwise rotation
        referenceVelocity = -1*potMeterIn1 * maxVelocity;
        ledr = 1;
        ledg = 1;
        ledb = 0;
        } 
        else if ( counts < referencePosition){
        // Counterclockwise rotation
        referenceVelocity = potMeterIn1 * maxVelocity;
        ledb = 1;
        ledg = 1;
        ledr = 0;
        }
        else{
        referenceVelocity = 0;
        ledb = 1;
        ledr = 1;
        ledg = 0;
        }
    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;
    
   // float referenceVelocity = GetReferenceVelocity();
               
        /* if(counts < referencePosition)
        {
            //float motorValue = FeedForwardControl(referenceVelocity);
            //SetMotor1(motorValue);
            motorValue = referenceVelocity / MotorGain;
        }
        else if (counts > referencePosition)
        {
            //float motorValue = b * FeedForwardControl(referenceVelocity);
            //SetMotor1(motorValue);
            
            motorValue=  -1 * (referenceVelocity / MotorGain);
        }  
        else
        {
            motorValue = 0;
        }*/
    
    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) motor1DC= 1;
        else motor1DC=0;
    if (fabs(motorValue)>1) motor1PWM = 1;
        else motor1PWM = fabs(motorValue);
}

void MeasureAndControl(void)
{
    float referenceVelocity = GetReferenceVelocity();
    int referencePosition = GetReferencePosition();
    float motorValue = FeedForwardControl(referenceVelocity);
    SetMotor1(motorValue);
    
    /*int counts =  Encoder.getPulses();
    
    
    
        if(counts < referencePosition)
        {
            float motorValue = FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        }
        else
        {
            int b = -1;
            float motorValue = b * FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        } */
        
}



int main()
{
    pc.baud(115200);
    QEI Encoder(D12,D13,NC,32);
              
    ledr = 1;
    ledb = 1;
    ledg = 1;
    
    controller.attach(&MeasureAndControl, 0.1);
       
    while(1)
    {      
        int counts = Encoder.getPulses(); 
        float rV = GetReferenceVelocity();
        float mV = FeedForwardControl(rV);
        int rP = GetReferencePosition();
          
        pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",rV,rP,mV,counts);
    }
   
}