Diff: Command.cpp

Revision:

2:58d7debaed1f

Parent:

1:e541c8ebe96b

Child:

3:56ad82f5f780

--- a/Command.cpp	Fri Aug 21 04:51:05 2015 +0000
+++ b/Command.cpp	Wed Oct 14 05:50:26 2015 +0000
@@ -1,47 +1,112 @@
 #include "Command.h"
+#include "Math.h"
+#include "Shooter.h"
+#include "Steering.h"
+#include "XBee.h"
 
-const float Command::mPositionSetOfShooting[] = {
-    0.5f,
-    0.5f,
-    0.5f
-};
+// angle to analog
+/*
+const float Command::mAngleToAnalogCoeffA = -0.01584967;
+const float Command::mAngleToAnalogCoeffB =  0.788235;
+*/
+const float Command::mAngleToAnalogCoeffA = -0.016122;
+const float Command::mAngleToAnalogCoeffB =  0.800000;
 
-const float Command::mAngleSetOfShooting[] = {
-    0.5f,
-    0.5f,
-    0.5f
+const float Command::mShootingAngleTable[] = {
+    //4.0f,   // OWN_POLE
+    //16.0f,   // OWN_POLE
+    22.0f - 6.0f,
+    //16.0f,
+    
+    //30.0f,   // CENTER_MIDDLE_POLE
+    36.0f - 6.0f,
+    //24.0f,   // CENTER_MIDDLE_POLE
+    
+    //36.0f,   // CENTER_SIDE_POLE
+    30.0f - 6.0f,
+    //46.0f,
+    
+    //10.0f    // ENEMYS_POLE
+    //10.0f - 6.0f,
+    16.0f - 6.0f,
+    //24.0f
 };
+const float Command::mMaxAngle_deg = 46.0f;
+const float Command::mMinAngle_deg =  2.0f;
 
-Command::Command( ActionType action, float moveDirection_rad, float moveDuty, float roll, bool isShootable, AimTargetType target ){
-    mActionType         = action;
-    mMoveDirection_rad  = moveDirection_rad;
-    mMoveDuty           = moveDuty;
-    mRollCoeff          = roll;
-    mIsShootable        = isShootable;
-    setAimStateByTargetType( target );
+Command::Command(){
+    mSteeringActionType = Steering::STOP;
+    mMoveDirection_rad  = gPI * 0.5f;
+    mMoveDuty           = 0.5f;
+    mRollCoeff          = 0.0f;
+    mShootingAngle_deg  = 0.0f;
+    mAngleAdjust_deg    = 0.0f;
+    mShooterPosition    = 0.0f;
+    mIsShooting         = false;
+    mIsSupplying        = false;
+    
+    setAimStateAndAimParam( ShootingSystem::AIM_OWN_POLE );
 }
 
-void Command::setAimStateByTargetType( AimTargetType target ){
-    switch ( target ){
-        case NONE:
+Command::Command( Steering::ActionType steeringAction,
+                  ShootingSystem::AimState aimState,
+                  float moveDirection_rad,  float angleAdjust_deg,
+                  float moveDuty, float roll, bool isSupplying ){
+    
+    mSteeringActionType     = steeringAction;
+    mMoveDirection_rad      = moveDirection_rad;
+    mShootingAngle_deg      = 0.0f;
+    mAngleAdjust_deg        = angleAdjust_deg;
+    mMoveDuty               = moveDuty;
+    mRollCoeff              = roll;
+    mIsShooting             = ( mSteeringActionType == Steering::SHOOT );
+    mIsSupplying            = isSupplying;
+
+    setAimStateAndAimParam( aimState );
+}
+
+void Command::setAimStateAndAimParam( ShootingSystem::AimState aimState ){
+    mAimState  = aimState;
+    
+    switch ( mAimState ){
+        case ShootingSystem::AIM_OWN_POLE:
+            mShootingAngle_deg  = mShootingAngleTable[ OWN_POLE_ID ] + mAngleAdjust_deg;
+            mShootVoltage       = Shooter::WORKING_8V;
+            //mShooterPosition    = 1.0f;
+            mShooterPosition    = 0.780392f;
+            break;
+        
+        case ShootingSystem::AIM_CENTER_MIDDLE_POLE:
+            mShootingAngle_deg  = mShootingAngleTable[ CENTER_MIDDLE_POLE_ID ] + mAngleAdjust_deg;
+            mShootVoltage       = Shooter::WORKING_12V;
+            mShooterPosition    = 0.305882f;
+            break;
+        
+        case ShootingSystem::AIM_CENTER_SIDE_POLE:
+            mShootingAngle_deg  = mShootingAngleTable[ CENTER_SIDE_POLE_ID ] + mAngleAdjust_deg;
+            //mShootVoltage       = Shooter::WORKING_12V;
+            mShootVoltage       = Shooter::WORKING_12V;
+            //mShooterPosition    = 1.0f;
+            mShooterPosition    = 0.780392f;
             break;
             
-        case OWN_POLE:
-            mPositionOfShooting = mPositionSetOfShooting[ OWN_POLE_ID ];
-            mAngleOfShooting    = mAngleSetOfShooting[ OWN_POLE_ID ];
-            break;
-        
-        case CENTER_POLE:
-            mPositionOfShooting = mPositionSetOfShooting[ CENTER_POLE_ID ];
-            mAngleOfShooting    = mAngleSetOfShooting[ CENTER_POLE_ID ];
-            break;
-            
-        case ENEMYS_POLE:
-            mPositionOfShooting = mPositionSetOfShooting[ ENEMYS_POLE_ID ];
-            mAngleOfShooting    = mAngleSetOfShooting[ ENEMYS_POLE_ID ];
+        case ShootingSystem::AIM_ENEMYS_POLE:
+            mShootingAngle_deg  = mShootingAngleTable[ ENEMYS_POLE_ID ] + mAngleAdjust_deg;
+            mShootVoltage       = Shooter::WORKING_12V;
+            //mShooterPosition    = 0.0f;
+            mShooterPosition    = 0.305882f;
             break;
             
         default:
             break;
     }
+    
+    if ( mShootingAngle_deg > mMaxAngle_deg ){
+        mShootingAngle_deg = mMaxAngle_deg;
+        XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::INCREMENT ] );
+    }
+    if ( mShootingAngle_deg < mMinAngle_deg ){
+        mShootingAngle_deg = mMinAngle_deg;
+        XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::DECREMENT ] );
+    }
 }