File content as of revision 8:7cc4d6d9c2b5:

#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);
DigitalOut led(D0);
//*******************Setting tickers and printers*******************
Ticker callMotor;
Ticker angPos;
Ticker t1;
Ticker t2;
Ticker t3;
Ticker myControllerTicker;
//**************Go flags********************************************
volatile bool fn1_go = false;
void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
volatile bool fn2_go = false;
void fn2_activate(){ fn2_go = true; }; //Activates the go-flag
volatile bool fn3_go = false;
void fn3_activate(){ fn3_go = true; }; //Activates the go-flag

//***************Global Variables***********************************
const double pi = 3.14159265359;
const double ts = 1.0/1000.0;
const int velocityChannel = 0;
const double transmissionShoulder =94.4/40.2;
const double transmissionElbow = 1.0;
const double MOTOR1_KP = 2.5;
const double CONTROLLER_TS = 0.01;
volatile double radians;
volatile double proportionalErrormotor1;
//*****************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
}
  
 // Next task, measure the error and apply the output to the plant
 void motor1_Controller(double radians)
 {
 double reference = -2*pi;
 volatile double error1 = reference - radians;
 proportionalErrormotor1 = error1 * MOTOR1_KP;
  // scope.set(0, proportionalErrormotor1);
   //scope.send();
   
   // scope.set(velocityChannel,proportionalErrormotor1);
    //scope.send();
 }
  
  
void control(double proportionalErrormotor1)
{
if(proportionalErrormotor1<0)
        {
        motor1MagnitudePin=1.0;//MOET NOG TUSSENWAAREN KRIJGEN
        }
    else
        {
        motor1MagnitudePin=0.0;    
        }
}


 
//****************MAIN FUNCTION*********************************
int main()
{motor1MagnitudePin.period(1.0/1000.0);
t1.attach(&fn1_activate, 0.1f);
t2.attach(&fn2_activate, 0.0001f);
t3.attach(&fn3_activate, 0.0001f);
while(true){
        if(fn1_go)
        {
        fn1_go = false;
        control(proportionalErrormotor1);
        }
        if(fn2_go)
        {
        fn2_go = false;
        motor1_Controller(radians);
        }
        if(fn3_go)
        {
        fn3_go = false;
        getAngPosition();
        }
}

}