File content as of revision 6:ccbbf4c77d35:

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

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

//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
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[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;
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 virtualMass = 0.005;
const float virtualDamping = 0.05;
const float virtualStiffness = 5;
const float arm = 0.07;

//Controller required variables 
bool controller_check = 0;
const float looptime  = 1.0/50; //50Hz, for the controller
const float ADDMlooptime = 1.0/5000; //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 = 1; //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;      //serves as input variable for the motor;

//passivity layer constant
const float beta =  0.15;
const float Hd = 0.5;
const float alpha = 10;

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

const int frequency_pwm = 20000;

//Time delay related variables
float timedelay = 0.04; //SECONDS
int delaysteps = timedelay/looptime;
std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0);
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--;
        }
    }
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 controllertrigger()
    {
        controller_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)/ADDMlooptime; //careful, this function should be called every 1/5000 seconds
    motor_vel = velocityFilter(encoder_vel * RadsPerCount);
    prev_encoderPos = encoderPos;
    force = -FORCESENSORGAIN*2.0f*(measuredForce - force_offset); //Measured force
}    
 
void doCalibration()
{
    u = 0.09;
    motor_update(u);
    led2=0;
    led=1;
    //switching states
    if((abs(motor_vel)<0.001f)&& t.read()>3.0f)
    {
        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.0f)
    {
        ref_angle += ref_vel*ADDMlooptime; //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.02f))
    {
        currentState = stateOperation;
        force_offset = measuredForce;
        motor_update(0.0);
        led2=1;
        led=1;
    }
        
}

void doOperation()
{
    ref_acc = (force*arm + control_torque- virtualDamping*ref_vel)/virtualMass;
    ref_vel += ref_acc*ADDMlooptime;
    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()
{
    sensorUpdate();
    switch(currentState)
    {
        case stateCalibration:
        doCalibration();
        break;
        case stateHoming:
        doHoming();
        break;
        case stateOperation:
        doOperation();
        break;
    }
}

void updateTransparency()
{
    transparancy++;
    if(transparancy>3)
        transparancy = 1;
}

void updatePassivity()
{
    passivity++;
    if(passivity>1)
        passivity = 0;    
}

float passivityLayer(float Ftl, float E_in)
{
    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)
    {
        float FMAX1 = abs(Ftl);
    }
    float FMAX2 = abs(tank/(ref_vel*looptime));
    
    float FMAX3 = 0.15;
    
    prev_ref_angle = ref_angle;  
    
    float Ftlc = 0.0;
    
    if((tank<Hd)&&(identity==1))
    {
        Ftlc = - alpha*(Hd-tank)*ref_vel;
    }    
    //pc.printf("Ftlc: %f\r\n",Ftlc);
    //pc.printf("tank: %f\r\n",tank);
    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 to 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 receiveUDP(void const *argument){
    while(true)
    {
        size = socket.receiveFrom(client, data, sizeof(data));
        if(size > 0)
        {
            data[size] = '\0';
            pc.printf("data:%s\r\n",data);
            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,"; ");
                    received_transparancy = atof(var);
                    var = strtok(NULL,"; ");
                    received_passivity = atof(var);
                    var = strtok(NULL,"; ");
                    input = atof(var);
                    var = strtok(NULL,"; ");
                    received_package += atof(var);
                    }
            }
        }
    }
}

osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);

int main(){
    inet_eth();
    inet_USB();
    
    osThreadCreate(osThread(receiveUDP), NULL);
    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.fall(&updateTransparency);
    Button2.fall(&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;
                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);                   
                }
                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;                 
                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out); 
                }
                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;                  
                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
                }   
            }
            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;                    
                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
                }
                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;                   
                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
                }
                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;                    
                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out);
                }
            }
            
            socket.sendTo(counterpart, output, sizeof(output));
            counter ++;
            led=!led;
            if(current_passivity==1)
                led2=0;
            else
                led2=1;   
            controller_check = 0;
            debug = 0;
            }
        osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
        }  
}