File content as of revision 2:c27b0654cffd:

#include "mbed.h"
#include "EthernetInterface.h"
#include "rtos.h"
#include "QEI.h"
#include <vector>
#include <cmath>

//Master or Slave? 1=Master, 0=Slave
static const int identity = 0;

//network config
static const char* master_ip = "192.168.1.101";
static const char* slave_ip = "192.168.1.102";
static const char* MASK = "255.255.255.0";
static const char* GATEWAY = "192.168.1.1";
static const int port = 865;

//declaration of interfaces

DigitalOut led(LED_GREEN); 
DigitalOut led2(LED_RED);
EthernetInterface eth;  //network 
Serial pc(USBTX, USBRX);//create PC interface
UDPSocket socket;       //socket to receive data on
Endpoint client;        //The virtual other side, not to send actual information to
Endpoint counterpart;   //The actual other side, this is where the information should go to
Ticker mainloop;

QEI M1(D3,D2,NC,1024,QEI::X4_ENCODING);

//variables 
char data[30]= {""};
int size;
int counter = 1;
int counter_received = 1;
char * var;
char output[30] = {""};
float input = 0.0;
float angle = 0.0;
float prev_angle = 0.0;
float torqueTL = 0.0;

bool main_loop_check = 0;
const float looptime  = 1.0/50; //50Hz

const float Ktl = 20;
const float Dtl=0.5;

const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
const float RadsPerCount = (2 * PI)/(1024*10);


int frequency_pwm = 10000;

float timedelay = 0.20; //SECONDS
std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0);


void inet_eth(){
    if(identity==1)
        {
        eth.init(master_ip, MASK,GATEWAY);
        eth.connect();
    
        socket.bind(port);
        counterpart.set_address(slave_ip,port);
        }
    else if(identity==0)
        {
        eth.init(slave_ip, MASK,GATEWAY);
        eth.connect();
    
        socket.bind(port);
        counterpart.set_address(master_ip,port);   
        }

    }

void inet_USB(){
    pc.baud(115200);
    }
    

void end_eth(){
    socket.close();
    eth.disconnect();
    }

void mainlooptrigger()
    {
    main_loop_check = 1;
    }
    
float update_delay(std::vector<float>&array, float new_value)
    {
        float return_value = array[1];
        for (int i=0; i<array.size()-1; ++i) 
        {
            array[i]=array[i+1];
        }
        array.back() = new_value;
        return return_value;
    }

void receiveUDP(void const *argument){
    while(true){
        size = socket.receiveFrom(client, data, sizeof(data));
        if(size > 0){
            data[size] = '\0';
            if(size>5) //first check, an minimum amount of data must have arrived
            {
                var = strtok(data,"; ");
                if(counter_received < atof(var)) //second check, data must be newer
                {
                    counter_received = atof(var);
                    var = strtok(NULL,"; ");
                    input = atof(var);
                    //pc.printf("data: %s \n\r",data);
                }
            }
            //pc.printf("data: %s \n\r",data);
            }
        }
    }

osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);

int main(){
    inet_eth();
    inet_USB();
    
    osThreadCreate(osThread(receiveUDP), NULL);
    led2=1;
    led=1;
    mainloop.attach(&mainlooptrigger,looptime);
    
    while(true){
        
        if(main_loop_check==1){
            angle = M1.getPulses()*RadsPerCount;
            float reference = update_delay(delayArrayINPUT,input);
            
            torqueTL = Ktl*(reference-angle) - Dtl * (angle-prev_angle) / looptime;
            
            pc.printf("Torque: %f\r\n",torqueTL);
            sprintf(output, "%i;%f",counter,angle);
            socket.sendTo(counterpart, output, sizeof(output));
            counter = counter + 1;
            led=!led;
            main_loop_check = 0;
            prev_angle = angle;
            }
        osDelay(1);
        }  
}