tarou yamada
Command.cpp@3:56ad82f5f780, 2015-11-13 (annotated)
- Committer:
- inst
- Date:
- Fri Nov 13 08:15:43 2015 +0000
- Revision:
- 3:56ad82f5f780
- Parent:
- 2:58d7debaed1f
Who changed what in which revision?
| User | Revision | Line number | New 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 | const float Command::mAngleToAnalogCoeffA = -0.016122; |
| inst | 2:58d7debaed1f | 9 | const float Command::mAngleToAnalogCoeffB = 0.800000; |
| inst | 1:e541c8ebe96b | 10 | |
| inst | 2:58d7debaed1f | 11 | const float Command::mShootingAngleTable[] = { |
| inst | 3:56ad82f5f780 | 12 | 22.0f - 6.0f, // OWN_POLE |
| inst | 3:56ad82f5f780 | 13 | 36.0f - 6.0f, // CENTER_MIDDLE_POLE |
| inst | 3:56ad82f5f780 | 14 | 30.0f - 6.0f, // CENTER_SIDE_POLE |
| inst | 3:56ad82f5f780 | 15 | 16.0f - 6.0f // ENEMYS_POLE |
| inst | 1:e541c8ebe96b | 16 | }; |
| inst | 2:58d7debaed1f | 17 | const float Command::mMaxAngle_deg = 46.0f; |
| inst | 2:58d7debaed1f | 18 | const float Command::mMinAngle_deg = 2.0f; |
| inst | 1:e541c8ebe96b | 19 | |
| inst | 2:58d7debaed1f | 20 | Command::Command(){ |
| inst | 2:58d7debaed1f | 21 | mSteeringActionType = Steering::STOP; |
| inst | 2:58d7debaed1f | 22 | mMoveDirection_rad = gPI * 0.5f; |
| inst | 2:58d7debaed1f | 23 | mMoveDuty = 0.5f; |
| inst | 2:58d7debaed1f | 24 | mRollCoeff = 0.0f; |
| inst | 2:58d7debaed1f | 25 | mShootingAngle_deg = 0.0f; |
| inst | 2:58d7debaed1f | 26 | mAngleAdjust_deg = 0.0f; |
| inst | 2:58d7debaed1f | 27 | mShooterPosition = 0.0f; |
| inst | 2:58d7debaed1f | 28 | mIsShooting = false; |
| inst | 2:58d7debaed1f | 29 | mIsSupplying = false; |
| inst | 2:58d7debaed1f | 30 | |
| inst | 2:58d7debaed1f | 31 | setAimStateAndAimParam( ShootingSystem::AIM_OWN_POLE ); |
| inst | 1:e541c8ebe96b | 32 | } |
| inst | 1:e541c8ebe96b | 33 | |
| inst | 2:58d7debaed1f | 34 | Command::Command( Steering::ActionType steeringAction, |
| inst | 2:58d7debaed1f | 35 | ShootingSystem::AimState aimState, |
| inst | 2:58d7debaed1f | 36 | float moveDirection_rad, float angleAdjust_deg, |
| inst | 2:58d7debaed1f | 37 | float moveDuty, float roll, bool isSupplying ){ |
| inst | 2:58d7debaed1f | 38 | |
| inst | 2:58d7debaed1f | 39 | mSteeringActionType = steeringAction; |
| inst | 2:58d7debaed1f | 40 | mMoveDirection_rad = moveDirection_rad; |
| inst | 2:58d7debaed1f | 41 | mShootingAngle_deg = 0.0f; |
| inst | 2:58d7debaed1f | 42 | mAngleAdjust_deg = angleAdjust_deg; |
| inst | 2:58d7debaed1f | 43 | mMoveDuty = moveDuty; |
| inst | 2:58d7debaed1f | 44 | mRollCoeff = roll; |
| inst | 2:58d7debaed1f | 45 | mIsShooting = ( mSteeringActionType == Steering::SHOOT ); |
| inst | 2:58d7debaed1f | 46 | mIsSupplying = isSupplying; |
| inst | 2:58d7debaed1f | 47 | |
| inst | 2:58d7debaed1f | 48 | setAimStateAndAimParam( aimState ); |
| inst | 2:58d7debaed1f | 49 | } |
| inst | 2:58d7debaed1f | 50 | |
| inst | 2:58d7debaed1f | 51 | void Command::setAimStateAndAimParam( ShootingSystem::AimState aimState ){ |
| inst | 2:58d7debaed1f | 52 | mAimState = aimState; |
| inst | 2:58d7debaed1f | 53 | |
| inst | 2:58d7debaed1f | 54 | switch ( mAimState ){ |
| inst | 2:58d7debaed1f | 55 | case ShootingSystem::AIM_OWN_POLE: |
| inst | 2:58d7debaed1f | 56 | mShootingAngle_deg = mShootingAngleTable[ OWN_POLE_ID ] + mAngleAdjust_deg; |
| inst | 2:58d7debaed1f | 57 | mShootVoltage = Shooter::WORKING_8V; |
| inst | 2:58d7debaed1f | 58 | mShooterPosition = 0.780392f; |
| inst | 2:58d7debaed1f | 59 | break; |
| inst | 2:58d7debaed1f | 60 | |
| inst | 2:58d7debaed1f | 61 | case ShootingSystem::AIM_CENTER_MIDDLE_POLE: |
| inst | 2:58d7debaed1f | 62 | mShootingAngle_deg = mShootingAngleTable[ CENTER_MIDDLE_POLE_ID ] + mAngleAdjust_deg; |
| inst | 2:58d7debaed1f | 63 | mShootVoltage = Shooter::WORKING_12V; |
| inst | 2:58d7debaed1f | 64 | mShooterPosition = 0.305882f; |
| inst | 2:58d7debaed1f | 65 | break; |
| inst | 2:58d7debaed1f | 66 | |
| inst | 2:58d7debaed1f | 67 | case ShootingSystem::AIM_CENTER_SIDE_POLE: |
| inst | 2:58d7debaed1f | 68 | mShootingAngle_deg = mShootingAngleTable[ CENTER_SIDE_POLE_ID ] + mAngleAdjust_deg; |
| inst | 2:58d7debaed1f | 69 | mShootVoltage = Shooter::WORKING_12V; |
| inst | 2:58d7debaed1f | 70 | mShooterPosition = 0.780392f; |
| inst | 1:e541c8ebe96b | 71 | break; |
| inst | 1:e541c8ebe96b | 72 | |
| inst | 2:58d7debaed1f | 73 | case ShootingSystem::AIM_ENEMYS_POLE: |
| inst | 2:58d7debaed1f | 74 | mShootingAngle_deg = mShootingAngleTable[ ENEMYS_POLE_ID ] + mAngleAdjust_deg; |
| inst | 2:58d7debaed1f | 75 | mShootVoltage = Shooter::WORKING_12V; |
| inst | 2:58d7debaed1f | 76 | mShooterPosition = 0.305882f; |
| inst | 1:e541c8ebe96b | 77 | break; |
| inst | 1:e541c8ebe96b | 78 | |
| inst | 1:e541c8ebe96b | 79 | default: |
| inst | 1:e541c8ebe96b | 80 | break; |
| inst | 1:e541c8ebe96b | 81 | } |
| inst | 2:58d7debaed1f | 82 | |
| inst | 2:58d7debaed1f | 83 | if ( mShootingAngle_deg > mMaxAngle_deg ){ |
| inst | 2:58d7debaed1f | 84 | mShootingAngle_deg = mMaxAngle_deg; |
| inst | 2:58d7debaed1f | 85 | XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::INCREMENT ] ); |
| inst | 2:58d7debaed1f | 86 | } |
| inst | 2:58d7debaed1f | 87 | if ( mShootingAngle_deg < mMinAngle_deg ){ |
| inst | 2:58d7debaed1f | 88 | mShootingAngle_deg = mMinAngle_deg; |
| inst | 2:58d7debaed1f | 89 | XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::DECREMENT ] ); |
| inst | 2:58d7debaed1f | 90 | } |
| inst | 1:e541c8ebe96b | 91 | } |