File content as of revision 17:055edbd6ba9d:

#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"

#define TSAMP 0.01

void clamp(float * in, float min, float max);
volatile bool looptimerflag;
HIDScope scope(3);
float new_pwm = 0;

void setlooptimerflag(void)
{
    looptimerflag = true;
}

void clamp(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}

void looper()
{
    if (new_pwm==1) {
        new_pwm = -1;
    } else {
        if (new_pwm==-1) {
            new_pwm = 0;
        } else {
            if (new_pwm==0) {
                new_pwm = 1;
            }
        }
    }
}

int main()
{
    //motor 1, 25D
    Encoder motor1(PTD3, PTD5);
    DigitalOut motor1dir(PTC9);
    PwmOut pwm_motor1(PTC8);
    pwm_motor1.period_us(100); //10kHz PWM frequency

    //motor 2, 25D
    Encoder motor2(PTD2,PTD0);
    DigitalOut motor2dir(PTA4);
    PwmOut pwm_motor2(PTA5);
    pwm_motor2.period_us(100); //10kHz PWM frequency

    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);
    Ticker flip_switch;
    flip_switch.attach(looper, 2);

    while(1) {
        while(!looptimerflag);
        looptimerflag = false; //clear flag

        clamp(&new_pwm, -1,1);

        scope.set(0, new_pwm);
        scope.set(1, motor1.getPosition());
        scope.set(2, motor2.getPosition());
        scope.send();

        if(new_pwm > 0)
            motor1dir = 0;
        else
            motor1dir = 1;

        if(new_pwm > 0)
            motor2dir = 0;
        else
            motor2dir = 1;

        pwm_motor1.write(abs(new_pwm));
        pwm_motor2.write(abs(new_pwm));
    }
}