Diff: main.cpp

Revision:

10:b871d1b05787

Parent:

9:08a7a8e59a6a

Child:

11:94a4dd7ed05c

--- a/main.cpp	Mon Oct 14 09:06:08 2019 +0000
+++ b/main.cpp	Mon Oct 14 14:28:37 2019 +0000
@@ -8,6 +8,8 @@
 // Button and potmeter1 control
 InterruptIn button1(D11);
 InterruptIn button2(D10);
+InterruptIn buttonsw2(SW2);
+InterruptIn buttonsw3(SW3);
 AnalogIn potmeter1(A0);
 AnalogIn potmeter2(A1);
 AnalogIn potmeter3(A2);
@@ -32,17 +34,16 @@
 //Motorcontrol
 bool motordir;
 double motorpwm;
+double premotorpwm;
 float u1= 0;
 double u2;
 double potValue;
 double pi2= 6.283185;
 float e; //e = error
-float Kp;
+float Kp=0.49;
 float Ki;
-float Kd;
 float u_k;
 float u_i;
-float u_d;
 
 //Hidscope
 HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
@@ -69,28 +70,18 @@
 float PID_controller(float e){
     static float error_integral=0;
     static float e_prev=e;
-    static BiQuad LowPassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
     
     //Proportional part:
-    //Kp=2*potmeter1.read();
-    Kp=101;
+    Kp=potmeter1.read();
     u_k=Kp*e;
     
     //Integral part
-    Ki=10;
+    Ki=potmeter2.read();
     error_integral=error_integral+e*Ts;
     u_i=Ki*error_integral;
     
-    //Derivative part
-    //Kd=2*potmeter2.read(); 
-    Kd=10;
-    float error_derivative =(e-e_prev)/Ts;
-    float filtered_error_derivative = LowPassFilter.step(error_derivative);
-    u_d=Kd*filtered_error_derivative;
-    e_prev=e;
-    
     // Sum and return
-    return u_k+u_i+u_d;    
+    return u_k+u_i;    
 }
 
 
@@ -103,12 +94,15 @@
 }
 
 void togglehoek(){
-    u1= 0.25*pi2+u1;
+    static float t = 0;
+    u1= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
+    t+=0.01;
     }
     
 void motorControl()
 {
-    button1.fall(&togglehoek);
+    togglehoek();
+    //button1.fall(&togglehoek);
     angle = counts * factorin / gearratio; // Angle of motor shaft in rad
     omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     potValue= potmeter1.read();
@@ -117,17 +111,20 @@
     
     u2=PID_controller(e);
     
-        motorpwm= abs(u2);
+    premotorpwm= fabs(u2);
+    if (premotorpwm>1.0){
+        motorpwm=1;}
+    else {
+        motorpwm=premotorpwm;}    
     if (u2<0){
         motordir= 0;}
     else {
          motordir= 1;}
-    motor1Power.pulsewidth(motorpwm * PWM_period*motor1Toggle );
+    motor1Power.write(motorpwm);
     motor1Direction= motordir;
 }
 
-void Plotje()
-{
+void Plotje(){
     scope.set(0,u1); //gewenste hoek
     scope.set(1,angle); //Gemeten hoek
     scope.set(2,e); //verschil in gewenste en gemeten hoek
@@ -158,7 +155,7 @@
         //pc.printf("Counts: %i   DeltaCounts: %i\r\n", counts, deltaCounts);
         pc.printf("Angle:  %f   Omega:       %f\r\n", angle, omega);
         pc.printf("U1: %f   Error:  %f     \r\n",u1, e);
-        pc.printf("Kp: %f   Kd: %f   Ki:    %f \r\n", Kp, Kd, Ki);
+        pc.printf("Kp: %f   Ki:    %f \r\n", Kp, Ki);
         
         wait(0.5);
     }