File content as of revision 2:b40c81648155:

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

// 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 * (-45.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_val)
{
    return (deg_max - deg_min) * potent_val + 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;
}

float target_deg = 0.0f;
float current_deg = 0.0f;

void steering()
{
    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
    
    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 < 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;
    
    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;
    
    // 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()
{
    writing_velocity = 0.0f;
    pc.printf("stop! \r\n");
}

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);
    //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("%d\r", hall_count);
            //pc.printf("%f,%f\r", target_deg, current_deg);
            //pc.printf("%d\r", encoder_count);
        }
        
//        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(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);
    }
}