File content as of revision 0:fb4269aa5fb4:

#include "mbed.h"

#include "gpio.h"

#include "robot.h"
#include "GP2Y0E03.h"
#include "MCP3208.h"
#include "L3GD20.h"

#define ERROR   (-1)
#define SUCCESS 0

//#define L3GD20_TEST
//#define GP2Y0E03_TEST
//#define MCP3208_TEST
//#define LBR127_TEST

/*---------RCサーボ制御用--------*/
float servo_pulse = 0.001f;     //RCサーボパルス幅[s]
uint32_t servo_state = 0;       //サーボピン状態
/*-----------------------------*/

/*-----------------ラインセンサ--------------------*/
float line_pos[2] = {0.0f, 0.0f};
uint32_t intersection = 0;          //交差点検知
/*------------------------------------------------*/

/*---------状態変数----------*/
float v_tr = 0.0f;      //並進方向電圧, unit: V
float theta = 0.0f;     //姿勢角, unit: degree

float omega_d = 0.0f;   //目標角速度, unit: degree/s
/*--------------------------*/

/*-----------------------------*/
float ts = 0.001f;                  //制御周期

uint32_t sequence       = 0;       //タスク管理変数
uint32_t error_seq      = 0xFF;        //エラーを起こしているタスク番号
int32_t (*Task_Fptr[10])(void);     //タスク管理用関数ポインタ
/*-----------------------------*/

/*-----制御器-----*/
PID_Struct trace_pid;   //ライントレース用
PID_Struct gyro_pid;   //ジャイロによるオドメトリ用

PID_Struct manual_control; //手動モータ制御用

PID_Struct *current_pid_ptr;   //現在接続されている制御器
/*----------------*/

/*---------各タスク関数--------*/
//ロボットの動作をシーケンシャルに記述する
//各タスク関数内ではフィードバック制御則へのアクセス原則禁止
int32_t Task0(void);   
int32_t Task1(void);
int32_t Task2(void);
int32_t Task3(void);
int32_t Task4(void);
int32_t Task5(void);   
int32_t Task6(void);
int32_t Task7(void);
int32_t Task8(void);
int32_t Task9(void);
/*---------------------------*/

void Turn_90deg(int32_t);
void Turn_180deg(int32_t);

void Accelerate(float);
void Decelerate();
void Stop();

int main() 
{
    uint32_t i;
    
    /*Peripheral initialization*/
    //GPIO_Init();
    
    SPI_Init();
    
    /*-----各種デバイス初期化----*/
    L3GD20_Init();
    MCP3208_Init();
    /*-------------------------*/
    
    /*------フィードバックコントローラ-------*/
    PID_Struct_Init(&trace_pid, 1.1, 20.0, 0.0);
    //PID_Struct_Init(&gyro_pid, 0.015, 0.0, 0.0);
    PID_Struct_Init(&gyro_pid, 0.02, 0.0, 0.0);
    current_pid_ptr = &manual_control;
    /*-----------------------------------*/
    
    TIM_Init();     //タイマスタート
    /*-------------------------*/
    
    /*-----関数ポインタ設定------*/
    Task_Fptr[0] = Task0;
    Task_Fptr[1] = Task1;
    Task_Fptr[2] = Task2;
    Task_Fptr[3] = Task3;
    Task_Fptr[4] = Task4;
    Task_Fptr[5] = Task5;
    Task_Fptr[6] = Task6;
    Task_Fptr[7] = Task7;
    Task_Fptr[8] = Task8;
    Task_Fptr[9] = Task9;
    /*-------------------------*/
    
    
    
    
    while(1)
    {
        if(Task_Fptr[sequence]() == ERROR)  //タスク呼び出し, エラーチェック
        {
            error_seq = sequence;       //エラーを起こしているタスク番号を取得
        }
    
        sequence ++;
        /*
        if(sequence == 1)
        {
            sequence = 3;
        }
        */
    }
}

int32_t Task0(void)
{
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    servo_pulse = 0.0021f;  //アームを折りたたむ
    wait(0.5f);
    
    /*-----最下点までリフトを移動-----*/
    Put_Vm_Lift(-0.8f);      //リフト下降
    wait(2.0f);
    Put_Vm_Lift(0.0f);      //リフト停止
    /*-----------------------------*/
    /*----リフトのリポジショニング----*/
    Put_Vm_Lift(1.0f);      //リフト上昇(どん兵衛用)
    wait(0.8f);
    Put_Vm_Lift(0.0f);      //リフト停止
    /*-----------------------------*/
    
    /*--------------------スタート待機-----------------------*/
    while(ain_cup > 0.5f) //カップ検出センサが反応するまで待機
    {
        printf("cup = %f\n", (float)ain_cup);
    }
    
    servo_pulse = 0.001f;  //アームを展開
    wait(0.3f);
    /*-----------------------------------------------------*/
    /*
    while(1)
    {
        HAL_Delay(10);
        printf("pos = %f\n", line_pos[0]);
    }
    */
    return 0;
}

int32_t Task1(void)
{
    uint32_t i;
    float v = 0.0f;
    
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(2.0f);       //加速
    
    for(i = 0; i < 2; i ++)     //1-2回目の交差点はスキップ
    {
        while(!intersection)    //一回目の交差点検出はスキップ
        {
            printf("running...\n");   
        }
        while(intersection)    //一回目の交差点通過を待つ
        {
            printf("running...\n");   
        }
    }
    /*-----------------------------------------------------*/
    
    return 0;
}

int32_t Task2(void)     //3本目の交差点を左折
{
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    while(!intersection)    //3回目の交差点検出まで直進
    {
        printf("running...\n");   
    }
    while(intersection)    //3回目の交差点通過を待つ
    {
        printf("running...\n");   
    }
    
    wait(0.4f);
    
    Stop();         //停止
    Turn_90deg(1);  //左90度旋回
    
    return 0;
}

int32_t Task3(void)     //横道を直進し、どん兵衛を回収、180度回転
{
    uint32_t i;
    float v = 0.0f;
    
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(1.0f);       //加速
    /*-----------------------------------------------------*/
    
    #if 0
    //本来はここでカップを検出するまで前進する
    wait(1.3f);
    #endif
    
    #if 1
    while(ain_cup > 0.6f) //カップ検出まで前進
    {
        printf("waiting...\n");
    }
    
    wait(0.05f);
    Stop();                 //停止
    
    servo_pulse = 0.0021f;  //カップ把持
    wait(1.0f);
    
    Put_Vm_Lift(1.5f);      //リフト上昇
    wait(0.8f);
    Put_Vm_Lift(0.0f);      //リフト停止
    #endif
    
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(1.5f);       //加速
    wait(0.5f);
    Stop();                 //停止
    
    Turn_180deg(1); //左180度旋回
    
    return 0;
}

int32_t Task4(void) //カップ回収後、メインストリートに戻る
{
    uint32_t i;
    float v = 0.0f;
    
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(1.5f);       //加速
    /*-----------------------------------------------------*/
    
    while(!intersection)    //交差点検出まで直進
    {
        printf("running...\n");   
    }
    while(intersection)    //交差点通過を待つ
    {
        printf("running...\n");   
    }
    
    wait(0.6f);
    
    Stop();
    
    Turn_90deg(1);  //左90度旋回

    return 0;
}

int32_t Task5(void) //カップ回収後のメインストリート走行+カップ棚置き
{
    uint32_t i;
    float v = 0.0f;
    
    /*
    if()
    {
        return ERROR;   
    }
    */
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(2.0f);       //加速
    /*-----------------------------------------------------*/
    
    while(ain_cup > 0.7f) //棚検出まで前進
    {
        printf("waiting...\n");
    }
    
    Stop();
    
    servo_pulse = 0.001f;  //カップ解放
    wait(0.5f);
    
    /*----------ジャイロを使って後退--------------*/
    current_pid_ptr = &gyro_pid;   //制御器変更
    Accelerate(-1.5f);       //加速
    
    wait(0.6f);
    
    Stop();
    /*------------------------------------------*/
    
    Turn_180deg(1);  //左180度旋回
    
    /*-----最下点までリフトを移動-----*/
    Put_Vm_Lift(-0.8f);      //リフト下降
    wait(2.5f);
    Put_Vm_Lift(0.0f);      //リフト停止
    /*-----------------------------*/
    /*----リフトのリポジショニング----*/
    Put_Vm_Lift(1.0f);      //リフト上昇(UFO用)
    wait(0.35f);
    Put_Vm_Lift(0.0f);      //リフト停止
    /*-----------------------------*/
    
    return 0;
}

int32_t Task6(void)     //4本目の交差点を棚側から左折
{
    
    uint32_t i;
    float v = 0;
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(1.5f);       //加速
    /*-----------------------------------------------------*/
    
    while(!intersection)    //交差点検出まで直進
    {
        printf("running...\n");   
    }
    while(intersection)    //交差点通過を待つ
    {
        printf("running...\n");   
    }
    
    wait(0.6f);
    
    Stop();         //停止
    Turn_90deg(1);  //左90度旋回
    
    return 0;
}

int32_t Task7(void)     //横道を直進し、UFOを回収、180度回転
{
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(1.0f);       //加速
    /*-----------------------------------------------------*/
   
    
    while(ain_cup > 0.6f) //カップ検出まで前進
    {
        printf("waiting...\n");
    }
    
    wait(0.05f);
    Stop();                 //停止
    
    servo_pulse = 0.0021f;  //カップ把持
    wait(1.0f);
    
    Put_Vm_Lift(1.5f);      //リフト上昇
    wait(0.8f);
    Put_Vm_Lift(0.0f);      //リフト停止
    
    
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(1.5f);       //加速
    wait(0.5f);
    Stop();                 //停止
    
    Turn_180deg(1); //左180度旋回
}


int32_t Task8(void)     //棚左側にUFOを置くために、位置合わせ
{
    uint32_t i;
    float v = 0;
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(1.5f);       //加速
    /*-----------------------------------------------------*/
    
    /*---------------1回目の交差点-----------------*/
    while(!intersection)    //交差点検出まで直進
    {
        printf("running...\n");   
    }
    while(intersection)    //交差点通過を待つ
    {
        printf("running...\n");   
    }
    
    wait(2.5f); //棚左側とカップの位置を合わせるためのパラメータ
    /*--------------------------------------------*/
    
    Stop();
    Turn_90deg(-1); //右90度旋回
    
    return 0;
}

int32_t Task9(void)     //ジャイロを使って棚まで接近、UFOを置く
{
    uint32_t i;
    float v = 0;
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    /*----------ジャイロを使って前進--------------*/
    current_pid_ptr = &gyro_pid;   //制御器変更
    Accelerate(1.5f);       //加速
    /*------------------------------------------*/
    
    while(ain_cup > 0.7f) //棚検出まで前進
    {
        printf("waiting...\n");
    }
    
    Stop();
    
    servo_pulse = 0.001f;  //カップ解放
    wait(0.5f);
    
    while(1)
    {
        printf("Tasks completed\n");
    }
    
    
    return 0;
}



















#if 0

int32_t Task8(void)     //棚左側にUFOを置くために、位置合わせ
{
    uint32_t i;
    float v = 0;
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    /*-------------------ライントレース--------------------*/
    current_pid_ptr = &trace_pid;   //制御器変更
    Accelerate(2.0f);       //加速
    
    for(i = 0; i < 3; i ++)     //1-3回目の交差点はスキップ
    {
        while(!intersection)    //一回目の交差点検出はスキップ
        {
            printf("running...\n");   
        }
        while(intersection)    //一回目の交差点通過を待つ
        {
            printf("running...\n");   
        }
    }
    /*-----------------------------------------------------*/
    
    /*---------------4回目の交差点-----------------*/
    while(!intersection)    //交差点検出まで直進
    {
        printf("running...\n");   
    }
    while(intersection)    //交差点通過を待つ
    {
        printf("running...\n");   
    }
    
    wait(1.5f);
    /*--------------------------------------------*/
    
    Stop();
    
    return 0;
}

int32_t Task9(void)
{
    uint32_t i;
    float v = 0;
    /*
    if()
    {
        return ERROR;   
    }
    */
    
    while(1)
    {
        printf("Tasks completed\n");
    }
    
    
    return 0;
}

#endif

void Turn_90deg(int32_t dir)
{
    uint32_t i;
    float v = 0.0f;
    float sign = 1.0f;
    
    if(dir == 1)    //回転方向
    {
        sign = 1.0f;
    }
    else if(dir == -1)
    {
        sign = -1.0f;
    }
    
    theta = 0.0f;   //角度初期化
    
    /*--------台形加速(角度)---------*/
    for(i = 0; i < 10; i ++)
    {
        v += 0.15f;
        Put_Vm_L(-sign * v); //回転
        Put_Vm_R(sign * v);
        wait(0.05f);
    }
    /*-----------------------------*/

    if(dir == 1)
    { 
        while(theta < 80.0f)
        {
            printf("Turning...\n");   
        }
    }
    else if(dir == -1)
    {
        while(theta > -80.0f)
        {
            printf("Turning...\n");   
        }
    }
    
    Put_Vm_L(0.0f); //停止
    Put_Vm_R(0.0f);
    wait(0.5f);
}

void Turn_180deg(int32_t dir)
{
    uint32_t i;
    float v = 0.0f;
    float sign = 1.0f;
    
    if(dir == 1)    //回転方向
    {
        sign = 1.0f;
    }
    else if(dir == -1)
    {
        sign = -1.0f;
    }
    
    theta = 0.0f;   //角度初期化
    
    /*--------台形加速(角度)---------*/
    for(i = 0; i < 10; i ++)
    {
        v += 0.15f;
        Put_Vm_L(-sign * v); //回転
        Put_Vm_R(sign * v);
        wait(0.05f);
    }
    /*-----------------------------*/

    while(theta < 170.0f)
    {
        printf("Turning...\n");   
    }
    Put_Vm_L(0.0f); //停止
    Put_Vm_R(0.0f);
    wait(0.5f);
}

void Accelerate(float v_desired)    //v_desired[V]まで緩やかに加速
{
    uint32_t i;
    float v = 0.0f;
    float v_step = v_desired / 10.0f;   //単位速度

    for(i = 0; i < 10; i ++)
    {
        v += v_step;
        v_tr = v;
        wait(0.05f);
    }
}

void Decelerate()           //緩やかに減速、停止
{
    uint32_t i;
    float v = 0.0f;
    float v_step = -v_tr / 10.0f;

    for(i = 0; i < 10; i ++)
    {
        v += v_step;
        v_tr = v;
        wait(0.05f);
    }

    v_tr = 0.0f;    //確実に停止
    wait(0.1f);
}

void Stop()         //急激に減速、停止
{
    current_pid_ptr = &manual_control;  //制御OFF
        Put_Vm_L(0.0f); 
        Put_Vm_R(0.0f);
        wait(0.5f);
}

void Flip1_Callback()       //RCサーボ制御用割り込み関数
{
    switch(servo_state)
    {
        case 0:
            flipper1.attach(&Flip1_Callback, servo_pulse);
            servo = 1;
            servo_state ++;
            break;
        case 1:
            flipper1.attach(&Flip1_Callback, 0.01f - servo_pulse);
            servo = 0;
            servo_state ++;
            break;
        default:
            servo_state = 0;
            break;
    }
}

void Flip2_Callback()       //フィードバック制御用割り込み関数
{
    int32_t i;
    uint32_t adres_max = 0;
    uint32_t adres_min = 65535;  
    uint32_t adres_mid = 0;
    int32_t pos_int = 0;
    
    float omega = 0.0f;    //unit: deg/s
    float vm_L, vm_R;       //unit: V
    
    /*-----------------センサ読み取り-----------------*/
    //MCP3208_Read(0);
    MCP3208_Read(1);
    MCP3208_Read(2);
    MCP3208_Read(3);
    MCP3208_Read(4);
    MCP3208_Read(5);
    
    L3GD20_Read();
    /*-----------------------------------------------*/
    
    /*---------------最大最小値を求める----------------*/
    for(i = 0; i < 5; i ++)
    {
        mcp3208.adres[i+1] = 4095 - mcp3208.adres[i+1];     //ADCの出力とラインの色に合わせて反転/非反転
        
        if(adres_min > mcp3208.adres[i+1])
            adres_min = mcp3208.adres[i+1];
        if(adres_max < mcp3208.adres[i+1])
            adres_max = mcp3208.adres[i+1];
    }        
    
    adres_mid = (adres_max + adres_min) / 2;
    /*-----------------------------------------------*/
    if(adres_max - adres_min > 200)
    {
        line_pos[0] = 0.0f;
        for(i = 0; i < 5; i ++)
        {
            pos_int += (i+1)*(mcp3208.adres[i+1] - adres_min);
        }
        line_pos[0] = ((float)pos_int);
        line_pos[0] = line_pos[0] / (mcp3208.adres[1]+mcp3208.adres[2]+mcp3208.adres[3]+mcp3208.adres[4]+mcp3208.adres[5]-5*adres_min) - 3.0f;
    }   
    else
    {
        line_pos[0] = 0.0f;
    }
    
    /*-------------------交差点検出-------------------*/
    if((mcp3208.adres[1] > adres_mid) && (mcp3208.adres[5] > adres_mid))
    {
        intersection = 1;
    }
    else if((mcp3208.adres[1] < adres_mid) && (mcp3208.adres[5] < adres_mid))
    {
        intersection = 0;
    }
    /*-----------------------------------------------*/
    
    /*---------------フィルタ----------------*/
    
    /*--------------------------------------*/
    
    /*-----------ジャイロ角速度取得-----------*/
    omega = (float)l3gd20.z * 0.07f;
    theta += omega * ts;
    /*--------------------------------------*/
    
    /*------------------制御則-----------------*/
    if(current_pid_ptr == &trace_pid)   //ライントレース
    {
        //vm_R = PD_Control(current_pid_ptr, omega, 10.0f * line_pos[0]);
        vm_R = PD_Control(current_pid_ptr, -line_pos[0], 0.0f);
        vm_L = -vm_R;
        
        vm_L += v_tr;
        vm_R += v_tr;
    }
    else if(current_pid_ptr == &gyro_pid)
    {
        vm_R = PD_Control(current_pid_ptr, omega, omega_d);
        vm_L = -vm_R;
        
        vm_L += v_tr;
        vm_R += v_tr;
    }
    else
    {
        vm_L = 0.0f;
        vm_R = 0.0f;
    }
    
    if(current_pid_ptr == &manual_control)
    {
        //手動でモータ電圧を管理している場合
    }
    else
    {
        Put_Vm_L(vm_L); //モータへ電圧印加
        Put_Vm_R(vm_R);
    }
    
    /*----------------------------------------*/
}

void Flip3_Callback()       
{
    
    
    
}

void PinIntrrpt1_Callback()
{
    
}

void PinIntrrpt2_Callback()
{
    
}