File content as of revision 10:54b66bd1db20:

#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 MOTOR1_KI = 1.0;
const double CONTROLLER_TS = 0.01;
double m1_err_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 P(volatile double error, const double Kp){
    return Kp * error;
    }

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
}
  
 // 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 = P(error1,MOTOR1_KP) ;
// scope.set(velocityChannel,proportionalErrormotor1);
//scope.send();
 }
  
  
void control(double motor1)
{
if(abs(motor1)>0.3)
        {
        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);

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();
        }
}
}