File content as of revision 4:9ecf57487c72:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"

/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
void keep_in_range(float * in, float min, float max);

volatile bool looptimerflag;

void setlooptimerflag(void)
{
    looptimerflag = true;
}


int main() {
    //LOCAL VARIABLES 
    AnalogIn potmeter(PTC2);
    MODSERIAL pc(USBTX,USBRX);

    Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx
    Encoder motor2(PTD5,PTC8);//first pin on PTAx or PTDx
    PwmOut pwm_motor1(PTA12);
    PwmOut pwm_motor2(PTA5);
    DigitalOut motordir1(PTD3);
    DigitalOut motordir2(PTD1);
//MOTOR A    
    float setpoint;
    float pwm_to_motor1;
    float setspeed;
    float speed;
    float position2;
    float setpoint2;
//MOTOR B    
    float setpointB;
    float pwm_to_motor2;
    float setspeedB;
    float speedB;
    float position2B;
    float setpoint2B;
    
    //START OF CODE
    pc.baud(230400);
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,0.01);  
    pc.printf("bla");
    //A
    speed = 0; 
    position2 = 0;
    setpoint2 = 0;
    //B
    speedB = 0; 
    position2B = 0;
    setpoint2B = 0;
    //INFINITE LOOP 
    while(1) {
        while(looptimerflag != true);
        looptimerflag = false;
        
//MOTOR A
        setpoint = (potmeter.read()-0.5)*8000;
        setspeed =(setpoint - setpoint2)/0.01;
        speed = (motor1.getPosition() - position2)/0.01;
        pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition());
        pwm_to_motor1 = (setpoint - motor1.getPosition())*.0001 + (setspeed - speed)*.00005 ;
        keep_in_range(&pwm_to_motor1, -1,1);
        setpoint2 = setpoint;
        position2 = motor1.getPosition();
        if(pwm_to_motor1 > 0)
            motordir1 = 1;
        else
            motordir1 = 0;
        //WRITE VALUE TO MOTOR  
        pwm_motor1.write(abs(pwm_to_motor1));
        
//MOTOR B
        setpointB = (potmeter.read()-0.5)*8000;
        setspeedB =(setpointB - setpoint2B)/0.01;
        speedB = (motor2.getPosition() - position2B)/0.01;
        pc.printf("s: %f, %d \n\r", setpointB, motor2.getPosition());
        pwm_to_motor2 = (setpointB - motor2.getPosition())*.0001 + (setspeedB - speedB)*.00005 ;
        keep_in_range(&pwm_to_motor2, -1,1);
        setpoint2B = setpointB;
        position2B = motor2.getPosition();
        if(pwm_to_motor2 > 0)
            motordir2 = 1;
        else
            motordir2 = 0;
        //WRITE VALUE TO MOTOR  
        pwm_motor2.write(abs(pwm_to_motor2));
    }
}


//coerces value 'in' to min or max when exceeding those values
//if you'd like to understand the statement below take a google for
//'ternary operators'.
void keep_in_range(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}