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

actuator/wheel.cpp

Committer:
alt0710
Date:
2018-05-03
Revision:
31:7b003082f31d
Parent:
17:e5e7b19dd707
Child:
32:718efbf4dc8a

File content as of revision 31:7b003082f31d:


#include "mbed.h"
#include "comm.h"
#include "interface_manager.h"
#include "parameter_manager.h"
#include "status_manager.h"
#include "wheel.h"
#include "math.h"

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

#elif  STM32

#endif

/* **ローカル関数定義** */

/* **ローカル関数** */

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

/* **クラス** */
class InterfaceManager;
//メンバ変数の初期化は、定義順（Ｗａｒｎｉｎｇになる）
Wheel::Wheel()
{
    order.Mot0Order.Vel_short = 0;
    order.Mot1Order.Vel_short = 0;
    order.Mot2Order.Vel_short = 0;
    order.Mot3Order.Vel_short = 0;   
}

void Wheel::setVelocity(short V_value, short V_angle, short V_omega)
{
    Vel_Value = V_value;
    Vel_Angle = V_angle;
    Vel_Omega = V_omega;
    
    calcWheelVelocity();
    
    InterfaceManager::can.setMotVel(order);    
}

void Wheel::calcWheelVelocity(void)
{
    convertPhasorToVector();
    resolveVector();   
}

void Wheel::convertPhasorToVector(void)
{
    double Vel_Angle_rad;
    
    Vel_Angle_rad   = (double)Vel_Angle * DEG2RAD;
    Vel_X           = (double)Vel_Value * cos(Vel_Angle_rad);
    Vel_Y           = (double)Vel_Value * sin(Vel_Angle_rad);
    Vel_OMEGA       = (double)Vel_Omega;
}

void Wheel::resolveVector(void)
{
    double floor_value[4];

    Vel_Mot0_double  = operationV0(Vel_X, Vel_Y, Vel_OMEGA );
    Vel_Mot1_double  = operationV1(Vel_X, Vel_Y, Vel_OMEGA );
    Vel_Mot2_double  = operationV2(Vel_X, Vel_Y, Vel_OMEGA );
    Vel_Mot3_double  = operationV3(Vel_X, Vel_Y, Vel_OMEGA );
    
//    Vel_Mot0_double = -500.0;
//    Vel_Mot1_double = -500.0;
//    Vel_Mot2_double = -500.0;
//    Vel_Mot3_double = -500.0;
    

    Vel_Mot0_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
    Vel_Mot1_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
    Vel_Mot2_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
    Vel_Mot3_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;

   
    if(Vel_Mot0_double > 0)floor_value[0] = 0.5;
    else if(Vel_Mot0_double < 0){ floor_value[0] = -0.5;}
    else {floor_value[0] = 0;}

    if(Vel_Mot1_double > 0)floor_value[1] = 0.5;
    else if(Vel_Mot1_double < 0){ floor_value[1] = -0.5;}
    else {floor_value[1] = 0;}

    if(Vel_Mot2_double > 0)floor_value[2] = 0.5;
    else if(Vel_Mot2_double < 0){ floor_value[2] = -0.5;}
    else {floor_value[2] = 0;}

    if(Vel_Mot3_double > 0)floor_value[3] = 0.5;
    else if(Vel_Mot3_double < 0){ floor_value[3] = -0.5;}
    else {floor_value[3] = 0;}
    

    order.Mot0Order.Vel_short = (short)(Vel_Mot0_double + floor_value[0]);
    order.Mot1Order.Vel_short = (short)(Vel_Mot1_double + floor_value[1]);
    order.Mot2Order.Vel_short = (short)(Vel_Mot2_double + floor_value[2]);
    order.Mot3Order.Vel_short = (short)(Vel_Mot3_double + floor_value[3]);
    
#ifndef EXCHANGE_REQUEST
    order.Mot0Order.Vel_short *= 3;
    order.Mot1Order.Vel_short *= 3;
    order.Mot2Order.Vel_short *= 3;
    order.Mot3Order.Vel_short *= 3;
#endif
//    order.Mot0Order.Vel_short = 0;
//    order.Mot1Order.Vel_short = 0;
//    order.Mot2Order.Vel_short = 0;
//    order.Mot3Order.Vel_short = 0;
        
StatusManager::Vel_Wheel1 = order.Mot1Order.Vel_short;
StatusManager::Vel_Wheel2 = order.Mot2Order.Vel_short;
StatusManager::Vel_Wheel3 = order.Mot3Order.Vel_short;
    
    /* モータ軸がCCWで車輪がCWに回転するので，極性反転*/
//    order.Mot0Order.Vel_short *= -1;
//    order.Mot1Order.Vel_short *= -1;
//    order.Mot2Order.Vel_short *= -1;
//    order.Mot3Order.Vel_short *= -1;
     
}

double Wheel::operationV0(double vx,double vy, double vw)
{
    double v;

    v = (CONST_V0_COEFFICIENT_VX_ * vx) + (CONST_V0_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );

    return v;
}

double Wheel::operationV1(double vx,double vy, double vw)
{
    double v;

    v = (CONST_V1_COEFFICIENT_VX_ * vx) + (CONST_V1_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );

    return v;
}

double Wheel::operationV2(double vx,double vy, double vw)
{
    double v;

    v = (CONST_V2_COEFFICIENT_VX_ * vx) + (CONST_V2_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );

    return v;
}

double Wheel::operationV3(double vx,double vy, double vw)
{
    double v;

    v = (CONST_V3_COEFFICIENT_VX_ * vx) + (CONST_V3_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );

    return v;
}