tarou yamada
/
XBee
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
XBee.cpp@3:115488d3ca1a, 2015-11-13 (annotated)
- Committer:
- inst
- Date:
- Fri Nov 13 08:15:58 2015 +0000
- Revision:
- 3:115488d3ca1a
- Parent:
- 2:869012d317b5
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| inst | 0:7e177131730d | 1 | #include "mbed.h" |
| inst | 0:7e177131730d | 2 | #include "I2CDevice.h" |
| inst | 0:7e177131730d | 3 | #include "XBee.h" |
| inst | 1:7f0b90daef14 | 4 | #include "Math.h" |
| inst | 1:7f0b90daef14 | 5 | #include "Steering.h" |
| inst | 1:7f0b90daef14 | 6 | #include "Command.h" |
| inst | 2:869012d317b5 | 7 | #include "ShootingSystem.h" |
| inst | 0:7e177131730d | 8 | |
| inst | 1:7f0b90daef14 | 9 | const size_t XBee::mNumOfDivDirection = 8; |
| inst | 2:869012d317b5 | 10 | const float XBee::mDutyTable[] = { |
| inst | 2:869012d317b5 | 11 | 0.05f, 0.90f |
| inst | 1:7f0b90daef14 | 12 | }; |
| inst | 1:7f0b90daef14 | 13 | // 右回りがプラスで左回りがマイナス |
| inst | 1:7f0b90daef14 | 14 | const float XBee::mRollCoeffTable[] = { |
| inst | 2:869012d317b5 | 15 | 0.05f, 0.9f |
| inst | 1:7f0b90daef14 | 16 | }; |
| inst | 2:869012d317b5 | 17 | const float XBee::mAngleAdjustTable[] = { |
| inst | 2:869012d317b5 | 18 | 0.0f, |
| inst | 2:869012d317b5 | 19 | 2.0f, |
| inst | 2:869012d317b5 | 20 | -2.0f |
| inst | 2:869012d317b5 | 21 | }; |
| inst | 2:869012d317b5 | 22 | float XBee::mAngleAdjust_deg = 0.0f; |
| inst | 2:869012d317b5 | 23 | float XBee::mMoveDirection_rad = 0.5f * gPI; |
| inst | 1:7f0b90daef14 | 24 | |
| inst | 1:7f0b90daef14 | 25 | XBee::XBee( char address ) : I2CDevice( address ){ |
| inst | 2:869012d317b5 | 26 | mMoveDirection_rad = 0; |
| inst | 2:869012d317b5 | 27 | mMoveDuty = 1.0f; |
| inst | 2:869012d317b5 | 28 | mRollCoeff = 0; |
| inst | 2:869012d317b5 | 29 | mSteeringActionType = Steering::STOP; |
| inst | 2:869012d317b5 | 30 | mAimState = ShootingSystem::AIM_OWN_POLE; |
| inst | 2:869012d317b5 | 31 | mIsNotStop = false; |
| inst | 2:869012d317b5 | 32 | mSpeedMode = HIGH_SPEED; |
| inst | 2:869012d317b5 | 33 | mPrevAngleAdjustType = NO_OPERATION; |
| inst | 0:7e177131730d | 34 | } |
| inst | 0:7e177131730d | 35 | |
| inst | 2:869012d317b5 | 36 | int XBee::read(){ |
| inst | 2:869012d317b5 | 37 | char buf[ 4 ] = { 0 }; |
| inst | 2:869012d317b5 | 38 | |
| inst | 2:869012d317b5 | 39 | // buf[ 0 ] : ActionType |
| inst | 2:869012d317b5 | 40 | // buf[ 1 ] : AngleAdjust, MoveDirection, Speed |
| inst | 2:869012d317b5 | 41 | // buf[ 2 ] : AimState |
| inst | 2:869012d317b5 | 42 | // buf[ 3 ] : IsSupply |
| inst | 2:869012d317b5 | 43 | int val = I2CDevice::read( buf, 4 ); |
| inst | 2:869012d317b5 | 44 | |
| inst | 2:869012d317b5 | 45 | Steering::ActionType action = mSteeringActionType; |
| inst | 2:869012d317b5 | 46 | mSteeringActionType = static_cast< Steering::ActionType >( buf[ 0 ] ); |
| inst | 1:7f0b90daef14 | 47 | |
| inst | 2:869012d317b5 | 48 | ShootingSystem::AimState newAimState = static_cast< ShootingSystem::AimState >( buf[ 2 ] ); |
| inst | 2:869012d317b5 | 49 | if ( newAimState != mAimState ){ |
| inst | 2:869012d317b5 | 50 | mAngleAdjust_deg = 0.0f; |
| inst | 2:869012d317b5 | 51 | } |
| inst | 2:869012d317b5 | 52 | mAimState = newAimState; |
| inst | 2:869012d317b5 | 53 | |
| inst | 2:869012d317b5 | 54 | mIsShooting = false; |
| inst | 2:869012d317b5 | 55 | mIsSupplying = buf[ 3 ]; |
| inst | 2:869012d317b5 | 56 | |
| inst | 2:869012d317b5 | 57 | if ( mSteeringActionType != Steering::STOP ){ |
| inst | 2:869012d317b5 | 58 | mPrevAngleAdjustType = NO_OPERATION; |
| inst | 2:869012d317b5 | 59 | } |
| inst | 1:7f0b90daef14 | 60 | |
| inst | 2:869012d317b5 | 61 | switch ( mSteeringActionType ){ |
| inst | 2:869012d317b5 | 62 | case Steering::STOP: |
| inst | 2:869012d317b5 | 63 | if ( ( buf[ 1 ] == LOW_SPEED ) || ( buf[ 1 ] == HIGH_SPEED )){ |
| inst | 2:869012d317b5 | 64 | mSpeedMode = static_cast< SpeedType >( buf[ 1 ] ); |
| inst | 2:869012d317b5 | 65 | mIsNotStop = true; |
| inst | 2:869012d317b5 | 66 | mPrevAngleAdjustType = NO_OPERATION; |
| inst | 2:869012d317b5 | 67 | } else if ( buf[ 1 ] == NO_OPERATION ){ |
| inst | 2:869012d317b5 | 68 | mIsNotStop = false; |
| inst | 2:869012d317b5 | 69 | mPrevAngleAdjustType = NO_OPERATION; |
| inst | 2:869012d317b5 | 70 | } else if ( mPrevAngleAdjustType != static_cast< AngleAdjustType >( buf[ 1 ] ) ){ |
| inst | 2:869012d317b5 | 71 | mAngleAdjust_deg += mAngleAdjustTable[ buf[ 1 ] ]; |
| inst | 2:869012d317b5 | 72 | mPrevAngleAdjustType = static_cast< AngleAdjustType >( buf[ 1 ] ); |
| inst | 2:869012d317b5 | 73 | } |
| inst | 1:7f0b90daef14 | 74 | break; |
| inst | 1:7f0b90daef14 | 75 | |
| inst | 2:869012d317b5 | 76 | case Steering::MOVE: |
| inst | 2:869012d317b5 | 77 | if ( mSpeedMode == LOW_SPEED ){ |
| inst | 2:869012d317b5 | 78 | mMoveDuty = mDutyTable[ 0 ]; |
| inst | 2:869012d317b5 | 79 | } else if ( mSpeedMode == HIGH_SPEED ){ |
| inst | 2:869012d317b5 | 80 | mMoveDuty = mDutyTable[ 1 ]; |
| inst | 2:869012d317b5 | 81 | } |
| inst | 2:869012d317b5 | 82 | |
| inst | 1:7f0b90daef14 | 83 | mMoveDirection_rad = 2.0f * gPI / mNumOfDivDirection * buf[ 1 ]; |
| inst | 1:7f0b90daef14 | 84 | break; |
| inst | 1:7f0b90daef14 | 85 | |
| inst | 2:869012d317b5 | 86 | case Steering::ROLL: |
| inst | 2:869012d317b5 | 87 | if ( mSpeedMode == LOW_SPEED ){ |
| inst | 2:869012d317b5 | 88 | mRollCoeff = mRollCoeffTable[ 0 ]; |
| inst | 2:869012d317b5 | 89 | } else if ( mSpeedMode == HIGH_SPEED ){ |
| inst | 2:869012d317b5 | 90 | mRollCoeff = mRollCoeffTable[ 1 ]; |
| inst | 2:869012d317b5 | 91 | } |
| inst | 2:869012d317b5 | 92 | |
| inst | 2:869012d317b5 | 93 | if ( buf[ 1 ] == CONTERCLOCKWISE ){ |
| inst | 2:869012d317b5 | 94 | mRollCoeff *= -1.0f; |
| inst | 2:869012d317b5 | 95 | } |
| inst | 2:869012d317b5 | 96 | break; |
| inst | 2:869012d317b5 | 97 | |
| inst | 2:869012d317b5 | 98 | case Steering::SHOOT: |
| inst | 2:869012d317b5 | 99 | mIsShooting = true; |
| inst | 1:7f0b90daef14 | 100 | break; |
| inst | 1:7f0b90daef14 | 101 | |
| inst | 1:7f0b90daef14 | 102 | default: |
| inst | 2:869012d317b5 | 103 | mSteeringActionType = Steering::STOP; |
| inst | 1:7f0b90daef14 | 104 | break; |
| inst | 1:7f0b90daef14 | 105 | } |
| inst | 2:869012d317b5 | 106 | |
| inst | 2:869012d317b5 | 107 | if ( mIsNotStop ){ |
| inst | 2:869012d317b5 | 108 | mSteeringActionType = action; |
| inst | 2:869012d317b5 | 109 | } |
| inst | 2:869012d317b5 | 110 | |
| inst | 2:869012d317b5 | 111 | return val; |
| inst | 1:7f0b90daef14 | 112 | } |
| inst | 1:7f0b90daef14 | 113 | |
| inst | 2:869012d317b5 | 114 | Command XBee::createCommand(){ |
| inst | 2:869012d317b5 | 115 | return Command( mSteeringActionType, mAimState, |
| inst | 2:869012d317b5 | 116 | mMoveDirection_rad, mAngleAdjust_deg, |
| inst | 2:869012d317b5 | 117 | mMoveDuty, mRollCoeff, mIsSupplying ); |
| inst | 1:7f0b90daef14 | 118 | } |