Diff: main.cpp

Revision:

0:f252870bfd39

Child:

1:fa0730bf53ef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 04 08:01:11 2022 +0000
@@ -0,0 +1,248 @@
+#include "mbed.h"
+
+// serial communication for debugging
+Serial pc(USBTX, USBRX); // tx, rx
+
+// global variable
+float dt = 0.01f; //0.002f;
+
+// reset btn pin
+DigitalIn reset_btn(D8);
+
+// steering pins
+AnalogIn steering_potent(A0);
+DigitalOut steering_go_sign(D3);
+DigitalOut steering_dir_sign(D4);
+PwmOut steering_pwm(D7);
+InterruptIn steering_encoder_1(D5);
+InterruptIn steering_encoder_2(D6);
+Ticker steering_ticker;
+//Ticker integral_ticker; // i in PID control
+//Ticker differential_ticker; // d in PID control
+
+// driving pins
+AnalogOut driving_analog_out(A2);
+Ticker driving_ticker;
+InterruptIn hall_a(D9);
+InterruptIn hall_b(D10);
+InterruptIn hall_c(D11);
+Ticker velocity_ticker;
+
+// stop btn pin
+DigitalIn stop_btn(D2);
+
+// reset btn function
+
+void calibration()
+{
+    bool calibrationing = true;
+
+    steering_dir_sign = 1;
+    steering_go_sign = 1;
+    steering_pwm.write(1.0f);
+    while (calibrationing)
+    {
+        if (!reset_btn) calibrationing = false;
+        wait(0.01f);
+    }
+    return ;
+}
+
+// steering functions
+int encoder_count = 0;
+float accumulated_error = 0.0f;
+float delta_error = 0.0f;
+float error_prev = 0.0f;
+float error_ = 0.0f;
+float threshold = 0.0f;
+bool steering_flag = false;
+float potent = 0.0f;
+float target_deg = 0.0f;
+float current_deg = 0.0f;
+float _error = 0.0f;
+float pid_output = 0.0f;
+float output = 0.0f;
+
+void encoder_counter ()
+{
+    if (steering_encoder_2 == 1) encoder_count++;
+    else encoder_count--;
+}
+
+void integrate_error ()
+{
+    accumulated_error += dt * error_;
+}
+
+void differentiate_error ()
+{
+    delta_error = (error_ - error_prev) / dt;
+}
+
+float potent_to_degree(int deg_min, int deg_max, float potent_val)
+{
+    return (deg_max - deg_min) * potent_val + deg_min;
+}
+
+// 76000 pulse = 360 degrees
+float count_to_degree(int cnt)
+{
+    return (360.0f / 76000.0f) * cnt;
+}
+
+float calc_pid_output(float k_p, float k_i, float k_d, float e)
+{
+    return k_p * e + k_i * accumulated_error + k_d * delta_error;
+}
+
+float clip(float x)
+{
+    if (-1.0f < x && x < 1.0f) return x;
+    else if (x >= 1.0f) return 1.0f;
+    else return -1.0f;
+}
+
+void call_steering()
+{
+    steering_flag = true;
+}
+
+void steering()
+{
+    potent = steering_potent.read();
+    target_deg = potent_to_degree(-40, 40, potent); // direction change = min max change
+    current_deg = count_to_degree(encoder_count);
+    error_prev = error_;
+    _error = target_deg - current_deg;
+    error_ = _error;
+    pid_output = calc_pid_output(0.000001f, 0.0f, 0.0f, _error); // 0.05f, 0.01f, 0.0001f, when 0.001f (1kHz)
+    output = clip(pid_output);
+    
+    if (output > threshold)
+    {
+        steering_go_sign = 1;
+        steering_dir_sign = 1;
+        steering_pwm.write(output);
+    } 
+    else if (output < (-1 * threshold))
+    {
+        steering_go_sign = 1;
+        steering_dir_sign = 0;
+        steering_pwm.write(-1 * output);
+    }
+    else
+    {
+        steering_dir_sign = 0;
+        steering_go_sign = 0;
+    }
+}
+
+// driving functions
+#define HALL_A_STEP 1 //0x001
+#define HALL_B_STEP 2 //0x010
+#define HALL_C_STEP 4 //0x100
+
+bool driving_flag = false;
+int hall_count = 0;
+float current_velocity = 0;
+float target_velocity = 0;
+float writing_velocity = 0; // 0~1
+float velocity_threshold = 0.05f;
+float step = 0.01f;
+int prev_hall_a = hall_a.read();
+int prev_hall_b = hall_b.read();
+int prev_hall_c = hall_c.read();
+int prev_step = 0;
+
+void call_driving()
+{
+    driving_flag = true;
+}
+
+void hall_counter()
+{
+    if(prev_step != HALL_A_STEP && prev_hall_a == 0 && prev_hall_a != hall_a.read())
+    {
+        hall_count++;
+        prev_step = HALL_A_STEP;
+    }
+    else if(prev_step != HALL_B_STEP && prev_hall_b == 0 && prev_hall_b != hall_b.read())
+    {
+        hall_count++;
+        prev_step = HALL_B_STEP;
+    }
+    else if(prev_step != HALL_C_STEP && prev_hall_c == 0 && prev_hall_c != hall_c.read())
+    {
+        hall_count++;
+        prev_step = HALL_C_STEP;
+    }
+    prev_hall_a = hall_a.read();
+    prev_hall_b = hall_b.read();
+    prev_hall_c = hall_c.read();
+}
+
+void calc_velocity()
+{
+    current_velocity = hall_count / dt;
+    hall_count = 0;
+}
+
+void driving()
+{
+    if (target_velocity < current_velocity) target_velocity = current_velocity;
+    if ((current_velocity < target_velocity - velocity_threshold) && (writing_velocity < 1.0f)) writing_velocity += step;
+    driving_analog_out.write(writing_velocity); //writing_velocity
+}
+
+// stop btn function
+void stop_btn_pressed()
+{
+    writing_velocity = 0.0f;
+    pc.printf("stop! \r\n");
+}
+
+int main ()
+{
+    //steering setting
+    steering_go_sign = 0;
+    steering_dir_sign = 0;
+    steering_pwm.period(dt);
+    steering_encoder_1.rise(&encoder_counter);
+    steering_ticker.attach(&call_steering, dt);
+    
+    //driving setting
+    driving_ticker.attach(&call_driving, dt);
+    velocity_ticker.attach(&calc_velocity, dt);
+    hall_a.mode(PullUp);
+    hall_b.mode(PullUp);
+    hall_c.mode(PullUp);
+    hall_a.rise(&hall_counter);
+    hall_b.rise(&hall_counter);
+    hall_c.rise(&hall_counter);
+
+    // calibration
+    //calibration();
+    
+    encoder_count = 0;// 76000 * (45.0f / 360.0f);
+
+    // driving
+    while(1)
+    {
+        //if(!reset_btn) pc.printf("reset!");
+        //if(!stop_btn) stop_btn_pressed();
+        if(steering_flag)
+        {
+            steering_flag = false;
+            integrate_error();
+            differentiate_error();
+            steering();
+        }
+        //if(driving_flag)
+        //{
+        //    driving();
+        //    driving_flag = false;
+        //}
+        pc.printf("%f,%f\r", target_deg, current_deg);
+        //pc.printf("%f,%f,%f\n", current_velocity, target_velocity, writing_velocity);
+    }
+}
\ No newline at end of file