File content as of revision 13:787cabccb2be:

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


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

//network config
static const char* master_ip = "192.168.1.101"; //only for direct communication, otherwise set it up using router tables
static const char* slave_ip = "192.168.1.102";  //only for direct communication, otherwise set it up using router tables
static const char* MASK = "255.255.255.0";
static const char* GATEWAY = "192.168.1.1";
static const char* laptop_IP = "192.168.1.103"; //only for direct communication, otherwise set it up using router tables
static const int port = 865;


//declaration of interfaces
DigitalOut led(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
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
Endpoint laptop;        //Interface to send information for data-storage
InterruptIn Button1(SW2);
InterruptIn Button2(SW3);
Ticker controllerloop;
Ticker mainloop;
DigitalOut debug(D11);

//Motor interfaces and variables
InterruptIn EncoderA(D2);
DigitalIn  EncoderB(D3);
DigitalOut M1_DIR(D4);
FastPWM 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[25]= {""};
char output[25] = {""};
char status[21] = {""};
int size;
unsigned int counter = 1;
unsigned int counter_received = 1;
unsigned int check = 1;
unsigned int counter_not_missed = 0;
float percentage_received = 0.0;
float input = 0.0;

//measured variables
float angle = 0.0;
int encoderPos = 0;
int prev_encoderPos = 0;
float encoder_vel = 0.0;
float motor_vel = 0.0;
float force = 0.0;
float control_torque = 0.0;
float force_offset = 0.5;

//Reference, used in admittance
float ref_angle = 0.0;
float ref_vel = 0.0;
float ref_acc = 0.0;
const float virtualInertia = 0.03;
const float virtualDamping = 0.1;
const float arm = 0.085;
const float max_velocity = 0.8;

//Controller required variables
bool controller_check = 0;
const float sample_frequency = 200;
float looptime  = 1.0/(sample_frequency); //200Hz, for the controller
const float ADDMlooptime = 1.0/2500; //2.5KHz, for the admittance controller
enum States {stateCalibration, stateHoming, stateOperation};
int currentState = stateCalibration;
int transparancy = 1; // 1=Pos-Pos, 2=Pos-Force, 3=Force-Pos
int passivity =0; // 0=off, 1=on
int received_transparancy = 1;
int received_passivity = 0;

//Controller parameters
const float Ktl = 4.5;  //high level controller parameters
const float Dtl=0.1;
const float Kp = 100;   //low level controller parameters
const float Dp = 0.5;
float u = 0.0;          //serves as input variable for the motor;

//passivity layer constant
const float beta =  0.2041;
const float Hd = 0.2778;
const float alpha = 0.5711;

//passivity layer variables
float tank = 0.0;
float prev_ref_angle = 0.0;
float package_out = 0.0;
float received_package = 0.0;
float prev_torque = 0.0;


//Constants
const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679; //Why so much accuracy? Because why not?
const float RadsPerCount = (2 * PI)/(1024*10);
const float FORCESENSORGAIN = 15.134;
const int MAX_ENCODER_LEFT = 1333; //dependent on setup. PLEASE CHECK
const int MAX_ENCODER_RIGHT = -1453; //dependent on setup. PLEASE CHECK
const float WORKSPACEBOUND = PI/6;

const int frequency_pwm = 20000;

//Time delay related variables
float timedelay = 0.1;//SECONDS
int delaysteps = timedelay/looptime; 
std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0); //creating a array that delays the input by x steps, so that time-delay is properly simulated
std::vector<float> delayArrayMODE(max(delaysteps,1),0.0);
std::vector<float> delayArrayPASS(max(delaysteps,1),0.0);
std::vector<float> delayArrayENERGY(max(delaysteps,1),0.0);
Timer t;

//FUNCTIONS START HERE

void encoderFunctionA()
{
    if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
        encoderPos++;
    } else {
        encoderPos--;
    }
}
float 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() //this function starts the ethernet connection op, including the ipadresses. PLEASE STICK TO STATIC ADDRESSING.
{
    if(identity==1) {
        eth.init(master_ip, MASK,GATEWAY);
        eth.connect();

        socket.bind(port);
        counterpart.set_address(slave_ip,port);
        laptop.set_address(laptop_IP,port);
    } else if(identity==0) {
        eth.init(slave_ip, MASK,GATEWAY);
        eth.connect();

        socket.bind(port);
        counterpart.set_address(master_ip,port);
        laptop.set_address(laptop_IP,port+1);
    }

}

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


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

void controllertrigger()
{
    controller_check = 1;
}


float update_delay(std::vector<float>&array, float new_value)
{
    float return_value = array[0];
    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)//motor function 
{
    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)/ADDMlooptime; //careful, this function should be called every 1/2500 seconds
    motor_vel = velocityFilter(encoder_vel * RadsPerCount);
    prev_encoderPos = encoderPos;
    force = (-FORCESENSORGAIN*2.0f*(measuredForce - force_offset)); //Measured force
}

void doCalibration() //first state. Move  maximally left, until the stops are hit. 
{
    u = -0.15;
    motor_update(u);
    led2=0;
    led=1;
    //switching states
    if((abs(motor_vel)<0.001f)&& t.read()>3.0f) {
        encoderPos = MAX_ENCODER_RIGHT - (1-identity)*180; //to make up for the difference in setups. Make a better way then this. 
        ref_angle = encoderPos * RadsPerCount;
        currentState = stateHoming;
        t.stop();
    }
}

void doHoming() //second state. Move to up. 
{
    led2=0;
    led=0;
    ref_vel = 0.2;
    if(ref_angle < 0.0f) {
        ref_angle += ref_vel*ADDMlooptime; 
    } 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.02f)) {
        currentState = stateOperation;
        force_offset = measuredForce;
        motor_update(0.0);
        led2=1;
        led=1;
    }

}

void doOperation() //final, and infinite state. Do the teleoperation. 
{
    ref_acc = (force*arm + control_torque- virtualDamping*ref_vel)/virtualInertia;
    ref_vel += ref_acc*ADDMlooptime;
    if(ref_vel>max_velocity)
        ref_vel=max_velocity;
    else if(ref_vel<-max_velocity)
        ref_vel=-max_velocity;


    ref_angle += ref_vel*ADDMlooptime;
    if(ref_angle > WORKSPACEBOUND) {
        ref_vel = 0.0;
        ref_acc = 0.0;
        ref_angle = WORKSPACEBOUND;
    } else if(ref_angle < -WORKSPACEBOUND) {
        ref_vel = 0.0;
        ref_acc = 0.0;
        ref_angle = -WORKSPACEBOUND;
    }
    u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
    motor_update(u);

    //no switching states for now
}

void loopfunction() //state control
{
    sensorUpdate();
    switch(currentState) {
        case stateCalibration:
            doCalibration();
            break;
        case stateHoming:
            doHoming();
            break;
        case stateOperation:
            doOperation();
            break;
    }
}

void updateTransparency() //determining the transparency layer to use. 
{
    transparancy++;
    if(transparancy>3)
        transparancy = 1;
}

void updatePassivity()  //determining wether or not to use the passivity layer 
{
    passivity++;
    if(passivity>1)
        passivity = 0;
}

float passivityLayer(float Ftl, float E_in) //just see the report on this topic. Too much stuff to explain.
{
    tank = tank + E_in - prev_torque*(ref_angle-prev_ref_angle);
    if(tank>0.0f) {
        package_out = tank*beta;
        tank = tank - package_out;
    } else
        package_out = 0.0;

    float FMAX1 = 0.0;
    if(tank>0.0f) {
        FMAX1 = abs(Ftl);
    }
    float FMAX2 = abs(tank/(ref_vel*looptime));
    float FMAX3 = 20.0;
    if(identity==0) {
        FMAX3 = 0.7;
    }
    prev_ref_angle = ref_angle;

    float Ftlc = 0.0;

    if((tank<Hd)&&(identity==1)) {
        Ftlc = - alpha*(Hd-tank)*ref_vel;
    }
    if(Ftl>=0.0f) {
        prev_torque = min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
        return min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc; //min() only takes two arguments, so nested min()'s
    } else {
        prev_torque = -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
        return -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
    }

}

void generateOutput(float variable) //generate packet to send to counterpart
{
    memcpy(&output[0],&counter,4);
    memcpy(&output[4],&transparancy,4);
    memcpy(&output[8],&passivity,4);
    memcpy(&output[12],&variable,4);
    memcpy(&output[16],&package_out,4);
}
void generateStatus() //generate packet to send to data-storage
{
    memcpy(&status[0],&counter,4);
    memcpy(&status[4],&ref_angle,4);
    memcpy(&status[8],&force,4);
    memcpy(&status[12],&tank,4);
    memcpy(&status[16],&percentage_received,4);
}

void receiveUDP(void const *argument) //This is what it is doing, if no trigger is called. 
{
    while(true) {
        size = socket.receiveFrom(client, data, sizeof(data));
        if(size > 0) {
            data[size] = '\0';
            if(size>18) { //first check, an minimum amount of data must have arrived
                memcpy(&check,&data[0],4);
                if(counter_received < check) { //second check, data must be newer
                    if((counter_received == check-1)&&(counter_not_missed<100)) {
                        counter_not_missed++;
                    } else if((counter_not_missed)>10){
                        counter_not_missed = counter_not_missed + counter_received-check + 1;
                    }
                    counter_received=check;
                    memcpy(&received_transparancy,&data[4],4);
                    memcpy(&received_passivity,&data[8],4);
                    memcpy(&input,&data[12],4);
                    memcpy(&received_package,&data[16],4);
                    percentage_received = (float) counter_not_missed/100;
                    if(percentage_received<0.90) {
                        led3=1;
                    } else {
                        led3=0;
                    }
                }
            }
        }
    }
}

osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);

int main()
{
    osThreadCreate(osThread(receiveUDP), NULL);

    inet_eth();
    inet_USB();

    led2=1;
    led=1;

    //Set all interrupt requests to 2nd place priority
    for(int n = 0; n < 86; n++) {
        NVIC_SetPriority((IRQn)n,1);
    }
    //Set out motor encoder interrupt to 1st place priority
    NVIC_SetPriority(PORTB_IRQn,0);

    controllerloop.attach(&controllertrigger,looptime);
    mainloop.attach(&loopfunction,ADDMlooptime);

    M1_pwm.period(1.0/frequency_pwm);
    EncoderA.rise(&encoderFunctionA);

    Button1.rise(&updateTransparency);
    Button2.rise(&updatePassivity);

    t.start();

    while(true) {
        if(controller_check==1) {
            debug = 1;
            float received_input = update_delay(delayArrayINPUT,input);
            float current_transparancy = update_delay(delayArrayMODE,received_transparancy);
            float current_passivity = update_delay(delayArrayPASS,received_passivity);
            float package_in = update_delay(delayArrayENERGY,received_package);
            received_package = 0; //IMPORTANT, WILL EXPLODE OTHERWISE

            if(identity==0) {
                transparancy = current_transparancy;
                passivity = current_passivity;
                if(transparancy==1) {
                    float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
                    if(current_passivity==1)
                        control_torque = passivityLayer(torque_tlc,package_in);
                    else
                        control_torque = torque_tlc;
                    generateOutput(ref_angle);
                } else if(transparancy==2) {
                    float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
                    if(current_passivity==1)
                        control_torque = passivityLayer(torque_tlc,package_in);
                    else
                        control_torque = torque_tlc;
                    generateOutput(force);
                } else if(transparancy==3) {
                    float torque_tlc = received_input*arm;
                    if(current_passivity==1)
                        control_torque = passivityLayer(torque_tlc,package_in);
                    else
                        control_torque = torque_tlc;
                    generateOutput(ref_angle);
                }
            } else if(identity == 1) {
                if(transparancy==1) {
                    float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
                    if(current_passivity==1)
                        control_torque = passivityLayer(torque_tlc,package_in);
                    else
                        control_torque = torque_tlc;
                    generateOutput(ref_angle);
                } else if(transparancy==2) {
                    float torque_tlc = received_input*arm;
                    if(current_passivity==1)
                        control_torque = passivityLayer(torque_tlc,package_in);
                    else
                        control_torque = torque_tlc;
                    generateOutput(ref_angle);
                } else if(transparancy==3) {
                    float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
                    if(current_passivity==1)
                        control_torque = passivityLayer(torque_tlc,package_in);
                    else
                        control_torque = torque_tlc;
                    generateOutput(force);
                }
            }
            socket.sendTo(counterpart, output, sizeof(output));
            socket.sendTo(counterpart, output, sizeof(output));
            counter ++;
            generateStatus();
            socket.sendTo(laptop,status,sizeof(status));
            led=0;
            if(current_passivity==1)
                led2=0;
            else
                led2=1;

            controller_check = 0;
            debug = 0;
        }
        osDelay(0.1); // This line is key, otherwise the MBED will not switch to the receive() thread.
    }
}

// Also, parts of this code is questionable. As in I don't know why it works. It can be advised to find a library that can have a interrupt on the UPD receive. Or just don't use UDP. although internet.