File content as of revision 11:eda4fbf91948:

#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(2);
DigitalOut led(D0);
//*******************Setting tickers and printers*******************
Ticker callMotor;
Ticker angPos;
Ticker t1;
Ticker t2;
Ticker t3;
Ticker myControllerTicker;
Ticker scopeTimer;
//**************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;
double m1_ts = 0.01;
const int velocityChannel = 0;
const double transmissionShoulder =94.4/40.2;
const double transmissionElbow = 1.0;
const double motor1_Kp = 1;
const double motor1_Ki = 0.1;
const double CONTROLLER_TS = 0.01;
double m1_error_int=0;
volatile double radians;
volatile double proportionalErrormotor1;
volatile double motor1;
//*****************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******************************
double PI(volatile double error, const double Kp, const double Ki, double Ts, double &error_int)
{
    error_int += Ts*error;
    return Kp * error + Ki*error_int;
    
    }

void getAngPosition() 
{   
    volatile int pulses = encoder.getPulses();
    radians = (pulses / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
//     scope.set(velocityChannel,pulses);
//     scope.send();
  
}
  
 // 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;
motor1 = PI(error1,motor1_Kp,motor1_Ki, m1_ts, m1_error_int);
pc.printf("%d\n\r",motor1);
// scope.set(velocityChannel,motor1);
//scope.send();
 }
  
  
void control(double motor1)
{
if(abs(motor1)>1.0)
        {
        motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
        }
else
        {
        motor1MagnitudePin=0.0;    
        }
if(motor1<=0)      
        {
        motor1DirectionPin=0.0;
        }
else    {
        motor1DirectionPin=1.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);
pc.baud(115200);
while(true)
{
        if(fn1_go)
        {
        fn1_go = false;
        control(motor1);
        }
        if(fn2_go)
        {
        fn2_go = false;
        motor1_Controller(radians);
        }
        if(fn3_go)
        {
        fn3_go = false;
        getAngPosition();
        }
}
}