tarou yamada / Command

Dependents:   OBROT_ALL

Command.cpp@2:58d7debaed1f, 2015-10-14 (annotated)

Committer:
inst
Date:
Wed Oct 14 05:50:26 2015 +0000
Revision:
2:58d7debaed1f
Parent:
1:e541c8ebe96b
Child:
3:56ad82f5f780

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


inst

1:e541c8ebe96b


     1

#include "Command.h"





inst

2:58d7debaed1f


     2

#include "Math.h"





inst

2:58d7debaed1f


     3

#include "Shooter.h"





inst

2:58d7debaed1f


     4

#include "Steering.h"





inst

2:58d7debaed1f


     5

#include "XBee.h"





inst

1:e541c8ebe96b


     6







inst

2:58d7debaed1f


     7

// angle to analog





inst

2:58d7debaed1f


     8

/*





inst

2:58d7debaed1f


     9

const float Command::mAngleToAnalogCoeffA = -0.01584967;





inst

2:58d7debaed1f


    10

const float Command::mAngleToAnalogCoeffB =  0.788235;





inst

2:58d7debaed1f


    11

*/





inst

2:58d7debaed1f


    12

const float Command::mAngleToAnalogCoeffA = -0.016122;





inst

2:58d7debaed1f


    13

const float Command::mAngleToAnalogCoeffB =  0.800000;





inst

1:e541c8ebe96b


    14







inst

2:58d7debaed1f


    15

const float Command::mShootingAngleTable[] = {





inst

2:58d7debaed1f


    16

    //4.0f,   // OWN_POLE





inst

2:58d7debaed1f


    17

    //16.0f,   // OWN_POLE





inst

2:58d7debaed1f


    18

    22.0f - 6.0f,





inst

2:58d7debaed1f


    19

    //16.0f,





inst

2:58d7debaed1f


    20

    





inst

2:58d7debaed1f


    21

    //30.0f,   // CENTER_MIDDLE_POLE





inst

2:58d7debaed1f


    22

    36.0f - 6.0f,





inst

2:58d7debaed1f


    23

    //24.0f,   // CENTER_MIDDLE_POLE





inst

2:58d7debaed1f


    24

    





inst

2:58d7debaed1f


    25

    //36.0f,   // CENTER_SIDE_POLE





inst

2:58d7debaed1f


    26

    30.0f - 6.0f,





inst

2:58d7debaed1f


    27

    //46.0f,





inst

2:58d7debaed1f


    28

    





inst

2:58d7debaed1f


    29

    //10.0f    // ENEMYS_POLE





inst

2:58d7debaed1f


    30

    //10.0f - 6.0f,





inst

2:58d7debaed1f


    31

    16.0f - 6.0f,





inst

2:58d7debaed1f


    32

    //24.0f





inst

1:e541c8ebe96b


    33

};





inst

2:58d7debaed1f


    34

const float Command::mMaxAngle_deg = 46.0f;





inst

2:58d7debaed1f


    35

const float Command::mMinAngle_deg =  2.0f;





inst

1:e541c8ebe96b


    36







inst

2:58d7debaed1f


    37

Command::Command(){





inst

2:58d7debaed1f


    38

    mSteeringActionType = Steering::STOP;





inst

2:58d7debaed1f


    39

    mMoveDirection_rad  = gPI * 0.5f;





inst

2:58d7debaed1f


    40

    mMoveDuty           = 0.5f;





inst

2:58d7debaed1f


    41

    mRollCoeff          = 0.0f;





inst

2:58d7debaed1f


    42

    mShootingAngle_deg  = 0.0f;





inst

2:58d7debaed1f


    43

    mAngleAdjust_deg    = 0.0f;





inst

2:58d7debaed1f


    44

    mShooterPosition    = 0.0f;





inst

2:58d7debaed1f


    45

    mIsShooting         = false;





inst

2:58d7debaed1f


    46

    mIsSupplying        = false;





inst

2:58d7debaed1f


    47

    





inst

2:58d7debaed1f


    48

    setAimStateAndAimParam( ShootingSystem::AIM_OWN_POLE );





inst

1:e541c8ebe96b


    49

}





inst

1:e541c8ebe96b


    50







inst

2:58d7debaed1f


    51

Command::Command( Steering::ActionType steeringAction,





inst

2:58d7debaed1f


    52

                  ShootingSystem::AimState aimState,





inst

2:58d7debaed1f


    53

                  float moveDirection_rad,  float angleAdjust_deg,





inst

2:58d7debaed1f


    54

                  float moveDuty, float roll, bool isSupplying ){





inst

2:58d7debaed1f


    55

    





inst

2:58d7debaed1f


    56

    mSteeringActionType     = steeringAction;





inst

2:58d7debaed1f


    57

    mMoveDirection_rad      = moveDirection_rad;





inst

2:58d7debaed1f


    58

    mShootingAngle_deg      = 0.0f;





inst

2:58d7debaed1f


    59

    mAngleAdjust_deg        = angleAdjust_deg;





inst

2:58d7debaed1f


    60

    mMoveDuty               = moveDuty;





inst

2:58d7debaed1f


    61

    mRollCoeff              = roll;





inst

2:58d7debaed1f


    62

    mIsShooting             = ( mSteeringActionType == Steering::SHOOT );





inst

2:58d7debaed1f


    63

    mIsSupplying            = isSupplying;





inst

2:58d7debaed1f


    64







inst

2:58d7debaed1f


    65

    setAimStateAndAimParam( aimState );





inst

2:58d7debaed1f


    66

}





inst

2:58d7debaed1f


    67







inst

2:58d7debaed1f


    68

void Command::setAimStateAndAimParam( ShootingSystem::AimState aimState ){





inst

2:58d7debaed1f


    69

    mAimState  = aimState;





inst

2:58d7debaed1f


    70

    





inst

2:58d7debaed1f


    71

    switch ( mAimState ){





inst

2:58d7debaed1f


    72

        case ShootingSystem::AIM_OWN_POLE:





inst

2:58d7debaed1f


    73

            mShootingAngle_deg  = mShootingAngleTable[ OWN_POLE_ID ] + mAngleAdjust_deg;





inst

2:58d7debaed1f


    74

            mShootVoltage       = Shooter::WORKING_8V;





inst

2:58d7debaed1f


    75

            //mShooterPosition    = 1.0f;





inst

2:58d7debaed1f


    76

            mShooterPosition    = 0.780392f;





inst

2:58d7debaed1f


    77

            break;





inst

2:58d7debaed1f


    78

        





inst

2:58d7debaed1f


    79

        case ShootingSystem::AIM_CENTER_MIDDLE_POLE:





inst

2:58d7debaed1f


    80

            mShootingAngle_deg  = mShootingAngleTable[ CENTER_MIDDLE_POLE_ID ] + mAngleAdjust_deg;





inst

2:58d7debaed1f


    81

            mShootVoltage       = Shooter::WORKING_12V;





inst

2:58d7debaed1f


    82

            mShooterPosition    = 0.305882f;





inst

2:58d7debaed1f


    83

            break;





inst

2:58d7debaed1f


    84

        





inst

2:58d7debaed1f


    85

        case ShootingSystem::AIM_CENTER_SIDE_POLE:





inst

2:58d7debaed1f


    86

            mShootingAngle_deg  = mShootingAngleTable[ CENTER_SIDE_POLE_ID ] + mAngleAdjust_deg;





inst

2:58d7debaed1f


    87

            //mShootVoltage       = Shooter::WORKING_12V;





inst

2:58d7debaed1f


    88

            mShootVoltage       = Shooter::WORKING_12V;





inst

2:58d7debaed1f


    89

            //mShooterPosition    = 1.0f;





inst

2:58d7debaed1f


    90

            mShooterPosition    = 0.780392f;





inst

1:e541c8ebe96b


    91

            break;





inst

1:e541c8ebe96b


    92

            





inst

2:58d7debaed1f


    93

        case ShootingSystem::AIM_ENEMYS_POLE:





inst

2:58d7debaed1f


    94

            mShootingAngle_deg  = mShootingAngleTable[ ENEMYS_POLE_ID ] + mAngleAdjust_deg;





inst

2:58d7debaed1f


    95

            mShootVoltage       = Shooter::WORKING_12V;





inst

2:58d7debaed1f


    96

            //mShooterPosition    = 0.0f;





inst

2:58d7debaed1f


    97

            mShooterPosition    = 0.305882f;





inst

1:e541c8ebe96b


    98

            break;





inst

1:e541c8ebe96b


    99

            





inst

1:e541c8ebe96b


   100

        default:





inst

1:e541c8ebe96b


   101

            break;





inst

1:e541c8ebe96b


   102

    }





inst

2:58d7debaed1f


   103

    





inst

2:58d7debaed1f


   104

    if ( mShootingAngle_deg > mMaxAngle_deg ){





inst

2:58d7debaed1f


   105

        mShootingAngle_deg = mMaxAngle_deg;





inst

2:58d7debaed1f


   106

        XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::INCREMENT ] );





inst

2:58d7debaed1f


   107

    }





inst

2:58d7debaed1f


   108

    if ( mShootingAngle_deg < mMinAngle_deg ){





inst

2:58d7debaed1f


   109

        mShootingAngle_deg = mMinAngle_deg;





inst

2:58d7debaed1f


   110

        XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::DECREMENT ] );





inst

2:58d7debaed1f


   111

    }





inst

1:e541c8ebe96b


   112

}