Diff: main.cpp

Revision:

8:5c891d5ebe45

Parent:

7:984f363f5e87

Child:

9:16044ec419af

--- a/main.cpp	Wed May 16 13:53:12 2018 +0000
+++ b/main.cpp	Fri May 18 06:44:24 2018 +0000
@@ -88,8 +88,8 @@
 
 //passivity layer constant
 const float beta =  0.15;
-const float Hd = 0.5;
-const float alpha = 10;
+const float Hd = 0.075;
+const float alpha = 0.2;
 
 //passivity layer variables
 float tank = 0.0;
@@ -110,12 +110,12 @@
 const int frequency_pwm = 20000;
 
 //Time delay related variables
-float timedelay = 0.04; //SECONDS
+float timedelay = 0.08; //SECONDS
 int delaysteps = timedelay/looptime;
-std::vector<float> delayArrayINPUT(max(delaysteps,2),0.0);
-std::vector<float> delayArrayMODE(max(delaysteps,2),0.0);
-std::vector<float> delayArrayPASS(max(delaysteps,2),0.0);
-std::vector<float> delayArrayENERGY(max(delaysteps,2),0.0);
+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
@@ -183,7 +183,7 @@
     
 float update_delay(std::vector<float>&array, float new_value)
     {
-    float return_value = array[1];
+    float return_value = array[0];
     for (int i=0; i<array.size()-1; ++i) 
     {
         array[i]=array[i+1];
@@ -224,14 +224,14 @@
  
 void doCalibration()
 {
-    u = 0.09;
+    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_LEFT;
+        encoderPos = MAX_ENCODER_RIGHT;
         ref_angle = encoderPos * RadsPerCount;
         currentState = stateHoming;
         t.stop();
@@ -242,8 +242,8 @@
 {
     led2=0;
     led=0;
-    ref_vel = -0.2;
-    if(ref_angle > 0.0f)
+    ref_vel = 0.2;
+    if(ref_angle < 0.0f)
     {
         ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds
     }
@@ -333,7 +333,7 @@
     float FMAX1 = 0.0;
     if(tank>0.0f)
     {
-        float FMAX1 = abs(Ftl);
+        FMAX1 = abs(Ftl);
     }
     float FMAX2 = abs(tank/(ref_vel*looptime));
     
@@ -347,16 +347,19 @@
     {
         Ftlc = - alpha*(Hd-tank)*ref_vel;
     }    
+    pc.printf("%f,%f,%f,%f\r\n",tank,Ftl,min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3)),Ftlc);
     //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
-    }
+        //return 0.0;
+        }
     else
     {
         prev_torque = -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))-Ftlc;
+        //return 0.0;
         return -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))-Ftlc;   
     }