File content as of revision 5:4b25551aeb6e:

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

// Algemeen
DigitalIn button2(SW2);
DigitalIn button3(SW3);
DigitalOut led(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
MODSERIAL pc(USBTX, USBRX);

//Motoren
DigitalOut direction1(D4);
PwmOut pwmpin1(D5);
PwmOut pwmpin2(D6);
DigitalOut direction2(D7);
volatile float PWM1;
volatile float PWM2;
volatile float pwm2;

//Encoder
DigitalIn EncoderA(D13);
DigitalIn EncoderB(D12);
QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
float Pulses2;
float Degrees2;

//Pot meter
AnalogIn pot(A1);
float Pot2;

//Ticker
Ticker Pwm;
Ticker PotRead;
Ticker Kin;

//Kinematica
float stap;
float KPot;
float KPotabs;
float ElbowReference;
float Ellebooghoek1;
float Ellebooghoek2;
float Ellebooghoek3;
float Ellebooghoek4;
float Hoeknieuw;

//Limiet in graden
float lowerlim = 0;
float upperlim = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor

   
float Kinematics(float KPot)
    {
            
    if (KPot > 0.45f){
    stap = KPot*15;         // 144 graden van de arm in 5 seconden
    Hoeknieuw = ElbowReference + stap;
    return Hoeknieuw;
    }
    
    else if (KPot < -0.45f){
    stap = KPot*15;
    Hoeknieuw = ElbowReference + stap;
    return Hoeknieuw;
    }
    
    else{
    return ElbowReference;    
    }  
    }

float Limits(float Ellebooghoek2){
    
    if (Ellebooghoek2 <= upperlim && Ellebooghoek2 >= lowerlim) {       //Binnen de limieten
        Ellebooghoek3 = Ellebooghoek2;
    } 
    
    else {
            if (Ellebooghoek2 >= upperlim) {                            //Boven de limiet
            Ellebooghoek3 = upperlim;
        } 
            else {                                                      //Onder de limiet
            Ellebooghoek3 = lowerlim;
        }
    }
    
    return Ellebooghoek3;
    }


void Period(void)
    {
    pwmpin2.period_us(60);
    }
    
void PwmMotor(void)
    {     
    float Ellebooghoek1 = Kinematics(pwm2);
    float Ellebooghoek4 = Limits(Ellebooghoek1);
    ElbowReference = Ellebooghoek4;
    
    pc.printf("ElbowReference:%f\n", ElbowReference);
    pc.printf("pwm2=%f\r\n",pwm2);
    
    Degrees2 = (Pulses2/8400)*360;
    pc.printf("Degrees is:%f\n", Degrees2);
    direction2 = pwm2 < 0.0f;    //positief = CW, negatief = CCW
    pwmpin2 = fabs(pwm2);
    

    }  
void MotorOn(void)
    {
    pwmpin2 = 0;
    Pwm.attach (PwmMotor, 0.1);
    
    }
void MotorOff(void)
    {
    Pwm.detach ();    
    pwmpin2 = 0;
    }

void ContinuousReader(void){
    Pot2 = pot.read();
    pwm2 =(Pot2*2)-1;            //scaling
    Pulses2 = encoder2.getPulses();
    //pc.printf("%f\r\n",Pot2);
    }
    

       
int main() {
    Period();
    PotRead.attach(ContinuousReader,0.1);
    pc.baud(115200);
    pc.printf("start\r\n");
    led = 1;
    led2 =1;
    led3 =1;
    
    while (true){
    led3 = 0;
    if (!button2)
    {
        led3 = 1;
        led = 0;
        pc.printf("MotorOn\r\n");
        MotorOn();   
    }
    if (!button3)
    {
        pc.printf("MotorOff\r\n");
        PotRead.detach();
        MotorOff();
    }
    led = 0;
        }
    }