File content as of revision 6:6bc6ce1fe94e:

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


//*****************Defining ports********************
DigitalOut motor1DirectionPin (D4);
PwmOut motor1MagnitudePin(D5);
DigitalIn button(D2);
Serial pc(USBTX,USBRX);
QEI encoder(D12,D13,NC,32);
HIDScope scope(1);

//*******************Setting tickers and printers*******************
Ticker callMotor;
Ticker angPos;
Ticker t1;

//**************Go flags********************************************
volatile bool fn1_go = false;
void fn1_activate(){ fn1_go = true; }; //Activates the go−flag

//***************Global Variables***********************************
const double pi = 3.14159265359;
const double ts = 1.0/1000.0;
const int velocityChannel = 0;
volatile double radians =0.0;
const double transmissionShoulder =94.4/40.2;
const double transmissionElbow = 1.0;
//*****************Angles Arms***********************

double O1=1.7633;
double O2=2.0915;
double O3=1.8685;
double O4=1.1363;
double O5=2.3960;
double O6=2.0827;
double B1=1.3551;
double B2=0.5964;
double B3=0.06652;
double B4=0.0669;
double B5=1.7462;
double B6=-0.8994;

//**********functions******************************
void getAngPosition() 
{   
    volatile int pulses = encoder.getPulses();
    radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
    scope.set(velocityChannel,radians);
    scope.send();
}
  
void control(double radians)
{
if(radians>=(-2*pi))
        {
        motor1MagnitudePin=1.0;//MOET NOG TUSSENWAAREN KRIJGEN
        }
    else
        {
        motor1MagnitudePin=0.0;    
        }
}
//****************MAIN FUNCTION*********************************
int main()
{
    angPos.attach(&getAngPosition,ts);
    t1.attach(&fn1_activate, 0.0001f);
    while(true)
        if(fn1_go)
        {
        fn1_go = false;
        control(radians);
        }


    pc.baud(115200);
    while(true);
}