File content as of revision 3:3f559b3f7656:

#include "mbed.h"
#include "QEI.h"
#include "PID.h"

// serial communication for debugging
Serial pc(USBTX, USBRX); // tx, rx
Ticker debugging_ticker;
bool debugging_flag = false;
void call_debugging()
{
    debugging_flag = true;
}


// global variable
#define DT 0.01f
#define DT_DEBUGGING 0.05f

// 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);
Ticker steering_ticker;
#define STEERING_PULSE_PER_ROTATION 304000
QEI steering_qei(D5, D6, NC, STEERING_PULSE_PER_ROTATION);
PID steering_pid(20.0f, 15.0f, 0.05f, DT); // kp ki kd 15 10 0.01

// driving pins
AnalogOut driving_analog_out(A2);
Ticker driving_ticker;
Ticker velocity_ticker;
#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);

bool calibrationing = true;

// reset btn function
void calibration()
{

    steering_dir_sign = 0;
    steering_go_sign = 1;
    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 * (-50.0f / 360.0f)) {}
    steering_qei.reset(); // reset after calibrationing
    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.000001f;
bool steering_flag = false;

void call_steering()
{
    steering_flag = true;
}


void encoder_counter ()
{
    encoder_count = steering_qei.getPulses();
}

float potent_to_degree(int deg_min, int deg_max, float potent_min, float potent_max, float potent_val)
{
    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
float count_to_degree(int cnt)
{
    return (360.0f / STEERING_PULSE_PER_ROTATION) * cnt;
}

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 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, 0.1f, 0.9f, potent); // direction change는 min max change
    
    encoder_counter();
    current_deg = count_to_degree(encoder_count);
    
    steering_pid.ReadCurrentValue(current_deg);
    steering_pid.SetTargetValue(target_deg);
    
    pid_output = steering_pid.Performance_Location();
    output = clip(pid_output);
    
    if (output > threshold)
    {
        steering_go_sign = 1;
        steering_dir_sign = 0;
        steering_pwm.write(output);
    }
    else if (output < (-1 * threshold))
    {
        steering_go_sign = 1;
        steering_dir_sign = 1;
        steering_pwm.write(-1 * output);
    }
    else
    {
        steering_dir_sign = 0;
        steering_go_sign = 0;
    }
}

// driving functions
bool driving_flag = false;
int hall_count = 0;
float current_velocity = 0;
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;

void call_driving()
{
    driving_flag = true;
}

void hall_counter()
{
    hall_count = driving_qei.getPulses();
}

void calc_velocity()
{
    current_velocity = hall_count / DT;
    driving_qei.reset();
}

void driving()
{
    float pid_output = 0.0f;
    
    hall_counter();
    calc_velocity();
    
    max_velocity = max_velocity < current_velocity ? current_velocity : max_velocity;
    
    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;
    
    // dead zone: 0.38 pwm
    writing_velocity = 0.7f * output + 0.3f;
    
    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
void stop_btn_pressed()
{
    driving_break = 1;
    target_velocity = 0.0f;
    writing_velocity = 0.0f;
    output = 0.0f;
    max_velocity = 0.0f;
}

int main ()
{
    // debugging
    debugging_ticker.attach(&call_debugging, DT_DEBUGGING);

    //steering setting
    steering_go_sign = 0;
    steering_dir_sign = 1;
    steering_pwm.period(0.00001f);
    steering_ticker.attach(&call_steering, DT);
    
    //driving setting
    driving_ticker.attach(&call_driving, DT);
    driving_qei.reset();

    // calibration
    calibration();

    // 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();
        else driving_break = 0;
        if(steering_flag)
        {
            steering_flag = false;
            steering();
        }
        if(driving_flag)
        {
            driving_flag = false;
            driving();
        }
    }
}
}