File content as of revision 0:f252870bfd39:

#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);
    }
}