Hyogo Kurobayashi / Mbed OS test_robocontroller3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp

Committer:
kuriba
Date:
2020-03-11
Revision:
8:0318c56aae15
Parent:
7:49b71bc18049

File content as of revision 8:0318c56aae15:

#include "mbed.h"
#include "math.h"

InterruptIn encR(D12);
InterruptIn encL(D11);
DigitalOut led(LED1);
PwmOut motL_a(D6);
PwmOut motL_b(D9);
PwmOut motR_a(D3);
PwmOut motR_b(D5);
AnalogIn v_R(A0);
AnalogIn v_L(A2);
AnalogIn v_C(A1);
Thread thread_1;
Thread thread_2;

#define CYC_FLAG 1

float counter_L = 0.0f;
float counter_R = 0.0f;
float LED_L = 0.0f;
float LED_R = 0.0f;
float tire_count_L = 0.0f;
float tire_count_R = 0.0f;
float LED_tire_R = 0.0f;
float LED_tire_L = 0.0f;
float LED_tire = 0.0f;
float PI = 3.1416;
float dis_L = 0.0f;
float dis_R = 0.0f;
float delta_theta = 0.0f;
float dis = 0.0f;
float r = 0.0f;
float velocity_L = 0.0f;
float velocity_R = 0.0f;
float velocity = 0.0f;
float omega = 0.0f;
float diff[2] = {0.0f, 0.0f};
float d1 = 0.13f;
float d2 = 0.23f;
float b = 1.4f;
float b1 = 0.6f;
float w = 0.3f;
float g1 = 0.4f;
float g_CL2 = 1.2f;
float Kp = 3.23f;
float Ki = 0.05f; //0.05
float n = 0.5f;
float i_control = 0.0f;
float delta_R = 0.0f;
float delta_L = 0.0f;
int t = 0;
int memo_R = 0;
int memo_L = 0;

/*void mot_L(float a, int n){
    switch(n) {
        case 0: //正転
        motL_a = 0;
        motL_b = a;
        break;
        case 1: //逆転
        motL_a = a;
        motL_b = 0;
        break;
        case 2: //ブレーキ
        motL_a = 1;
        motL_b = 1;
        break;
    }
        
}*/

void event_handler_L(void) {
    counter_L++;
    LED_L++;
}

void event_handler_R(void) {
    counter_R++;
    LED_R++;
}

void omega_controller_R(float b, int n) {
    switch(n) {
        case 0: //0.05秒で0.03回
        diff[0] = diff[1];
        //diff[1] = 0.03f - b; //普通充電時
        diff[1] = 0.025f - b; //Max充電時
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d1+delta_R;
        motR_b = 0;
        break;
        case 1: //0.01秒で0.025回転
        diff[0] = diff[1];
        //diff[1] = 0.02f - b; //普通充電時
        diff[1] = 0.0125f - b; //Max充電時
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d1+delta_R;
        motR_b = 0;
        break;
        case 2: //0.01秒で0.0回転
        diff[0] = diff[1];
        diff[1] = 0.1f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d2+delta_R;
        motR_b = 0;
        break;
        case 3: //0.01秒で0.0
        diff[0] = diff[1];
        diff[1] = 0.01f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = 0;
        motR_b = 0.24f;
        break;
        case 4: //0.01秒で0.0回転
        diff[0] = diff[1];
        diff[1] = 0.07f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d2+delta_R;
        motR_b = 0;
        break;
        case 5: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.009f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d1+delta_R;
        motR_b = 0;
        break;
        case 6: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.0045f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d1+delta_R;
        motR_b = 0;
        break;
        case 7: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.002f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d1+delta_R;
        motR_b = 0;
        break;
        case 8: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.0001f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = 0;
        motR_b = 0;
        break;
        case 9: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.055f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d2+delta_R;
        motR_b = 0;
        break;
        case 10: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.015f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d1+delta_R;
        motR_b = 0;
        break;
        case 11: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.002f - b;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_R = Kp * diff[1] + i_control * n;
        motR_a = d1+delta_R;
        motR_b = 0;
        break;
        case 12: //0.01秒で0.001回転
        motR_a = 0;
        motR_b = 0;
        break;
    }
        
}

void omega_controller_L(float a, int n) {
    switch(n) {
        case 0: //0.01秒で0.005回
        diff[0] = diff[1];
        //diff[1] = 0.03f - a; //普通充電時
        diff[1] = 0.025f - a; //Max充電時
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d1+delta_L;
        break;
        case 1: //0.01秒で0.003回転
        diff[0] = diff[1];
        //diff[1] = 0.02f - a; //普通充電時
        diff[1] = 0.0125f - a; //Max充電時
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d1+delta_L;
        break;
        case 2: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.1f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d2+delta_L;
        break;
        case 3: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.01f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0.24f;
        motL_b = 0;
        break;
        case 4: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.07f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d2+delta_L;
        break;
        case 5: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.01f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d1+delta_L;
        break;
        case 6: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.005f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d1+delta_L;
        break;
        case 7: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.003f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d1+delta_L;
        break;
        case 8: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.0007f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = 0;
        break;
        case 9: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.055f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d2+delta_L;
        break;
        case 10: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.015f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d1+delta_L;
        break;
        case 11: //0.01秒で0.001回転
        diff[0] = diff[1];
        diff[1] = 0.002f - a;
        i_control += Ki*((diff[1] + diff[0]) / 2.0f)*0.01f;
        delta_L = Kp * diff[1] + i_control * n;
        motL_a = 0;
        motL_b = d1+delta_L;
        break;
        case 12: //0.01秒で0.001回転
        motL_a = 0;
        motL_b = 0;
        break;
    }
}

void LED() {
       // ThisThread::flags_wait_all(CYC_FLAG);
        
        LED_tire_R = (LED_R/12.0f)/38.2f; 
        LED_tire_L = (LED_L/12.0f)/38.2f;
        
        dis_R = LED_tire_R * 57.3f * PI;
        dis_L = LED_tire_L * 57.3f * PI;        
        dis += (dis_R + dis_L) / 2.0f;
        LED_R = 0.0f;
        LED_L = 0.0f; 
        
        if(dis >= 200.0f) {
            led = 1;
        }
        
        if(dis >= 400.0f) {
            led = 0;
            dis = 0;
        }
}

void robot_control_thread() {
    while(true) {
        ThisThread::flags_wait_all(CYC_FLAG);
        tire_count_R = (counter_R/12.0f)/38.2f;
        tire_count_L = (counter_L/12.0f)/38.2f;
        LED_tire += (counter_R/12.0f)/38.2f;
        
        if(LED_tire < 24) {
            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f<w){
                omega_controller_R(tire_count_R, 5);
                omega_controller_L(tire_count_L, 5);
            }
        
            if(v_L*3.3f>b && v_C*3.3f>b && v_R*3.3f<w) {   //左小カーブ
                omega_controller_R(tire_count_R, 5);
                omega_controller_L(tire_count_L, 6);
            }
        
            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f>b1) {   //右小カーブ
                omega_controller_R(tire_count_R, 7);
                omega_controller_L(tire_count_L, 5);
            }
            
            if(v_L*3.3f<w && v_C*3.3f<w && v_R*3.3f>b1) {   //右大カーブ
                omega_controller_R(tire_count_R, 12);
                omega_controller_L(tire_count_L, 7);
            }
        
            if(v_L*3.3f>b && v_C*3.3f<w && v_R*3.3f<w) {  //左大カーブ
                omega_controller_R(tire_count_R, 7);
                omega_controller_L(tire_count_L, 12);
            }
        } 
        
        if(LED_tire > 24) {  
            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f<w){
                omega_controller_R(tire_count_R, 0);
                omega_controller_L(tire_count_L, 0);
            }
        
            if(v_L*3.3f>b && v_C*3.3f>b && v_R*3.3f<w) {   //左小カーブ
                omega_controller_R(tire_count_R, 0);
                omega_controller_L(tire_count_L, 12);
            }
        
            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f>b1) {   //右小カーブ
                omega_controller_R(tire_count_R, 12);
                omega_controller_L(tire_count_L, 0);
            }
        
            if(v_L*3.3f<w && v_C*3.3f<w && v_R*3.3f>b1) {   //右大カーブ
                omega_controller_R(tire_count_R, 3);
                omega_controller_L(tire_count_L, 1);
            }
        
            if(v_L*3.3f>b && v_C*3.3f<w && v_R*3.3f<w) {  //左大カーブ
                omega_controller_R(tire_count_R, 1);
                omega_controller_L(tire_count_L, 3);
            }
        }
        
        /*if(LED_tire > 39 && LED_tire < 48) {
            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f<w){
                omega_controller_R(tire_count_R, 5);
                omega_controller_L(tire_count_L, 5);
            }
        
            if(v_L*3.3f>b && v_C*3.3f>b && v_R*3.3f<w) {   //左小カーブ
                omega_controller_R(tire_count_R, 6);
                omega_controller_L(tire_count_L, 7);
            }
        
            if(v_L*3.3f<w && v_C*3.3f>b && v_R*3.3f>b1) {   //右小カーブ
                omega_controller_R(tire_count_R, 7);
                omega_controller_L(tire_count_L, 6);
            }
        
            if(v_L*3.3f<w && v_C*3.3f<w && v_R*3.3f>b1) {   //右大カーブ
                omega_controller_R(tire_count_R, 12);
                omega_controller_L(tire_count_L, 7);
            }
        
            if(v_L*3.3f>b && v_C*3.3f<w && v_R*3.3f<w) {  //左大カーブ
                omega_controller_R(tire_count_R, 7);
                omega_controller_L(tire_count_L, 12);
            }
        }*/
        //printf("v_L = %.1f, v_R = %.1f, v_C = %.1f\n", v_L*3.3F, v_R*3.3F, v_C*3.3F);
        counter_R = 0;
        counter_L = 0;
    }
}      
              
int main() {
    thread_1.start(callback(robot_control_thread));
    //thread_2.start(callback(LED));
    encL.rise(&event_handler_L);
    encR.rise(&event_handler_R);
    
     while(true) {
         wait(0.01);
         thread_1.flags_set(CYC_FLAG);
         LED();
        // thread_2.flags_set(CYC_FLAG);
    }
}