tarou yamada / SteeringTire

SteeringTire.cpp@0:0cd6b505ba45, 2015-10-15 (annotated)

Committer:
inst
Date:
Thu Oct 15 08:46:09 2015 +0000
Revision:
0:0cd6b505ba45

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


inst

0:0cd6b505ba45


     1

#include "SteeringTire.h"





inst

0:0cd6b505ba45


     2

#include "Command.h"





inst

0:0cd6b505ba45


     3

#include "I2CMotor.h"





inst

0:0cd6b505ba45


     4

#include "Math.h"





inst

0:0cd6b505ba45


     5

#include "Steering.h"





inst

0:0cd6b505ba45


     6

#include "mbed.h"





inst

0:0cd6b505ba45


     7







inst

0:0cd6b505ba45


     8

const uint32_t SteeringTire::mReleaseStopTime_ms[] = {





inst

0:0cd6b505ba45


     9

    //10,





inst

0:0cd6b505ba45


    10

    100,





inst

0:0cd6b505ba45


    11

    300





inst

0:0cd6b505ba45


    12

};





inst

0:0cd6b505ba45


    13

const float SteeringTire::mBrakeReleaseThreshold = 0.50f;





inst

0:0cd6b505ba45


    14







inst

0:0cd6b505ba45


    15

SteeringTire::SteeringTire( I2CMotor** tire ) : mTire( tire ){





inst

0:0cd6b505ba45


    16

    mIsReleaseStop  = false;





inst

0:0cd6b505ba45


    17

    mTimer          = new Timer;





inst

0:0cd6b505ba45


    18

    mPrevActionType = Steering::STOP;





inst

0:0cd6b505ba45


    19

}





inst

0:0cd6b505ba45


    20







inst

0:0cd6b505ba45


    21

SteeringTire::~SteeringTire(){





inst

0:0cd6b505ba45


    22

    delete mTire;





inst

0:0cd6b505ba45


    23

}





inst

0:0cd6b505ba45


    24







inst

0:0cd6b505ba45


    25

void SteeringTire::update( Steering::ActionType action, Command command ){





inst

0:0cd6b505ba45


    26

    bool isReleaseStop = ( mPrevActionType == Steering::MOVE );





inst

0:0cd6b505ba45


    27

    isReleaseStop &= ( action == Steering::STOP );





inst

0:0cd6b505ba45


    28

    





inst

0:0cd6b505ba45


    29

    if ( isReleaseStop ){





inst

0:0cd6b505ba45


    30

        mIsReleaseStop = true;





inst

0:0cd6b505ba45


    31

        mTimer->reset();





inst

0:0cd6b505ba45


    32

        mTimer->start();





inst

0:0cd6b505ba45


    33

    }





inst

0:0cd6b505ba45


    34

    





inst

0:0cd6b505ba45


    35

    switch ( action ){





inst

0:0cd6b505ba45


    36

        case Steering::STOP:





inst

0:0cd6b505ba45


    37

            updateStop( command );





inst

0:0cd6b505ba45


    38

            break;





inst

0:0cd6b505ba45


    39

            





inst

0:0cd6b505ba45


    40

        case Steering::MOVE:





inst

0:0cd6b505ba45


    41

            updateMove( command );





inst

0:0cd6b505ba45


    42

            break;





inst

0:0cd6b505ba45


    43

            





inst

0:0cd6b505ba45


    44

        case Steering::ROLL:





inst

0:0cd6b505ba45


    45

            updateRoll( command );





inst

0:0cd6b505ba45


    46

            break;





inst

0:0cd6b505ba45


    47

            





inst

0:0cd6b505ba45


    48

        case Steering::WAIT_SERVO:





inst

0:0cd6b505ba45


    49

            updateWaitServo( command );





inst

0:0cd6b505ba45


    50

            break;





inst

0:0cd6b505ba45


    51

    }





inst

0:0cd6b505ba45


    52

    





inst

0:0cd6b505ba45


    53

    if ( action != Steering::WAIT_SERVO ){





inst

0:0cd6b505ba45


    54

        mPrevActionType = action;





inst

0:0cd6b505ba45


    55

    }





inst

0:0cd6b505ba45


    56

}





inst

0:0cd6b505ba45


    57







inst

0:0cd6b505ba45


    58

void SteeringTire::updateMove( Command command ){





inst

0:0cd6b505ba45


    59

    I2CMotor::ActionType action = I2CMotor::FORWARD;





inst

0:0cd6b505ba45


    60

    





inst

0:0cd6b505ba45


    61

    // 目標角度>PI ならモータを逆に回す0-pi[rad]で対応





inst

0:0cd6b505ba45


    62

    if ( command.getMoveDirection_rad() > gPI ){





inst

0:0cd6b505ba45


    63

        action = I2CMotor::REVERSE;





inst

0:0cd6b505ba45


    64

    }





inst

0:0cd6b505ba45


    65

    // タイヤの回転方向（正転or逆転）と速さ（duty）を更新





inst

0:0cd6b505ba45


    66

    for ( int i = 0; i < Steering::mNumOfTire; ++i ){





inst

0:0cd6b505ba45


    67

        mTire[ i ]->setActionType( action );





inst

0:0cd6b505ba45


    68

        mTire[ i ]->setPercent( command.getMoveDuty() );





inst

0:0cd6b505ba45


    69

    }





inst

0:0cd6b505ba45


    70

    





inst

0:0cd6b505ba45


    71

    if ( isNear( command.getMoveDirection_rad(), 0.0f, 0.01f ) ||





inst

0:0cd6b505ba45


    72

         isNear( command.getMoveDirection_rad(),  gPI, 0.01f ) ||





inst

0:0cd6b505ba45


    73

         isNear( command.getMoveDirection_rad(), -gPI, 0.01f ) ){





inst

0:0cd6b505ba45


    74

        mTire[ 0 ]->setPercent( command.getMoveDuty() * 0.30f );





inst

0:0cd6b505ba45


    75

        mTire[ 2 ]->setPercent( command.getMoveDuty() * 0.30f );





inst

0:0cd6b505ba45


    76

    }





inst

0:0cd6b505ba45


    77

}





inst

0:0cd6b505ba45


    78







inst

0:0cd6b505ba45


    79

void SteeringTire::updateRoll( Command command ){





inst

0:0cd6b505ba45


    80

    float coeff = command.getRollCoeff();





inst

0:0cd6b505ba45


    81

    // デフォルトでは0,1が正転で2,3が逆転1（時計回り）





inst

0:0cd6b505ba45


    82

    I2CMotor::ActionType action[] = { I2CMotor::FORWARD, I2CMotor::REVERSE };





inst

0:0cd6b505ba45


    83

    





inst

0:0cd6b505ba45


    84

    if ( coeff < 0.0f ){





inst

0:0cd6b505ba45


    85

        // 回転係数がマイナスなら逆回転（反時計周り）





inst

0:0cd6b505ba45


    86

        I2CMotor::ActionType temp = action[ 0 ];





inst

0:0cd6b505ba45


    87

        action[ 0 ] = action[ 1 ];





inst

0:0cd6b505ba45


    88

        action[ 1 ] = temp;





inst

0:0cd6b505ba45


    89

        





inst

0:0cd6b505ba45


    90

        coeff = abs( coeff );





inst

0:0cd6b505ba45


    91

    }





inst

0:0cd6b505ba45


    92

    





inst

0:0cd6b505ba45


    93

    for ( int i = 0; i < Steering::mNumOfTire; ++i ){





inst

0:0cd6b505ba45


    94

        // 左半分と右半分のモータは逆に駆動する





inst

0:0cd6b505ba45


    95

        mTire[ i ]->setActionType( action[ i / 2 ] );





inst

0:0cd6b505ba45


    96

        mTire[ i ]->setPercent( coeff );





inst

0:0cd6b505ba45


    97

    }





inst

0:0cd6b505ba45


    98

}





inst

0:0cd6b505ba45


    99







inst

0:0cd6b505ba45


   100

void SteeringTire::updateStop( Command command ){





inst

0:0cd6b505ba45


   101

    I2CMotor::ActionType action = I2CMotor::BRAKE;





inst

0:0cd6b505ba45


   102

    





inst

0:0cd6b505ba45


   103

    if ( mIsReleaseStop ){





inst

0:0cd6b505ba45


   104

        float t = mReleaseStopTime_ms[ 0 ];





inst

0:0cd6b505ba45


   105

        if ( command.getMoveDuty() > mBrakeReleaseThreshold ){





inst

0:0cd6b505ba45


   106

            t = mReleaseStopTime_ms[ 1 ];





inst

0:0cd6b505ba45


   107

        }





inst

0:0cd6b505ba45


   108

        if ( mTimer->read_ms() > t ){





inst

0:0cd6b505ba45


   109

            mIsReleaseStop = false;





inst

0:0cd6b505ba45


   110

            mTimer->stop();





inst

0:0cd6b505ba45


   111

            mTimer->reset();





inst

0:0cd6b505ba45


   112

        } else {





inst

0:0cd6b505ba45


   113

            action = I2CMotor::RELEASE;





inst

0:0cd6b505ba45


   114

        }





inst

0:0cd6b505ba45


   115

    }





inst

0:0cd6b505ba45


   116

    





inst

0:0cd6b505ba45


   117

    for ( int i = 0; i < Steering::mNumOfTire; ++i ){





inst

0:0cd6b505ba45


   118

        mTire[ i ]->setActionType( action );





inst

0:0cd6b505ba45


   119

    }





inst

0:0cd6b505ba45


   120

}





inst

0:0cd6b505ba45


   121







inst

0:0cd6b505ba45


   122

void SteeringTire::updateWaitServo( Command command ){





inst

0:0cd6b505ba45


   123

    // サーボの位置調整待ち中は移動用モータは止める





inst

0:0cd6b505ba45


   124

    for ( int i = 0; i < Steering::mNumOfTire; ++i ){





inst

0:0cd6b505ba45


   125

        mTire[ i ]->setActionType( I2CMotor::BRAKE );





inst

0:0cd6b505ba45


   126

    }





inst

0:0cd6b505ba45


   127

}