Diff: main.cpp

Revision:

8:1655d27152e6

Parent:

7:15e6fc689368

Child:

9:56aed8c6779f

--- a/main.cpp	Thu May 16 20:42:39 2019 +0000
+++ b/main.cpp	Fri May 17 14:35:54 2019 +0000
@@ -70,8 +70,11 @@
     pc.printf("Siamo accesi\n\r");
     wait(1);
     
-    linkspedal.sendBuffer(156,4900);
-    rechtspedal.sendBuffer(156,4900);
+    linkspedal.sendBuffer(156,5000);
+    rechtspedal.sendBuffer(156,5000);
+    wait(0.05);
+    linkspedal.sendBuffer(154,int(80*40.96));
+    rechtspedal.sendBuffer(154,int(80*40.96));
     
     linkspedal.sendBuffer(495,0);
     rechtspedal.sendBuffer(495,0);
@@ -99,7 +102,7 @@
     float PedaleFreq = 0.0f,PedaleFreqOld = 0.0f, PedaleFreqFil, PedaleFreqFilOld=0.0f; // in rad/s
     
     // PID instance
-    float Kp = 1.0, Ki = 20.0, Kd = 0.0;
+    float Kp = 1.0, Ki = 30.0, Kd = 0.0;
     PID pid(Kp, Ki, Kd, dt);
     pid.setInputLimits(0.0,200.0);
     pid.setOutputLimits(1.0,100.0);
@@ -122,7 +125,7 @@
     rechtspedal.sendBuffer(495,0); 
     init = false;
 while(1){
-        while(i<4000){
+       // while(i<4000){
 
             
             // Measure data form sensors
@@ -157,7 +160,7 @@
             beta = 0.52f; // 30°
             phi = angle_rad;  
             R = sqrt(pow(a,2) * pow(sin(beta + phi),2) + pow(b,2) * pow(cos(beta + phi),2)); // Torque in function of the Ellipse parameters
-            daumenValue = 5;
+            daumenValue = 8;
             if(daumenValue < 1) daumenValue = 1;
             rec = ((float)daumenValue)+(R*pid.getCalculation());
             
@@ -178,11 +181,11 @@
             GegenMomRechts.write(0.02+(rec*0.008f)); // offset for 0.2V -> ~0.02
             GegenMomLinks.write(0.02+(rec*0.008f));  // offset for 0.2V -> ~0.02
 
-            
-           MessungP[i] = pid.getSetPoint();
+           /* 
+           MessungP[i] = F_Value;
            MessungFreq[i] = F_RPM;
            MessungPID[i] = rec;
-
+            */
             // Store Old Values
             angleOld = angle_rad;
             F_ValueOld = F_Value;
@@ -190,14 +193,14 @@
             F_AccOld = F_Acc;
             i++;
             wait(0.005);// Set to 5 ms
-       }
+      /* }
        printf("[");
        for(i=0;i<3999;i++){
             printf("%.3f,%.3f,%.3f;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i]);
        }
        printf("%.3f,%.3f,%.3f];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999]);
        i=0; 
-       
+       */
 }