Diff: main.cpp

Revision:

2:b40c81648155

Parent:

1:fa0730bf53ef

Child:

3:3f559b3f7656

--- a/main.cpp	Tue Aug 23 08:55:01 2022 +0000
+++ b/main.cpp	Thu Aug 25 06:25:39 2022 +0000
@@ -34,25 +34,30 @@
 Ticker driving_ticker;
 Ticker velocity_ticker;
 #define DRIVING_PULSE_PER_ROTATION 40
-QEI driving_qei(D9, D10, NC, DRIVING_PULSE_PER_ROTATION);
-PID driving_pid(10.0f, 0.0f, 0.0f, DT); // kp ki kd 15 10 0.01
+QEI driving_qei(D10, D9, NC, DRIVING_PULSE_PER_ROTATION);
+PID driving_pid(0.03f, 0.0f, 0.0005f, DT);
 
 // stop btn pin
 DigitalIn stop_btn(D2);
 
+bool calibrationing = true;
+
 // reset btn function
 void calibration()
 {
-    bool calibrationing = true;
 
-    steering_dir_sign = 1;
+    steering_dir_sign = 0;
     steering_go_sign = 1;
-    steering_pwm.write(1.0f);
+    steering_pwm.write(0.5f);
     while (calibrationing)
     {
         if (!reset_btn) calibrationing = false;
         wait(0.01f);
     }
+    steering_qei.reset(); // reset after calibrationing
+    steering_dir_sign = 1;
+    while(steering_qei.getPulses() > STEERING_PULSE_PER_ROTATION * (-45.0f / 360.0f)) {}
+    steering_qei.reset(); // reset after calibrationing
     return ;
 }
 
@@ -141,6 +146,7 @@
 float max_velocity = 0;
 float target_velocity = 0;
 float writing_velocity = 0; // 0~1
+float output = 0.0f;
 float velocity_threshold = 0.05f;
 float step = 0.01f;
 int prev_step = 0;
@@ -163,15 +169,15 @@
 
 void driving()
 {
-    float output = 0.0f;
     float pid_output = 0.0f;
     
     hall_counter();
     calc_velocity();
     
-    max_velocity = max_velocity < current_velocity ? currentRPS : max_velocity;
+    max_velocity = max_velocity < current_velocity ? current_velocity : max_velocity;
     
-    if (max_velocity < 200) target_velocity = 100;
+    if (max_velocity < 100) target_velocity = 0;
+    else if (max_velocity < 200) target_velocity = 100;
     else if (max_velocity < 400) target_velocity = 300;
     else if (max_velocity < 600) target_velocity = 500;
     else if (max_velocity < 800) target_velocity = 700;
@@ -181,12 +187,18 @@
     
     driving_pid.ReadCurrentValue(current_velocity);
     driving_pid.SetTargetValue(target_velocity);
+    pid_output = driving_pid.Performance_Location();
+    output += pid_output;
+        
+    if (output > 1.0f) output = 1.0f;
+    else if (output < 0.0f) output = 0.0f;
     
-    pid_output = driving_pid.Performance_Location();
-    output = clip(pid_output);
+    // dead zone: 0.38 pwm
+    writing_velocity = 0.7f * output + 0.3f;
     
-    pc.printf("%d,%f,%f,%f\r", hall_count, current_velocity, target_velocity, pid_output);
-    //driving_analog_out.write(output); //writing_velocity
+    driving_analog_out.write(writing_velocity); //writing_velocity
+    
+    pc.printf("%d,%f,%f,%f,%f\r", hall_count, current_velocity, target_velocity, pid_output, writing_velocity);
 }
 
 // stop btn function
@@ -206,7 +218,6 @@
     steering_dir_sign = 1;
     steering_pwm.period(0.00001f);
     steering_ticker.attach(&call_steering, DT);
-    steering_qei.reset();
     
     //driving setting
     driving_ticker.attach(&call_driving, DT);
@@ -218,9 +229,9 @@
 //    hall_b.rise(&hall_counter);
 
     // calibration
-    //calibration();
-    
-    encoder_count = 0;// 76000 * (45.0f / 360.0f);
+    calibration();
+
+    //encoder_count = 3000;
 
     // driving
     while(1)
@@ -229,7 +240,7 @@
         {
             debugging_flag = false;
 //            pc.printf("%d\r", hall_count);
-            //pc.printf("%f,%f,%f\r", target_deg, current_deg, output);
+            //pc.printf("%f,%f\r", target_deg, current_deg);
             //pc.printf("%d\r", encoder_count);
         }
         
@@ -243,12 +254,11 @@
             steering_flag = false;
             steering();
         }
-        if(driving_flag)
-        {
-            driving_flag = false;
-            driving_analog_out.write(steering_potent.read());
-            driving();
-        }
+//        if(driving_flag)
+//        {
+//            driving_flag = false;
+//            driving();
+//        }
         //pc.printf("%f,%f,%f\n", current_velocity, target_velocity, writing_velocity);
     }
 }
\ No newline at end of file