Diff: main.cpp

Revision:

1:0122c72f6e1b

Parent:

0:aa8e05bc0533

diff -r aa8e05bc0533 -r 0122c72f6e1b main.cpp
--- a/main.cpp	Thu Feb 27 12:40:03 2020 +0000
+++ b/main.cpp	Thu Feb 27 13:10:57 2020 +0000
@@ -102,6 +102,7 @@
 
 /* prototipe fungsi */
 //void pid (float ref, float curr_feed, float prev_feed, float feedforward, float actual, float kp, float ki, float kd, float TS, float* output);
+void pidyol(float target,float feedback);
 
 int main() {
     arm1.period(0.02);
@@ -225,6 +226,7 @@
         /* setup and initialization*/
         timer1.start();
         float period = 0.0005;
+        float setpoint = 150;
         arm1.period(period);
     
         /* command move motor and sample data*/ 
@@ -239,38 +241,12 @@
                 
                 samp = timer1.read_us(); 
             } 
+            if(!mybutton){
+                setpoint = 0;
+            }
             if (timer1.read_us() - samp_pid > 9127)
             {
-//                float setpoint = 110;
-                float setpoint = 150;
-                float error = setpoint - curr_theta1;
-
-                lowpass_error = 0.1*error + 0.9*prev_error;
-//                lowpass_error = 0.1*lowpass_error + 0.9*prev_lowpass_error;
-//                prev_lowpass_error= lowpass_error;
-                
-                pwm1 = kp2*(lowpass_error) + kd2*(lowpass_error - prev_lowpass_error);
-                pwm1 = fabs(pwm1) > 0.85 ? 0.85*fabs(pwm1)/pwm1 : pwm1;
-                
-                prev_lowpass_error = lowpass_error;
-                
-                prev_error = error;
-                                
-                //arm2.speed(pwm2);
-//                if (curr_theta1>110 && curr_theta1 < 140 && setpoint >= 120){
-                if (curr_theta1>140 && curr_theta1 < 170 && setpoint >= 145){
-                    arm1.speed(0);
-                    arm1.brake();
-                }
-                else if (curr_theta1>130 && curr_theta1 <= 140){
-                    arm1.speed(0.075*pwm1);
-                }
-                else if (curr_theta1>115 && curr_theta1 <= 130){
-                    arm1.speed(0.65*pwm1);
-                }
-                else {
-                    arm1.speed(pwm1); 
-                }
+                pidyol(setpoint,curr_theta1);
                 samp_pid = timer1.read_us(); 
                 
                 
@@ -288,3 +264,51 @@
 
 
 /* definisi fungsi */
+void pidyol(float target,float feedback){
+    float error = target - feedback;
+
+    lowpass_error = 0.1*error + 0.9*prev_error;
+//                lowpass_error = 0.1*lowpass_error + 0.9*prev_lowpass_error;
+//                prev_lowpass_error= lowpass_error;
+    
+    pwm1 = kp2*(lowpass_error) + kd2*(lowpass_error - prev_lowpass_error);
+    pwm1 = fabs(pwm1) > 0.85 ? 0.85*fabs(pwm1)/pwm1 : pwm1;
+    
+    prev_lowpass_error = lowpass_error;
+    
+    prev_error = error;
+                    
+    //arm2.speed(pwm2);
+//                if (curr_theta1>110 && curr_theta1 < 140 && setpoint >= 120){
+    if (error>0){
+        if (feedback>140 && feedback < 170 && target >= 145){
+            arm1.speed(0);
+            arm1.brake();
+        }
+        else if (feedback>130 && feedback <= 140){
+            arm1.speed(0.075*pwm1);
+        }
+        else if (feedback>115 && feedback <= 130){
+            arm1.speed(0.65*pwm1);
+        }
+        else {
+            arm1.speed(pwm1); 
+        }
+    }
+    else if (error<0){
+        //kode turun
+        if (feedback>20 && feedback< 40 && target <= 20){
+            arm1.speed(0);
+            arm1.brake();
+        }
+        else if (feedback>40 && feedback <= 60){
+            arm1.speed(0.1*pwm1);
+        }
+        else if (feedback>60 && feedback <= 90){
+            arm1.speed(0.65*pwm1);
+        }
+        else {
+            arm1.speed(pwm1); 
+        }
+    }
+}
\ No newline at end of file