Diff: main.cpp

Revision:

5:4d5b077b3fe6

Parent:

4:610b5051182a

Child:

6:ccbbf4c77d35

--- a/main.cpp	Fri May 11 08:25:05 2018 +0000
+++ b/main.cpp	Fri May 11 11:15:51 2018 +0000
@@ -2,6 +2,7 @@
 #include "EthernetInterface.h"
 #include "rtos.h"
 #include <vector>
+#include "FastPWM.h"
 
 //Master or Slave? 1=Master, 0=Slave
 static const int identity = 0;
@@ -23,13 +24,14 @@
 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 controllerloop;
 Ticker mainloop;
 
 //Motor interfaces and variables
 InterruptIn EncoderA(D2);
 DigitalIn EncoderB(D3);
 DigitalOut M1_DIR(D4);
-PwmOut M1_pwm(D5);
+FastPWM M1_pwm(D5);
 
 AnalogIn measuredForce(A5);
 
@@ -50,30 +52,32 @@
 
 //measured variables
 float angle = 0.0;
-long encoderPos = 0;
-long prev_encoderPos = 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;
 
 //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 virtualMass = 0.03;
+const float virtualDamping = 0.1;
 const float virtualStiffness = 5;
 const float arm = 0.05;
 
 
-bool main_loop_check = 0;
-const float looptime  = 1.0/50; //50Hz
+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;
 
 //Controller parameters
-const float Ktl = 4; //high level controller parameters
+const float Ktl = 1; //high level controller parameters
 const float Dtl=0.1;
 
 const float Kp = 5; //low level controller parameters
@@ -87,7 +91,7 @@
 const float FORCESENSORGAIN = 15.134;
 const int MAX_ENCODER_LEFT = 1333;
 const int MAX_ENCODER_RIGHT = -1453;
-const float WORKSPACEBOUND = PI/8;
+const float WORKSPACEBOUND = PI/6;
 
 int frequency_pwm = 20000;
 
@@ -154,9 +158,9 @@
     eth.disconnect();
     }
 
-void mainlooptrigger()
+void controllertrigger()
     {
-        main_loop_check = 1;
+        controller_check = 1;
     }
     
     
@@ -195,7 +199,7 @@
 void sensorUpdate()
 {
     angle = encoderPos * RadsPerCount;
-    encoder_vel = (encoderPos - prev_encoderPos)/looptime; //careful, this function should be called every 1/50 seconds
+    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
@@ -224,7 +228,7 @@
     ref_vel = -0.2;
     if(ref_angle > 0.0)
     {
-        ref_angle += ref_vel*looptime; //careful, this function should be called every 1/50 seconds
+        ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds
     }
     else
     {
@@ -241,23 +245,46 @@
     {
         currentState = stateOperation;
         motor_update(0.0);
-        led=1;
+        led2=1;
     }
         
 }
 
 void doOperation()
 {
-    float reference = update_delay(delayArrayINPUT,input);
-    
-    u = 0.59*(Ktl*(reference-angle) - Dtl*motor_vel);
-    
+    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);
-    
-    sprintf(output, "%i;%f",counter,angle);
-    socket.sendTo(counterpart, output, sizeof(output));
-    counter ++;
-    led=!led;   
+}
+
+void loopfunction()
+{
+    sensorUpdate();
+    switch(currentState)
+    {
+        case stateCalibration:
+        doCalibration();
+        break;
+        case stateHoming:
+        doHoming();
+        break;
+        case stateOperation:
+        doOperation();
+        break;
+    }
 }
 
 void receiveUDP(void const *argument){
@@ -291,29 +318,33 @@
     osThreadCreate(osThread(receiveUDP), NULL);
     led2=1;
     led=1;
-    mainloop.attach(&mainlooptrigger,looptime);
+    
+    //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);
     
     t.start();
     
     while(true){
-        if(main_loop_check==1){
-            sensorUpdate();
-            switch(currentState)
-            {
-                case stateCalibration:
-                doCalibration();
-                break;
-                case stateHoming:
-                doHoming();
-                break;
-                case stateOperation:
-                doOperation();
-                break;
-            }
+        if(controller_check==1){
+            float reference = update_delay(delayArrayINPUT,input);
+            control_torque = Ktl*(reference-angle) - Dtl*motor_vel;
             
-            main_loop_check = 0;
+            sprintf(output, "%i;%f",counter,angle);
+            socket.sendTo(counterpart, output, sizeof(output));
+            counter ++;
+            led=!led;   
+            controller_check = 0;
+            //pc.printf("force: %f \n\r",force);
             }
         osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
         }