File content as of revision 5:6c16e9335262:

#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);
DigitalIn   button2(D10); 

MODSERIAL pc(USBTX,USBRX);
QEI Encoder(D12,D13,NC,32);
Ticker controller; // ticker with the name controller


float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    
    const float maxVelocity=8.4; // in rad/s of course!
    
    float referenceVelocity;  // in rad/s
    
    
    if (button1)   {
        // Clockwise rotation // this should be counterclockwise (what is chosen then: CCW = positive direction)
        // als button niet is ingedrukt:
        // linksom = positief
        
        referenceVelocity = potMeterIn1 * maxVelocity;
        ledr = 1;
        ledb = 0;
        } 
        else {
        // Counterclockwise rotation // this should be clockwise
        // als button wel is ingedrukt:
        referenceVelocity = -1*potMeterIn1 * maxVelocity;
        ledb = 1;
        ledr = 0;
        }
  
  
   /*
     
   int rP1 = GetReferencePosition();
   int counts = Encoder.getPulses();
    
     if (counts > rP) {
          
       referenceVelocity = 0*potMeterIn1 * maxVelocity;;
       ledb = 1;
        ledr = 0;  
       } 

*/


/* 
        
    if (button2==false) {
       
       referenceVelocity = 0*potMeterIn1 * maxVelocity;;
       ledb = 1;
        ledr = 0;
       
       } 
*/

// check of hij stopt

     
        
    return referenceVelocity;
}


float FeedForwardControl(float &referenceVelocity) // is de motorvalue
{
    // very simple linear feed-forward control
    const float MotorGain=8.4; // unit: (rad/s) / PWM
    float motorValue = referenceVelocity / MotorGain;
    return motorValue;
}

float GetReferencePosition()
{
    int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position 
    // integrated quadrature encoder that provides a resolution of 64 counts per revolution of the motor shaft, which corresponds to 8400 counts per revolution of the gearbox’s output shaft.
    int referencePosition;
    referencePosition = (potMeterIn2 * potmultiplier);
    return referencePosition;
}

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; // direction
        else motor1DC=0;
    if (fabs(motorValue)>1) motor1PWM = 1; //??. motor1PWM is the speed
       else motor1PWM = fabs(motorValue);
}

void MeasureAndControl(void)
{
       
    float referenceVelocity = GetReferenceVelocity(); // these variables are going to be ticked by 0.1
    int referencePosition = GetReferencePosition();
    int counts =  Encoder.getPulses();
    

    
       // if(counts < referencePosition)
       
       
        {
            float motorValue = FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        }
        
        
/*
        if(button2==false)
        {
        float motorValue = -1 * FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        }    
                
  if(referencePosition==1575) // dit werkt!
        {
        float motorValue = -1 * FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        }    
        
         if(referencePosition > 1575) // dit werkt!
        {
        float motorValue = -1 * FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        }    
 
 */
 
          if(counts > 1575) // dit werkt niet...
        {
        float motorValue = -1 * FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        }    
 


        
        /*
        else
        {
            float motorValue = -1 * FeedForwardControl(referenceVelocity);
            SetMotor1(motorValue);
        }    
*/
 
        
}



int main()
{
    pc.baud(9600);
    QEI Encoder(D12,D13,NC,32);
              
    ledr = 1;
    ledb = 1;
    ledg = 1;
    
   controller.attach(&MeasureAndControl, 0.1); //ticker. Change ticker, what happens? maybe then conditions with counts work?
//controller.attach(&MeasureAndControl, 1); // werkt niet
//controller.attach(&MeasureAndControl, 0.01); // werkt niet
//controller.attach(&MeasureAndControl, 0.1); // origineel

       
    while(1)
    {      
        
        float rV = GetReferenceVelocity();    
        int rP = GetReferencePosition();
        float mV = FeedForwardControl(rV); 
        int counts = Encoder.getPulses(); 
 
 
/*         
        if(counts > rP) // dit werkt niet... in de main, while loop
        
        {
        float motorValue = 0 * FeedForwardControl(rV);
            SetMotor1(motorValue);                         // hij verandert niet van richting, maar beweegt heen en weer
        }    
  
*/

        if(button2 == false) // counts > rP && 
        
   {
       const float maxVelocity=8.4; // in rad/s of course!
       rV = 0*potMeterIn1 * maxVelocity;;
       ledb = 1;
        ledr = 0;
        
          
        float motorValue = 0 * FeedForwardControl(rV);
            SetMotor1(motorValue);                         // hij verandert niet van richting, maar beweegt heen en weer
        
       } 



          
        wait(0.2) ; 
        pc.printf("\r reference velocity: %0.2f. Reference Position: %i Motor Value is: %0.2f number of counts: %i\n",rV,rP,mV,counts);

    }
     
   
}

// Voor Serial gebruik: Mac Terminal: screen /dev/tty.usbmodem1422 9600