#4 IFFFF command handler was added

robot-mitya · Mar 25, 2014 · 55f9cac · 55f9cac
1 parent abac2b3
commit 55f9cac
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 81 deletions.
diff --git a/robo_body/rm_act.cpp b/robo_body/rm_act.cpp
@@ -69,7 +69,18 @@ void Action::execute(String command, int value)
 
   if (command == "I")
   {
-    Equipment::executeInstruction(value);
+    if (value == Cfg::INSTRUCTION_STATE_REQUEST)
+    {
+      voltageTimerHandler(0, Equipment::getVoltage(0));
+      voltageTimerHandler(1, Equipment::getVoltage(1));
+      Message::send("I", Cfg::stateHeadlights);
+      Message::send("I", Cfg::stateMainAccumulatorCharge);
+      Message::send("I", Cfg::statePhoneAccumulatorCharge);
+    }
+    else
+    {
+      Equipment::executeInstruction(value);
+    }
     processedCommand = true;
   }
 
@@ -136,83 +147,3 @@ void Action::voltageTimerHandler(int voltageDivider, unsigned int voltage)
   Message::send("~", (voltageDivider << 12) | voltage);
 }
 
-
-
-
-
-
-
-//  switch(command[0]) {  // Сейчас у нас односимвольные команды, но на случай развития команда определена как String
-//    case 'M':
-//    {
-//      switch(value)
-//      {
-//        case 0x0101:
-//        {
-//          happyReflexStart();
-//          if (inPlaybackMode)
-//          {
-//            sendMessageToRobot(command, value);
-//          }
-//          break; 
-//        }
-//        case 0x0102:
-//        {
-//          const int ReadyToPlayVerticalDegree = 70;
-//          int verticalAmplitude = abs((ReadyToPlayVerticalDegree - servoHeadVertical.read()) * 2);
-//          boolean swingDirection = ReadyToPlayVerticalDegree > servoHeadVertical.read();
-//          servoHeadVertical.startSwing(2, 400, 0.25, verticalAmplitude, 1, swingDirection);
-//          servoHeadHorizontal.startSwing(2, 250, 3.5, 40, 0.8, true);
-//          servoTail.startSwing(value, 250, 6, 70, 0.9, true);
-//          readyToPlayReflexStart();
-//          if (inPlaybackMode)
-//          {
-//            sendMessageToRobot(command, value);
-//          }
-//          break; 
-//        }
-//        case 0x0103:
-//        {
-//          int verticalAmplitude = (servoHeadVertical.read() - Configuration::SERVO_HEAD_VERTICAL_MIN_DEGREE) * 2;
-//          servoHeadVertical.startSwing(2, 6000, 0.25, verticalAmplitude, 1, false);
-//          servoHeadHorizontal.startSwing(2, 750, 2, 60, 0.6, true);
-//          sadReflexStart();
-//          if (inPlaybackMode)
-//          {
-//            sendMessageToRobot(command, value);
-//          }
-//          break;
-//        }
-//        case 0x0104:
-//        {
-//          angryReflexStart();
-//          if (inPlaybackMode)
-//          {
-//            sendMessageToRobot(command, value);
-//          }
-//          break;
-//        }
-//        case 0x0105:  
-//        {
-//          int musicTacts = 12;
-//          servoHeadVertical.startSwing(1, musicPeriod, musicTacts, 30, 1, true);
-//          servoHeadHorizontal.startSwing(2, musicPeriod * musicTacts / 1.5, 1.5, 50, 1, true);
-//          servoTail.startSwing(value, musicPeriod, musicTacts, 70, 1, true);
-//          musicReflexStart();
-//          break;
-//        }
-//        default:
-//        {
-//          sendMessageToRobot(command, value);
-//        }
-//      } // (M-command's switch)
-//      break;
-//    }
-////    case 's':    
-////    {
-////      // Команда выстрела пушке:
-////      irsend.sendSony(0xABC0, 16);
-////      irrecv.enableIRIn(); // (надо для повторной инициализации ИК-приёмника)
-////      break;
-////    }
-
diff --git a/robo_body/rm_cfg.cpp b/robo_body/rm_cfg.cpp
@@ -0,0 +1,8 @@
+#include "rm_cfg.h"
+
+using namespace robot_mitya;
+
+int Cfg::stateHeadlights = Cfg::INSTRUCTION_HEADLIGHTS_OFF;
+int Cfg::stateMainAccumulatorCharge = Cfg::INSTRUCTION_MAIN_ACCUMULATOR_CHARGE_OFF;
+int Cfg::statePhoneAccumulatorCharge = Cfg::INSTRUCTION_PHONE_ACCUMULATOR_CHARGE_OFF;
+
diff --git a/robo_body/rm_cfg.h b/robo_body/rm_cfg.h
@@ -91,6 +91,12 @@ namespace robot_mitya
       static const int INSTRUCTION_MAIN_ACCUMULATOR_CHARGE_ON = 0x0031;
       static const int INSTRUCTION_PHONE_ACCUMULATOR_CHARGE_OFF = 0x0040;
       static const int INSTRUCTION_PHONE_ACCUMULATOR_CHARGE_ON = 0x0041;
+      static const int INSTRUCTION_STATE_REQUEST = 0xFFFF;
+
+      // Current state.
+      static int stateHeadlights;
+      static int stateMainAccumulatorCharge;
+      static int statePhoneAccumulatorCharge;
   };
 }
 

diff --git a/robo_body/rm_equ.cpp b/robo_body/rm_equ.cpp
@@ -131,21 +131,27 @@ void Equipment::executeInstruction(int value)
   switch (value) {
     case Cfg::INSTRUCTION_HEADLIGHTS_OFF:
       digitalWrite(Cfg::LIGHT_PIN, LOW);
+      Cfg::stateHeadlights = value;
       break;
     case Cfg::INSTRUCTION_HEADLIGHTS_ON:
       digitalWrite(Cfg::LIGHT_PIN, HIGH);
+      Cfg::stateHeadlights = value;
       break;
     case Cfg::INSTRUCTION_MAIN_ACCUMULATOR_CHARGE_OFF:
       digitalWrite(Cfg::MAIN_ACCUMULATOR_CHARGER_PIN, LOW);
+      Cfg::stateMainAccumulatorCharge = value;
       break;
     case Cfg::INSTRUCTION_MAIN_ACCUMULATOR_CHARGE_ON:
       digitalWrite(Cfg::MAIN_ACCUMULATOR_CHARGER_PIN, HIGH);
+      Cfg::stateMainAccumulatorCharge = value;
       break;
     case Cfg::INSTRUCTION_PHONE_ACCUMULATOR_CHARGE_OFF:
       digitalWrite(Cfg::PHONE_ACCUMULATOR_CHARGER_PIN, LOW);
+      Cfg::statePhoneAccumulatorCharge = value;
       break;
     case Cfg::INSTRUCTION_PHONE_ACCUMULATOR_CHARGE_ON:
       digitalWrite(Cfg::PHONE_ACCUMULATOR_CHARGER_PIN, HIGH);
+      Cfg::statePhoneAccumulatorCharge = value;
       break;
   }
 }