File content as of revision 4:f6ca4f7793bd:

#include "mbed.h"
#include "MODSERIAL.h" //for serial connection w/ PUTTY
#define SERIAL_BAUD 115200
#include "QEI.h" //for encoder readout
#include "HIDScope.h" //HIDScope

MODSERIAL pc(USBTX, USBRX); //initiate serial connection
QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); //Initiate encoder readout with pins D12, D13, 64 pulses per rotation and x4 encoding
DigitalOut motor1_direction(D4); //Pin used to control motor direction
PwmOut motor1_pwm(D5); //Pin used to control motor speed
InterruptIn button1(D2); //button
InterruptIn button2(D3); //button
AnalogIn potmeter1(A0); //potmeter

HIDScope scope(3); //initiate HIDScope with 2 channesl
Ticker scopeTimer; //Ticker for HIDScope

Timeout timeout;

const float timer = 0.01; //refresh timer
const float timer1 = 0.1; //refresh timer
const float countspr = 8400; //counts per rotation of motor axis
const float max_velocity = 5.2; //freespin max velocity of motor
const float pi = 3.14159265359; //delicious pi
int counts; //counts from encoder
int counts_p; //previous counts from encoder
int pwmvalue; //pwm value for motor control
float buttoncount;
float potmeter; //potmeter from 0-1
float referenceangle;
float referenceangle_p = 0;
float angle = 0; //angle of motor axis
float angleerror;
float referencevelocity; //controllable velocity
float velocity; //calculated velocity from encoder

void subtractcount();

void print(){ //print over serial connection
    pc.printf("%i\r\n", counts);
    pc.printf("%1.1f%\r\n", buttoncount);
    pc.printf("rangle: %3.3f%\r\n", referenceangle);
    pc.printf("angle: %3.3f%\r\n", angle);
    pc.printf("error: %3.3f%\r\n", angleerror);
    pc.printf("velocity: %3.3f%\r\n", velocity);
}

void controlmotor()    { //set motor speed
    angleerror = referenceangle-angle;
    if (angleerror < 0) motor1_direction = 1;
    if (angleerror > 0) motor1_direction = 0;
    potmeter = potmeter1.read();
    referencevelocity = potmeter*max_velocity;
    motor1_pwm.pulsewidth(fabs(angleerror)*3*referencevelocity/max_velocity*0.020);
}

void calculateangle()   { //calculate the angle of the axis based on counts
    if (counts == 0) angle = 0;
    else angle = -counts/countspr*2*pi;
}

void derivative()   { //calculate velocity based on derivative
    velocity = (counts-counts_p)/countspr*2*pi/timer;
    counts_p = counts;
}

void scopeSend()    {//send data over HIDScope
    scope.set(0,velocity);
    scope.set(1,angle);
    scope.set(2,referenceangle);
    scope.send();
}

void calculatereferenceangle()  {
    referenceangle = referenceangle_p+(buttoncount*pi/10);
    if(referenceangle < -pi) referenceangle = -pi;
    else if(referenceangle > pi) referenceangle = pi;
    referenceangle_p = referenceangle;
    buttoncount = 0;
}
void addcount()  {
    buttoncount++;
    button1.rise(&addcount);
    button2.rise(&subtractcount);
    timeout.attach(calculatereferenceangle, 0.5);
}
void subtractcount()    {
    buttoncount--;
    button1.rise(&addcount);
    button2.rise(&subtractcount);
    timeout.attach(calculatereferenceangle, 0.5);
}
int main()
{
    pc.baud(SERIAL_BAUD);
    Ticker time;
    Ticker time1;
    Ticker time2;
    Ticker time3;
    time.attach(print, timer1);
    time1.attach(controlmotor, timer);
    time2.attach(calculateangle, timer);
    time3.attach(derivative, timer);
    button1.rise(&addcount);
    button2.rise(&subtractcount);
    
    scopeTimer.attach_us(&scopeSend, 1e4);
    motor1_pwm.period(0.020f); // set pwm period to 20ms
    while (true) {
        counts = Encoder.getPulses();
    }
}