File content as of revision 4:610b5051182a:

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

//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;

//Motor interfaces and variables
InterruptIn EncoderA(D2);
DigitalIn EncoderB(D3);
DigitalOut M1_DIR(D4);
PwmOut M1_pwm(D5);

AnalogIn measuredForce(A5);


//Low pass filter filter coeffs, 2nd order, 50 Hz
double b[3] = {0.020083365564211, 0.020083365564211*2, 0.020083365564211};
double a[3] = {1.00000000000000, -1.561018075, 0.64135153805};


//variables related to networking
char data[30]= {""};
int size;
int counter = 1;
int counter_received = 1;
char * var;
char output[30] = {""};
float input = 0.0;

//measured variables
float angle = 0.0;
long encoderPos = 0;
long prev_encoderPos = 0;
float encoder_vel = 0.0;
float motor_vel = 0.0;
float force = 0.0;

//Reference, used in admittance
float ref_angle = 0.0;
float ref_vel = 0.0;
float ref_acc = 0.0;
const float virtualMass = 0.015;
const float virtualDamping = 1;
const float virtualStiffness = 5;
const float arm = 0.05;


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

enum States { stateCalibration, stateHoming, stateOperation};
int currentState = stateCalibration;

//Controller parameters
const float Ktl = 4; //high level controller parameters
const float Dtl=0.1;

const float Kp = 5; //low level controller parameters
const float Dp = 0.05;

float u = 0.0;

//Constants
const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
const float RadsPerCount = (2 * PI)/(1024*10);
const float FORCESENSORGAIN = 15.134;
const int MAX_ENCODER_LEFT = 1333;
const int MAX_ENCODER_RIGHT = -1453;
const float WORKSPACEBOUND = PI/8;

int frequency_pwm = 20000;

//Time delay related variables
float timedelay = 0.04; //SECONDS
std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0);

Timer t;

//FUNCTIONS START HERE

void encoderFunctionA()
    {
        if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
            encoderPos++;
        } else {
            encoderPos--;
        }
    }
double velocityFilter(float x)
{
    double y = 0.0; //Output
    static double y_1 = 0.0; //Output last loop
    static double y_2 = 0.0; //Output 2 loops ago
    static double x_1 = 0.0; //Input last loop
    static double x_2 = 0.0; //Input two loops ago

    //Finite difference equation for 2nd order Butterworth low-pass
    y = -a[1]*y_1 - a[2]*y_2 + x*b[0] + x_1*b[1] + x_2*b[2];
    y_2 = y_1;
    y_1 = y;
    x_2 = x_1;
    x_1 = x;
    return (float)y;
}

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 limit(float &x, float lower, float upper)
{
    if (x > upper)
        x = upper;
    if (x < lower)
        x = lower; 
}

void motor_update(float PWM) //angle required to safeguard it from crashing into its stops
{
    limit(PWM,-1.0f,1.0f);
    if(PWM >= 0.0f)
    {
        M1_DIR = false;
        M1_pwm = PWM;
    } else {
        M1_DIR = true;
        M1_pwm = -PWM;    
    }
}

void sensorUpdate()
{
    angle = encoderPos * RadsPerCount;
    encoder_vel = (encoderPos - prev_encoderPos)/looptime; //careful, this function should be called every 1/50 seconds
    motor_vel = velocityFilter(encoder_vel * RadsPerCount);
    prev_encoderPos = encoderPos;
    force = -FORCESENSORGAIN*2.0f*(measuredForce - 0.5f); //Measured force
}    
 
void doCalibration()
{
    u = 0.09;
    motor_update(u);
    led2=1;
    led=0;
    //switching states
    if((abs(motor_vel)<0.001f)&& t.read()>1.0)
    {
        encoderPos = MAX_ENCODER_LEFT;
        ref_angle = encoderPos * RadsPerCount;
        currentState = stateHoming;
        t.stop();
    }    
}

void doHoming()
{
    led2=0;
    led=0;
    ref_vel = -0.2;
    if(ref_angle > 0.0)
    {
        ref_angle += ref_vel*looptime; //careful, this function should be called every 1/50 seconds
    }
    else
    {
        ref_angle = 0.0;
        ref_vel = 0.0;
    }
    u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
    motor_update(u);
    
    
    
    //switching states
    if ((abs(encoderPos)<20)&&abs((ref_vel - motor_vel)<0.02))
    {
        currentState = stateOperation;
        motor_update(0.0);
        led=1;
    }
        
}

void doOperation()
{
    float reference = update_delay(delayArrayINPUT,input);
    
    u = 0.59*(Ktl*(reference-angle) - Dtl*motor_vel);
    
    motor_update(u);
    
    sprintf(output, "%i;%f",counter,angle);
    socket.sendTo(counterpart, output, sizeof(output));
    counter ++;
    led=!led;   
}

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("input: %f \n\r",input);
                    }
            }
        }
    }
}

osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);

int main(){
    inet_eth();
    inet_USB();
    
    osThreadCreate(osThread(receiveUDP), NULL);
    led2=1;
    led=1;
    mainloop.attach(&mainlooptrigger,looptime);
    M1_pwm.period(1.0/frequency_pwm);
    EncoderA.rise(&encoderFunctionA);
    
    t.start();
    
    while(true){
        if(main_loop_check==1){
            sensorUpdate();
            switch(currentState)
            {
                case stateCalibration:
                doCalibration();
                break;
                case stateHoming:
                doHoming();
                break;
                case stateOperation:
                doOperation();
                break;
            }
            
            main_loop_check = 0;
            }
        osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
        }  
}