Diff: main.cpp

Revision:

4:610b5051182a

Parent:

3:376fccdc7cd6

Child:

5:4d5b077b3fe6

--- a/main.cpp	Tue May 08 13:14:18 2018 +0000
+++ b/main.cpp	Fri May 11 08:25:05 2018 +0000
@@ -25,14 +25,21 @@
 
 Ticker mainloop;
 
-
+//Motor interfaces and variables
 InterruptIn EncoderA(D2);
 DigitalIn EncoderB(D3);
-
 DigitalOut M1_DIR(D4);
 PwmOut M1_pwm(D5);
 
-//variables 
+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;
@@ -40,25 +47,56 @@
 char * var;
 char output[30] = {""};
 float input = 0.0;
+
+//measured variables
 float angle = 0.0;
-float prev_angle = 0.0;
+long encoderPos = 0;
+long prev_encoderPos = 0;
+float encoder_vel = 0.0;
+float motor_vel = 0.0;
+float force = 0.0;
 
-int encoderPos = 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
 
-const float Ktl = 4;
+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;
 
-float timedelay = 0.20; //SECONDS
+//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()
@@ -69,7 +107,22 @@
             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)
@@ -103,9 +156,10 @@
 
 void mainlooptrigger()
     {
-    main_loop_check = 1;
+        main_loop_check = 1;
     }
     
+    
 float update_delay(std::vector<float>&array, float new_value)
     {
     float return_value = array[1];
@@ -125,54 +179,108 @@
         x = lower; 
 }
 
-void motor_update(float angle, float &torque) //angle required to safeguard it from crashing into its stops
+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)
     {
-        //if((angle>PI/4)&&(torque>0.0))
-//            {
-//            torque = 0;
-//            }
-//        if((angle<-PI/4)&&(torque<0.0))
-//             {
-//            torque = 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;
+    }
         
-        float PWM = torque*0.58; //converting torque to PWM.
-        pc.printf("PWM: %f\r\n",PWM);
-        limit(PWM,-1.0f,1.0f);
-        //pc.printf("PWM: %f\r\n",PWM);
-        if(PWM >= 0.0f)
-        {
-            M1_DIR = false;
-            M1_pwm = PWM;
-        } else {
-            M1_DIR = true;
-            M1_pwm = -PWM;    
-        }
-        
-        
-    }
+}
+
+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){
+    while(true)
+    {
         size = socket.receiveFrom(client, data, sizeof(data));
-        if(size > 0){
+        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);
-                    //MORE CHECKS NEED TO BE IMPLEMENTED
-                }
-            }
+                    pc.printf("input: %f \n\r",input);
+                    }
             }
         }
     }
+}
 
 osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);
 
@@ -183,31 +291,30 @@
     osThreadCreate(osThread(receiveUDP), NULL);
     led2=1;
     led=1;
-    
     mainloop.attach(&mainlooptrigger,looptime);
+    M1_pwm.period(1.0/frequency_pwm);
+    EncoderA.rise(&encoderFunctionA);
     
-    M1_pwm.period(1.0/frequency_pwm);
-    
-    EncoderA.rise(&encoderFunctionA);
+    t.start();
     
     while(true){
         if(main_loop_check==1){
-            angle = encoderPos*RadsPerCount;
-            //pc.printf("Angle: %f\r\n",angle);
-            float reference = update_delay(delayArrayINPUT,input);
-            
-            float torqueTL = Ktl*(reference-angle)- Dtl * (angle-prev_angle) / looptime; 
-            
-            motor_update(angle,torqueTL);
+            sensorUpdate();
+            switch(currentState)
+            {
+                case stateCalibration:
+                doCalibration();
+                break;
+                case stateHoming:
+                doHoming();
+                break;
+                case stateOperation:
+                doOperation();
+                break;
+            }
             
-            //pc.printf("Torque: %f\r\n",torqueTL);
-            sprintf(output, "%i;%f",counter,angle);
-            socket.sendTo(counterpart, output, sizeof(output));
-            counter ++;
-            led=!led;
             main_loop_check = 0;
-            prev_angle = angle;
             }
-        osDelay(1);
+        osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
         }  
 }    
\ No newline at end of file