Diff: main.cpp

Revision:

6:ccbbf4c77d35

Parent:

5:4d5b077b3fe6

Child:

7:984f363f5e87

--- a/main.cpp	Fri May 11 11:15:51 2018 +0000
+++ b/main.cpp	Tue May 15 11:28:18 2018 +0000
@@ -3,9 +3,10 @@
 #include "rtos.h"
 #include <vector>
 #include "FastPWM.h"
+#include <cmath>
 
 //Master or Slave? 1=Master, 0=Slave
-static const int identity = 0;
+static const int identity = 1;
 
 //network config
 static const char* master_ip = "192.168.1.101";
@@ -15,7 +16,6 @@
 static const int port = 865;
 
 //declaration of interfaces
-
 DigitalOut led(LED_GREEN); 
 DigitalOut led2(LED_RED);
 EthernetInterface eth;  //network 
@@ -23,24 +23,23 @@
 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);
+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;
@@ -58,32 +57,47 @@
 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.03;
-const float virtualDamping = 0.1;
+const float virtualMass = 0.005;
+const float virtualDamping = 0.05;
 const float virtualStiffness = 5;
-const float arm = 0.05;
+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};
+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;
 
-float u = 0.0;
+//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;
@@ -93,12 +107,15 @@
 const int MAX_ENCODER_RIGHT = -1453;
 const float WORKSPACEBOUND = PI/6;
 
-int frequency_pwm = 20000;
+const int frequency_pwm = 20000;
 
 //Time delay related variables
 float timedelay = 0.04; //SECONDS
-std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0);
-
+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
@@ -202,17 +219,17 @@
     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 - 0.5f); //Measured force
+    force = -FORCESENSORGAIN*2.0f*(measuredForce - force_offset); //Measured force
 }    
  
 void doCalibration()
 {
     u = 0.09;
     motor_update(u);
-    led2=1;
-    led=0;
+    led2=0;
+    led=1;
     //switching states
-    if((abs(motor_vel)<0.001f)&& t.read()>1.0)
+    if((abs(motor_vel)<0.001f)&& t.read()>3.0f)
     {
         encoderPos = MAX_ENCODER_LEFT;
         ref_angle = encoderPos * RadsPerCount;
@@ -226,7 +243,7 @@
     led2=0;
     led=0;
     ref_vel = -0.2;
-    if(ref_angle > 0.0)
+    if(ref_angle > 0.0f)
     {
         ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds
     }
@@ -238,14 +255,14 @@
     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))
+    if ((abs(encoderPos)<20)&&abs((ref_vel - motor_vel)<0.02f))
     {
         currentState = stateOperation;
+        force_offset = measuredForce;
         motor_update(0.0);
         led2=1;
+        led=1;
     }
         
 }
@@ -268,6 +285,8 @@
     }
     u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);       
     motor_update(u);
+    
+    //no switching states for now
 }
 
 void loopfunction()
@@ -287,6 +306,62 @@
     }
 }
 
+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)
     {
@@ -294,6 +369,7 @@
         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,"; ");
@@ -301,8 +377,13 @@
                     {
                     counter_received = atof(var);
                     var = strtok(NULL,"; ");
+                    received_transparancy = atof(var);
+                    var = strtok(NULL,"; ");
+                    received_passivity = atof(var);
+                    var = strtok(NULL,"; ");
                     input = atof(var);
-                    pc.printf("input: %f \n\r",input);
+                    var = strtok(NULL,"; ");
+                    received_package += atof(var);
                     }
             }
         }
@@ -319,32 +400,105 @@
     led2=1;
     led=1;
     
-    //Set all interrupt requests to 2nd place priority
+   //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){
-            float reference = update_delay(delayArrayINPUT,input);
-            control_torque = Ktl*(reference-angle) - Dtl*motor_vel;
+            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
             
-            sprintf(output, "%i;%f",counter,angle);
+            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;   
+            led=!led;
+            if(current_passivity==1)
+                led2=0;
+            else
+                led2=1;   
             controller_check = 0;
-            //pc.printf("force: %f \n\r",force);
+            debug = 0;
             }
         osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
         }