Revision 3:3f559b3f7656, committed 2022-09-19

Comitter:

ohdoyoel

Date:

Mon Sep 19 09:59:26 2022 +0000

Parent:

2:b40c81648155

Commit message:

prototype - 0919

Changed in this revision

--- a/main.cpp	Thu Aug 25 06:25:39 2022 +0000
+++ b/main.cpp	Mon Sep 19 09:59:26 2022 +0000
@@ -36,6 +36,7 @@
 #define DRIVING_PULSE_PER_ROTATION 40
 QEI driving_qei(D10, D9, NC, DRIVING_PULSE_PER_ROTATION);
 PID driving_pid(0.03f, 0.0f, 0.0005f, DT);
+DigitalOut driving_break(D11);
 
 // stop btn pin
 DigitalIn stop_btn(D2);
@@ -56,7 +57,7 @@
     }
     steering_qei.reset(); // reset after calibrationing
     steering_dir_sign = 1;
-    while(steering_qei.getPulses() > STEERING_PULSE_PER_ROTATION * (-45.0f / 360.0f)) {}
+    while(steering_qei.getPulses() > STEERING_PULSE_PER_ROTATION * (-50.0f / 360.0f)) {}
     steering_qei.reset(); // reset after calibrationing
     return ;
 }
@@ -81,9 +82,11 @@
     encoder_count = steering_qei.getPulses();
 }
 
-float potent_to_degree(int deg_min, int deg_max, float potent_val)
+float potent_to_degree(int deg_min, int deg_max, float potent_min, float potent_max, float potent_val)
 {
-    return (deg_max - deg_min) * potent_val + deg_min;
+    if (potent_val > potent_max) potent_val = potent_max;
+    else if (potent_val < potent_min) potent_val = potent_min;
+    return ((deg_max - deg_min) / (potent_max - potent_min)) * (potent_val - potent_min) + deg_min;
 }
 
 // 76000 pulse = 360 degrees
@@ -99,17 +102,16 @@
     else return -1.0f;
 }
 
-float target_deg = 0.0f;
-float current_deg = 0.0f;
-
 void steering()
 {
+    float target_deg = 0.0f;
+    float current_deg = 0.0f;
     float output = 0.0f;
     float pid_output = 0.0f;
     float potent = 0.0f;
 
     potent = steering_potent.read();
-    target_deg = potent_to_degree(-40, 40, potent); // direction change = min max change
+    target_deg = potent_to_degree(-40, 40, 0.1f, 0.9f, potent); // direction change는 min max change
     
     encoder_counter();
     current_deg = count_to_degree(encoder_count);
@@ -125,7 +127,7 @@
         steering_go_sign = 1;
         steering_dir_sign = 0;
         steering_pwm.write(output);
-    } 
+    }
     else if (output < (-1 * threshold))
     {
         steering_go_sign = 1;
@@ -176,20 +178,17 @@
     
     max_velocity = max_velocity < current_velocity ? current_velocity : max_velocity;
     
-    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;
-    else if (max_velocity < 1000) target_velocity = 900;
-    else if (max_velocity < 1200) target_velocity = 1100;
-    else target_velocity = 1300;
+    if (max_velocity < 200) target_velocity = 0;
+    else if (max_velocity < 600) target_velocity = 400;
+    else if (max_velocity < 1000) target_velocity = 800;
+    else target_velocity = 1200;
     
     driving_pid.ReadCurrentValue(current_velocity);
     driving_pid.SetTargetValue(target_velocity);
     pid_output = driving_pid.Performance_Location();
     output += pid_output;
-        
+    
+    // clip
     if (output > 1.0f) output = 1.0f;
     else if (output < 0.0f) output = 0.0f;
     
@@ -204,8 +203,11 @@
 // stop btn function
 void stop_btn_pressed()
 {
+    driving_break = 1;
+    target_velocity = 0.0f;
     writing_velocity = 0.0f;
-    pc.printf("stop! \r\n");
+    output = 0.0f;
+    max_velocity = 0.0f;
 }
 
 int main ()
@@ -221,44 +223,40 @@
     
     //driving setting
     driving_ticker.attach(&call_driving, DT);
-    //velocity_ticker.attach(&calc_velocity, DT);
     driving_qei.reset();
-//    hall_a.mode(PullUp);
-//    hall_b.mode(PullUp);
-//    hall_a.rise(&hall_counter);
-//    hall_b.rise(&hall_counter);
 
     // calibration
     calibration();
 
-    //encoder_count = 3000;
-
     // driving
     while(1)
     {
         if (debugging_flag)
         {
             debugging_flag = false;
+            pc.printf("%f\r", steering_potent.read());
 //            pc.printf("%d\r", hall_count);
             //pc.printf("%f,%f\r", target_deg, current_deg);
             //pc.printf("%d\r", encoder_count);
+            //pc.printf("%f,%f,%f\n", current_velocity, target_velocity, writing_velocity);\
         }
         
 //        steering_go_sign = 1;
 //        steering_dir_sign = 0;
 //        steering_pwm.write(steering_potent.read());
         //if(!reset_btn) pc.printf("reset!");
-        //if(!stop_btn) stop_btn_pressed();
+        if(!stop_btn) stop_btn_pressed();
+        else driving_break = 0;
         if(steering_flag)
         {
             steering_flag = false;
             steering();
         }
-//        if(driving_flag)
-//        {
-//            driving_flag = false;
-//            driving();
-//        }
-        //pc.printf("%f,%f,%f\n", current_velocity, target_velocity, writing_velocity);
+        if(driving_flag)
+        {
+            driving_flag = false;
+            driving();
+        }
     }
+}
 }
\ No newline at end of file