#1 skript is ready and was tested on Romeo v1.1

robo_body_bt/rm_act.cpp

@@ -57,7 +57,7 @@ void Action::execute(String command, int value)
     Equipment::setHeadlightState(value);
     processedCommand = true;
   }
-  
+
   if (command == "=")
   {
     int voltageDividerIndex = value >> 12;
@@ -79,6 +79,48 @@ void Action::execute(String command, int value)
   {
     Message::sendResult(Message::RET_OK);
   }
+}
+
+boolean Action::isMotorCommand(String command)
+{
+  return (command == "L") || (command == "R") || (command == "G");
+}
+
+boolean Action::isMoveHeadCommand(String command)
+{
+  return (command == "H") || (command == "V");
+}
+
+boolean Action::isRotateHeadCommand(String command)
+{
+  return (command == "h") || (command == "v");
+}
+
+boolean Action::isSwingHeadCommand(String command)
+{
+  return (command == "y") || (command == "n");
+}
+
+boolean Action::isMoveTailCommand(String command)
+{
+  return (command == "T");
+}
+
+boolean Action::isSwingTailCommand(String command)
+{
+  return (command == "t");
+}
+
+void Action::voltageTimerHandler(int voltageDivider, unsigned int voltage)
+{
+  Message::send("~", voltage);
+}
+
+
+
+
+
+
 
 //  switch(command[0]) {  // Сейчас у нас односимвольные команды, но на случай развития команда определена как String
 //    case 'M':
@@ -153,39 +195,4 @@ void Action::execute(String command, int value)
 ////      irrecv.enableIRIn(); // (надо для повторной инициализации ИК-приёмника)
 ////      break;
 ////    }
-}
-
-boolean Action::isMotorCommand(String command)
-{
-  return (command == "L") || (command == "R") || (command == "G");
-}
-
-boolean Action::isMoveHeadCommand(String command)
-{
-  return (command == "H") || (command == "V");
-}
-
-boolean Action::isRotateHeadCommand(String command)
-{
-  return (command == "h") || (command == "v");
-}
 
-boolean Action::isSwingHeadCommand(String command)
-{
-  return (command == "y") || (command == "n");
-}
-
-boolean Action::isMoveTailCommand(String command)
-{
-  return (command == "T");
-}
-
-boolean Action::isSwingTailCommand(String command)
-{
-  return (command == "t");
-}
-
-void Action::voltageTimerHandler(int voltageDivider, unsigned int voltage)
-{
-  Message::send("~", voltage);
-}

robo_body_bt/rm_btn.cpp

@@ -6,7 +6,7 @@
 using namespace robot_mitya;
 
 static const int NUMBER_OF_BUTTONS = 5;
-static const int buttonValues[NUMBER_OF_BUTTONS] = { 30, 150, 360, 535, 760 };
+static const int buttonValues[NUMBER_OF_BUTTONS] = { 50, 180, 370, 550, 780 };
 static Button currentButton = NONE;
 static Button previousButton = NONE;
 static Button pressedButton = NONE;

robo_body_bt/rm_cfg.h

@@ -36,8 +36,12 @@ namespace robot_mitya
       // Headlights pin.
       static const int LIGHT_PIN = 13;
 
-      // Romeo buttons.
-      static const int BUTTONS_PIN = A0;
+      // Romeo buttons
+      static const boolean USE_BUTTONS = true;
+      // Romeo V2.0 buttons.
+//      static const int BUTTONS_PIN = A0;
+      // Romeo V1.1 buttons.
+      static const int BUTTONS_PIN = A7;
 
       // Boundary values for servos.
       static const int SERVO_HEAD_HORIZONTAL_MIN_DEGREE = 0;

robo_body_bt/rm_div.cpp

@@ -7,17 +7,18 @@ using namespace robot_mitya;
 
 VoltageDivider::VoltageDivider(int dividerIndex, int dividerPin, float arefVoltage, float r1, float r2)
 {
+  this->timerEnabled = false;
+  this->timerDelay = 0;
+  this->timerHandler = NULL;
+
   this->dividerIndex = dividerIndex;
-
   this->dividerPin = dividerPin;
 
   // How to get voltPerUnit value:
   // Take voltmeter and meassure voltage between AREF and GND pins
   // on Arduino controller and divide it by 1024.
   float voltPerUnit = arefVoltage / 1024;
-
   float dividerRatio = (r1 + r2) / r2; // (R1+R2)/R2
-
   this->voltRatio = voltPerUnit * dividerRatio * 100;
 }
 
@@ -26,22 +27,22 @@ unsigned int VoltageDivider::getVoltage()
   return voltRatio * Equipment::getVoltageDividerPinValue(this->dividerPin);
 }
 
-void VoltageDivider::setTimer(unsigned int timerDelay, void (*handler)(int, unsigned int))
+void VoltageDivider::setTimer(unsigned long timerDelay, void (*handler)(int, unsigned int))
 {
   this->timerDelay = timerDelay;
   this->timerEnabled = timerDelay != 0;
   this->timerHandler = handler;
-  this->timerLastTime = millis();
+  this->timerNextTime = millis() + this->timerDelay;
 }
 
 void VoltageDivider::refresh()
 {
   if (!this->timerEnabled) return;
-  
-  unsigned int currentTime = millis();
-  if (currentTime > (this->timerLastTime + this->timerDelay))
+
+  unsigned long currentTime = millis();
+  if (currentTime >= this->timerNextTime)
   {
-    this->timerLastTime = currentTime;
+    this->timerNextTime = currentTime + this->timerDelay;
     if (this->timerHandler != NULL)
     {
       this->timerHandler(this->dividerIndex, this->getVoltage());

robo_body_bt/rm_div.h

@@ -26,15 +26,15 @@ namespace robot_mitya
       // Return value is measured voltage * 100
       unsigned int getVoltage();
 
-      void setTimer(unsigned int timerDelay, void (*handler)(int, unsigned int) /* param1 - divider index, param2 - voltage */);
+      void setTimer(unsigned long timerDelay, void (*handler)(int, unsigned int) /* param1 - divider index, param2 - voltage */);
       void refresh();
     private:
       int dividerIndex;
       int dividerPin;
       float voltRatio;
       boolean timerEnabled;
-      unsigned int timerDelay;
-      unsigned int timerLastTime;
+      unsigned long timerDelay;
+      unsigned long timerNextTime;
       void (*timerHandler)(int, unsigned int); // param1 - divider index, param2 - voltage
   };
 }

robo_body_bt/rm_equ.cpp

@@ -62,8 +62,11 @@ void Equipment::initialize()
   moveMotor("G", 0);
 
   // Initializing buttons:
-  RomeoButtons::initialize(Cfg::BUTTONS_PIN);
-  RomeoButtons::setHandler(Equipment::buttonsHandler);
+  if (Cfg::USE_BUTTONS)
+  {
+    RomeoButtons::initialize(Cfg::BUTTONS_PIN);
+    RomeoButtons::setHandler(Equipment::buttonsHandler);
+  }
 
   // Initializing robot's state:
   State::initialize();
@@ -93,7 +96,10 @@ void Equipment::refresh()
   }
 
   // Handle buttons.
-  RomeoButtons::refresh(analogRead(Cfg::BUTTONS_PIN));
+  if (Cfg::USE_BUTTONS)
+  {
+    RomeoButtons::refresh(analogRead(Cfg::BUTTONS_PIN));
+  }
 
   // For Leonardo (Romeo V2) board we use SoftwareServo library because of Timers lack.
   #ifdef USBCON
@@ -169,6 +175,7 @@ void Equipment::moveTail(int degree)
 void Equipment::swingTail(int mode)
 {
   servoTail->stop();
+  moveTail(Cfg::SERVO_TAIL_DEFAULT_STATE);
   if ((mode == 1) || (mode == 2))
   {
     servoTail->startSwing(mode, 250, 6, 70, 0.9, true);
@@ -220,7 +227,7 @@ unsigned int Equipment::getVoltage(int dividerIndex)
   return 0;
 }
 
-void Equipment::setVoltageTimer(int dividerIndex, int timerDelay, void (*handler)(int, unsigned int))
+void Equipment::setVoltageTimer(int dividerIndex, unsigned long timerDelay, void (*handler)(int, unsigned int))
 {
   if (dividerIndex == Cfg::VOLTAGE_BATTERY_DIVIDER_INDEX)
   {
@@ -237,37 +244,58 @@ void Equipment::buttonsHandler(ButtonState buttonState, Button button)
   if (button == S5)
   {
     if (buttonState == PRESSED) State::setNextButtonsControlMode();
+    return;
   }
-  
-  if (State::getButtonsControlMode() == OTHER_CONTROL)
+
+  switch (State::getButtonsControlMode())
   {
-    if (button == S2)
+    case HEAD_CONTROL:
     {
-      if (buttonState == PRESSED) Action::execute("I", 1);
-      else if (buttonState == RELEASED) Action::execute("I", 0);
+      int const period = buttonState == PRESSED ? 400 : 0;
+      switch (button)
+      {
+        // Right:
+        case S1: Action::execute("h", -period); break;
+        // Forward:
+        case S2: Action::execute("v", -period); break;
+        // Left:
+        case S3: Action::execute("h", period); break;
+        // Backward:
+        case S4: Action::execute("v", period); break;
+      }
+      break;
+    }
+    case MOTORS_CONTROL:
+    {
+      int const speed = buttonState == PRESSED ? 128 : 0;
+      switch (button)
+      {
+        // Right:
+        case S1: Action::execute("L", speed); Action::execute("R", -speed); break;
+        // Forward:
+        case S2: Action::execute("G", speed); break;
+        // Left:
+        case S3: Action::execute("L", -speed); Action::execute("R", speed); break;
+        // Backward:
+        case S4: Action::execute("G", -speed); break;
+      }
+      break;
+    }
+    case OTHER_CONTROL:
+    {
+      switch (button)
+      {
+        // Right:
+        case S1: break;
+        // Forward:
+        case S2: buttonState == PRESSED ? Action::execute("I", 1) : Action::execute("I", 0); break;
+        // Left:
+        case S3: break;
+        // Backward:
+        case S4: if (buttonState == PRESSED) Action::execute("t", 1); break;
+      }
+      break;
     }
   }
-/*
-  String s = "";
-  
-  if (buttonState == PRESSED)
-    s += "pressed ";
-  else if (buttonState == RELEASED)
-    s += "released ";
-  else
-    s += "bad state ";
-    
-  switch (button)
-  {
-    case S1: s += "S1"; break;
-    case S2: s += "S2"; break;
-    case S3: s += "S3"; break;
-    case S4: s += "S4"; break;
-    case S5: s += "S5"; break;
-    default: s += "bad button";
-  }
-  
-  Message::debugOutput(s);
-*/
 }
 

robo_body_bt/rm_equ.h

@@ -33,7 +33,7 @@ namespace robot_mitya
 
       static unsigned int getVoltageDividerPinValue(int dividerPin);
       static unsigned int getVoltage(int dividerIndex);
-      static void setVoltageTimer(int dividerIndex, int timerDelay, void (*handler)(int, unsigned int));
+      static void setVoltageTimer(int dividerIndex, unsigned long timerDelay, void (*handler)(int, unsigned int));
     private:
       static void buttonsHandler(ButtonState buttonState, Button button);
   };

robo_body_bt/rm_sts.cpp

@@ -8,13 +8,13 @@ static ButtonsControlMode buttonsControlMode;
 
 void State::initialize()
 {
-  buttonsControlMode = HEAD_CONTROL;
+  buttonsControlMode = DISABLED;
 }
 
 void State::setNextButtonsControlMode()
 {
   int mode = buttonsControlMode + 1;
-  if (mode > (int)OTHER_CONTROL) mode = (int)HEAD_CONTROL;
+  if (mode > (int)OTHER_CONTROL) mode = (int)DISABLED;
   buttonsControlMode = (ButtonsControlMode)mode;
   logButtonsMode();
 }
@@ -29,6 +29,7 @@ void State::logButtonsMode()
   String text = "Switched to ";
   switch (buttonsControlMode)
   {
+    case DISABLED:        text += "DISABLED"; break;
     case HEAD_CONTROL:    text += "HEAD_CONTROL"; break;
     case MOTORS_CONTROL:  text += "MOTORS_CONTROL"; break;
     default:              text += "OTHER_CONTROL";

robo_body_bt/rm_sts.h

@@ -13,7 +13,7 @@
 
 namespace robot_mitya
 {
-  enum ButtonsControlMode { HEAD_CONTROL, MOTORS_CONTROL, OTHER_CONTROL };
+  enum ButtonsControlMode { DISABLED, HEAD_CONTROL, MOTORS_CONTROL, OTHER_CONTROL };
 
   class State
   {

robo_body_bt/robo_body_bt.ino

@@ -30,6 +30,8 @@ void setup()
 
   // Initializing controller's pins, motors, servos and headlights states
   Equipment::initialize();
+
+  Message::debugOutput("------------- Robot Mitya -------------");
 }
 
 // Sketch iteration

test_buttons/rm_btn.cpp

@@ -6,7 +6,7 @@
 using namespace robot_mitya;
 
 static const int NUMBER_OF_BUTTONS = 5;
-static const int buttonValues[NUMBER_OF_BUTTONS] = { 30, 150, 360, 535, 760 };
+static const int buttonValues[NUMBER_OF_BUTTONS] = { 50, 180, 370, 550, 780 };
 static Button currentButton = NONE;
 static Button previousButton = NONE;
 static Button pressedButton = NONE;
@@ -47,6 +47,10 @@ void RomeoButtons::refresh(int analogButtonsValue)
     else return;
 
     currentButton = getButton(analogButtonsValue);
+
+    Serial.println(analogButtonsValue);
+    delay(500);
+
     if (currentButton != previousButton)
     {
       previousButton = currentButton;