yuya horie / Mbed 2 deprecated Roots

RobocupSSLのメイン基板白mbedのプログラム

Dependencies:   mbed

Rootsロボット mainプログラム

~ Robocup SSL（小型車輪リーグ）ロボット ~

Robocup SSLとは

●試合構成
Robocup小型ロボットリーグ（Small Size League)は，直径180[mm]，高さ150[mm]以内のサイズのロボット6台が1チームとなり，オレンジ色のゴルフボールを使ってサッカー競技を行う自立型ロボットコンテストである. フィールドの上には2台のWebカメラが設置され，フィールド上のロボットとボールを撮影する．Visionサーバは，フィールドの画像データよりロボットとボールの座標データを算出し，LANを用い各チームのAI用PCに送信する．Webカメラの撮影速度は，60[fps]である．レフリーボックスは，ファウルやフリーキック，スローインなどの審判の判定を入力し，LANを通じて各チームのAI用PCに送信する.それぞれのチームのAI用PCは，ロボットとボールの座標，審判の判定を元にロボットの移動，キックなどの作戦を決定し，無線によってロボットに指令を送信する． 700

ロボット機能紹介

●オムニホイールによる方向転換不要の全方位移動

オムニホイールは，自由に回転可能なローラをホイールの外周上に配置した車輪である．ローラの回転により，車輪の回転と垂直の方向に駆動力を発することはできないが移動は可能となる．各車輪の角速度を調整することによって全方向への移動を可能にする．
400

●ドリブルバーのバックスピンによるボール保持

●電磁力を利用したキッカー

●キッカーの電磁力エネルギーを充電する充電回路

●ロボット情報が一目でわかるLCD

mode/operation_check/operation_check.cpp

Committer:
alt0710
Date:
2019-09-22
Revision:
34:857b6cca2021
Parent:
33:13efae0ae6f4

File content as of revision 34:857b6cca2021:


#include "mbed.h"
#include "comm.h"
#include "mode.h"
#include "interface_manager.h"
#include "status_manager.h"
#include "parameter_manager.h"
#include "operation_check.h"

/* **mbedクラス** */
#ifdef LPC4088

#elif  STM32
Timeout time_charge;
#endif

static char m_charge;

/* **ローカル関数定義** */
void charge_off(void);
/* **ローカル関数** */
void charge_off(void)
{
    m_charge = 0;
    time_charge.detach();
}

/* **グルーバル関数** */

/* **クラス** */
class InterfaceManager;
class StatusManager;
class ParameterManager;



//メンバ変数の初期化は、定義順（Ｗａｒｎｉｎｇになる）
OperationCheck::OperationCheck()
:checkMode(SELECT),
 button_count(0),
 sideSW_num(0),
 charge_flag(0),
 kick_flag(0),
 power(0),
 ball(0),
 vel_value(0),
 vel_angle(0)
{    
}
 
void OperationCheck::run(void)
{
    switch(checkMode)
    {
        case SELECT:
        selectCheckMode();
        viewSelect();
        resetValue();
        break;
        
        case WHEEL: 
        //sprintf(str,"WHEEL");
        checkWheel();
        checkBack();
        break;
        
        case DRIBBLE: 
        sprintf(str,"DRIBBLE");
        checkDribble();
        checkBack();
        break;
        
        case STR_KICK: 
        sprintf(str,"STR_KICK");
        checkStrKick();
        checkBack();
        break;
        
        case CHIP_KICK: 
        sprintf(str,"CHO_KICK");
        checkChpKick();
        checkBack();
        break;
        
        default:
        sprintf(str,"ERROR");
        break;  
    }
    
    if(InterfaceManager::ball_sensor == 0)
    {
        InterfaceManager::LED = 0;
    }
    else if(InterfaceManager::ball_sensor == 1)
    {   
        InterfaceManager::LED = 1;
    }
//    //充電：左
//    //強制放電：下
//    //キック：上  
//    /* ***************** 充電 ******************************* */  
//    if(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
//    {
//        charge_flag = 1;
//    }
//    if(StatusManager::is_kicking == 1)
//    {
//        charge_flag = 0;
//        kick_flag   = 0;
//    }
//    
//    /* ***************** キック ******************************* */
//    if(InterfaceManager::button.getButtonStatus(CROSS_UP))
//    {
//        kick_flag = 1;
//    }
//    
//    /* ***************** 威力 ******************************* */
//    if(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
//    {
//        power++;
//        if(power > 15)
//        {
//            power = 0;
//        }
//    }
//
//    if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
//    {    
//        dribble.setdribble(1, power);
//    }
//    else
//    {
//        dribble.setdribble(0, 0);
//    }
//    
//    if(InterfaceManager::ball_sensor)
//    {    
//        ball = 1;
//    }
//    else
//    {
//        ball = 0;
//    }    
//    /* ***************** 強制放電 ***************************** */
////    kicker.setCharge(charge_flag);    
////    kicker.setKick(kick_flag, STRAIGHT, power);
////    kicker.forceFireKick();
//    
//    //createLcdData();
//    sprintf(str,"F:%dP:%d\nB:%dK:%d",charge_flag,power,ball,kick_flag);
//    //sprintf(str,"Hello");
    InterfaceManager::i2c.setLCDMsg(str,sizeof(str));   
        
}

void OperationCheck::selectCheckMode(void)
{
    if(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
    {
        button_count++;
    }
    
    if(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
    {
        button_count--;
    }
    
    if(button_count > 3)
    {
        button_count = 0;   
    }
    if(button_count < 0)
    {
        button_count = 3;   
    }
    
    if(InterfaceManager::button.getButtonStatus(CROSS_UP))
    {
        switch(button_count)
        {
            case 0:
            checkMode = WHEEL;
            break;
            
            case 1: 
            checkMode = DRIBBLE;
            break;
            
            case 2: 
            checkMode = STR_KICK;
            break;
            
            case 3: 
            checkMode = CHIP_KICK;
            break;
            
            default:
            checkMode = SELECT;
            break;  
        }
    }
    
}

void OperationCheck::viewSelect(void)
{
    switch(button_count)
    {
        case 0:
        sprintf(str,"CHECK\nWHEEL");
        break;
            
        case 1: 
        sprintf(str,"CHECK\nDRIBBLE");
        break;
            
        case 2: 
        sprintf(str,"CHECK\nSTR_KICK");
        break;
            
        case 3: 
        sprintf(str,"CHECK\nCHP_KICK");
        break;
            
        default:
        sprintf(str,"ERROR");
        break;  
    } 
}

void OperationCheck::checkBack(void)
{    
    if(InterfaceManager::button.getButtonStatus(CROSS_UP))
    {
        checkMode = SELECT;
    }
    
}

void OperationCheck::createLcdData(void)
{
    
    /*
 
     * 0    : 正常終了
 
     * -1   : タイムアウト
 
     * -2   : オーバーランエラー
 
     * -3   : フレーミングエラー
 
     * 1    : 受信中
 
     * 2    : フレームリーダー1まで受信
 
     * 3    : フレームリーダー2まで受信
 
     * 4    : マシンIDまで受信
 
     */
    switch(StatusManager::uart){
    case 0:
        sprintf(str,"ID%d EN:\nComplete",ParameterManager::machine_id);
        
        break;
 
    case 1:
        break;
 
    case 2:
        sprintf(str,"ID%d EN:\nHeader1",ParameterManager::machine_id);
        
        break;

    case 3:
        sprintf(str,"ID%d EN:\nHeader2",ParameterManager::machine_id);
        
        break;
 
    case 4:
        sprintf(str,"ID%d EN:\nCheckER1",ParameterManager::machine_id);
        
        break;

    case 5:
        sprintf(str,"ID%d EN:\nCheckER2",ParameterManager::machine_id);
        
        break;

    case -1:
        sprintf(str,"ID%d EN:\nTimeOut",ParameterManager::machine_id);
        
        break;
        
    case -2:
        sprintf(str,"ID%d EN:\nOERR",ParameterManager::machine_id);
        
        break;

    case -3:
        sprintf(str,"ID%d EN:\nFERR",ParameterManager::machine_id);
        
        break;
 
    default:
    sprintf(str,"ID%d EN: \n %d",ParameterManager::machine_id, StatusManager::uart);
        break;
    }

}

void OperationCheck::checkWheel(void)
{
    switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
    {
        case SINGLE_CLICK:
        vel_angle += 10;
        break;
        
        case DOUBLE_CLICK:
        vel_angle -= 10;
        break;
        default:
        
        break;
    }
    
    if(vel_angle < 0)
    {
        vel_angle = 360;
    }
    if(vel_angle > 360)
    {
        vel_angle = 0;
    }    

    switch(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
    {
        case SINGLE_CLICK:
        vel_value += 10;
        break;
        
        case DOUBLE_CLICK:
        vel_value -= 10;
        break;
        default:
        
        break;
    }
    
    if(vel_value < -100)
    {
        vel_value = 0;
    }
    if(vel_value > 100)
    {
        vel_value = 0;
    }  

    if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
    {
        wheel.setVelocity(vel_value,vel_angle,0);
    }  
    else
    {
       wheel.setVelocity(0,0,0);
    } 
    
    sprintf(str,"V:%d\nA:%d",vel_value,vel_angle);
       
    
}
void OperationCheck::checkDribble(void)
{
    switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
    {
        case SINGLE_CLICK:
        power += 1;
        break;
        
        case DOUBLE_CLICK:
        power -= 1;
        break;
        default:
        
        break;
    }
    
    if(power < 0)
    {
        power = 15;
    }
    if(power > 15)
    {
        power = 0;
    }     

    if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
    {
        dribble.setdribble(1,power);
    }  
    else
    {
       dribble.setdribble(0,0);
    } 
    
     
    
    //sprintf(str,"DRIBBLE\nPOWER:%d",power);
    sprintf(str,"POWER:%d\nC:%f",power,StatusManager::dribbleMotor_current); 
      
}
void OperationCheck::checkStrKick(void)
{
    switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
    {
        case SINGLE_CLICK:
        power += 1;
        break;
        
        case DOUBLE_CLICK:
        power -= 1;
        break;
        default:
        
        break;
    }
    
    if(power < 0)
    {
        power = 15;
    }
    if(power > 15)
    {
        power = 0;
    }    

    switch(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
    {
        case SINGLE_CLICK:
        //charge_flag = 1;
        m_charge = 1;
        time_charge.attach(&charge_off,1.0);
        //time_charge.attach(&charge_off,0.1);
        break;
        
        case DOUBLE_CLICK:
        m_charge = 0;
        //charge_flag = 0;
        break;
        default:
        
        break;
    }
    charge_flag = m_charge;
    
    
    if(StatusManager::is_kicking == 1)
    {
        charge_flag = 0;
        kick_flag   = 0;
    }
    kicker.setCharge(charge_flag); 

    if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
    {
       kick_flag = 1;
    }  
    else
    {
       kick_flag = 0;
    } 
    kicker.setKick(kick_flag,0,power);
    //kicker.forceFireKick();
    
    sprintf(str,"C:%dF:%d\nPOWER:%d",charge_flag,StatusManager::charge_end,power);  
}
void OperationCheck::checkChpKick(void)
{
    switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
    {
        case SINGLE_CLICK:
        power += 1;
        break;
        
        case DOUBLE_CLICK:
        power -= 1;
        break;
        default:
        
        break;
    }
    
    if(power < 0)
    {
        power = 15;
    }
    if(power > 15)
    {
        power = 0;
    }    

    switch(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
    {
        case SINGLE_CLICK:
        charge_flag = 1;
        break;
        
        case DOUBLE_CLICK:
        charge_flag = 0;
        break;
        default:
        
        break;
    }
    
    if(StatusManager::is_kicking == 1)
    {
        charge_flag = 0;
        kick_flag   = 0;
    }
    kicker.setCharge(charge_flag); 

    if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
    {
       kick_flag = 1;
    }  
    else
    {
       kick_flag = 0;
    } 
    kicker.setKick(kick_flag,1,power);
    kicker.forceFireKick();
    
    sprintf(str,"C:%dF:%d\nPOWER:%d",charge_flag,StatusManager::charge_end,power);      
}

void OperationCheck::resetValue(void)
{
    vel_value = 0;
    vel_angle = 0;
    power = 0;
    charge_flag = 0;
    kick_flag = 0;
}