delete println

code/Arduino/CommandsController.cpp

@@ -4,7 +4,6 @@
 
 CommandsController::CommandsController()
 {
- servoController.init();
 }
 
 void CommandsController::handle(ConnectingDevice *device, String command) 

code/Arduino/CommandsEnum.h

@@ -2,25 +2,16 @@
 
 
 enum ControllerEnum {
- movementControllerID = '\001',
+ movementControllerID = '\001', 
  sensorsControllerID = '\002',
  servoControllerID = '\003'
 };
 
 enum SensorsEnum {
- distance_sensor_1 = '\001',
- distance_sensor_2 = '\002',
- distance_sensor_3 = '\003',
- distance_sensor_4 = '\004',
- distance_sensor_5 = '\005',
- distance_sensor_all = '\006',
-
- line_sensor_1 = '\007',
- line_sensor_2 = '\008',
- line_sensor_3 = '\009',
- line_sensor_4 = '\010',
- line_sensor_5 = '\011',
- line_sensor_all = '\012'
+ distance_sensor = '\001', //get value from single distance senceor
+ distance_sensor_all = '\002', //get value from all distance senceors
+ line_sensor = '\003', //get value from single line senceor
+ line_sensor_all = '\004' //get value from all line senceors
 };
 
 
@@ -32,16 +23,23 @@ enum MoveEnum {
  stop = '\005', //stop
  forward_speed = '\006', //move forward with established speed
  forward_time = '\007', //move forward while established time
- back_speed = '\008', //move back with established speed
- track_set_speed = '\009' //choose track and set passed speed
+ back_speed = '\010', //move back with established speed
+ track_set_speed = '\011' //choose track and set passed speed
 };
 
 enum TrackDiraction {
- forward_direction = true,
- back_direction = false
+ forward_direction = true, //rotation of track in forward diration
+ back_direction = false //rotation of track in backword diration
 };
 
 enum TrackID {
- left_track = 0,
- right_track = 1
+ left_track = 0, //left track id
+ right_track = 1 //right track id
+};
+
+enum ServoCommands {
+ set_horizontal_angle = '\001', //set horizontal angle of servo
+ set_vertical_angle = '\002', //set vertical angle of servo
+ set_horiz_vertical_angles = '\003', //set horizontal and verticalse angles of servo
+ get_coodrinates = '\004', //get current angels of horizontal and vertical servos
 };

code/Arduino/ConnectingDevice.cpp

@@ -15,7 +15,6 @@ void ConnectingDevice::send(String data)
 
 bool ConnectingDevice::isActive() 
 {
- (*this).send("lol");
  if ((*device).available()) {
  return true;
  }

code/Arduino/Constants.cpp

@@ -30,7 +30,7 @@ Constants::Constants()
  servo_horizontal_pin = 34;
  servo_vertical_pin = 35;
 
- commands_delimetr = '\255';
+ commands_delimetr = ';';
 }
 
 

code/Arduino/MainController.cpp

@@ -13,6 +13,7 @@ int* MainController::parse_command(String command, int begin, char delimetr, int
  arr[i] = command.substring(begin, delimetrPos).toInt();
  begin = delimetrPos + 1;
  command = command.substring(begin, command.length());
+ begin = 0;
  }
  }
  return arr;
@@ -22,6 +23,17 @@ int MainController::parse_command(String command, int begin, int end) {
  return command.substring(begin, end).toInt();
 }
 
+
+String MainController::intArrayToString(int* array, int size) {
+ String str = "";
+ for (int i = 0; i < size; i++) {
+ str += String(array[i]);
+ str += ";";
+ }
+ return str;
+}
+
+
 MainController::~MainController() {
 
 }

code/Arduino/MainController.h

@@ -12,5 +12,7 @@ class MainController
  MainController();
  int* parse_command(String command, int begin, char delimetr, int paramsLength);
  int parse_command(String command, int begin, int end);
+
+ String intArrayToString(int*, int);
  ~MainController();
 };

code/Arduino/MovementController.cpp

@@ -45,11 +45,12 @@ void MovementController::exec(ConnectingDevice *device, String command) {
  move_back(parse_command(command, 2, command.length()));
  break;
  case track_set_speed:
- choose_track_set_speed(parse_command(command, 2, constants.commands_delimetr), 2);
+ choose_track_set_speed(parse_command(command, 2, constants.commands_delimetr, 2));
  break;
  default:
  break;
  }
+
 }
 
 
@@ -67,6 +68,7 @@ void MovementController::move_forward(int speed, int time_ms) {
  left_track_control(forward_direction, speed);
  right_track_control(forward_direction, speed);
  delay(time_ms);
+ stop_moving();
 }
 
 void MovementController::move_back() {

code/Arduino/SensorsController.cpp

@@ -25,38 +25,14 @@ SensorsController::SensorsController()
 void SensorsController::exec(ConnectingDevice *device, String command) {
  switch (command[1])
  {
- case distance_sensor_1:
- device->send(String(getDistance(1)));
- break;
- case distance_sensor_2:
- device->send(String(getDistance(2)));
- break;
- case distance_sensor_3:
- device->send(String(getDistance(3)));
- break;
- case distance_sensor_4:
- device->send(String(getDistance(4)));
- break;
- case distance_sensor_5:
- device->send(String(getDistance(5)));
+ case distance_sensor:
+ device->send(String(getDistance(parse_command(command, 2, command.length()))));
  break;
  case distance_sensor_all:
  device->send(intArrayToString(getDistanceAll(), countDistanceSensors));
  break;
- case line_sensor_1:
- device->send(String(getLine(1)));
- break;
- case line_sensor_2:
- device->send(String(getLine(2)));
- break;
- case line_sensor_3:
- device->send(String(getLine(3)));
- break;
- case line_sensor_4:
- device->send(String(getLine(4)));
- break;
- case line_sensor_5:
- device->send(String(getLine(5)));
+ case line_sensor:
+ device->send(String(getLine(parse_command(command, 2, command.length()))));
  break;
  case line_sensor_all:
  device->send(intArrayToString(getLineAll(), countLineSensors));
@@ -66,14 +42,6 @@ void SensorsController::exec(ConnectingDevice *device, String command) {
  }
 }
 
-String SensorsController::intArrayToString(int* array, int size) {
- String str = "";
- for (int i = 0; i < size; i++) {
- str += String(array[i]);
- }
- return str;
-}
-
 void SensorsController::chooseLineSensor(int number) {
  switch (number) {
  case 1:

code/Arduino/SensorsController.h

@@ -31,7 +31,6 @@ class SensorsController: public MainController
  int getLine(int);
  int* getLineAll();
 
- String intArrayToString(int*, int);
  ~SensorsController();
 };
 

code/Arduino/ServoController.cpp

@@ -10,24 +10,57 @@ ServoController::ServoController()
 void ServoController::init() {
  horizontalServo.attach(constants.servo_horizontal_pin);
  verticalServo.attach(constants.servo_vertical_pin);
+ horizontalServo.write(0);
+ verticalServo.write(0);
 }
 
 void ServoController::exec(ConnectingDevice* device, String command) {
-
+ switch (command[1])
+ {
+ case set_horizontal_angle:
+ setHorizontalAngle(parse_command(command, 2, command.length()));
+ break;
+ case set_vertical_angle:
+ setVerticalAngle(parse_command(command, 2, command.length()));
+ break;
+ case set_horiz_vertical_angles:
+ setHorizontalAndVerticalAngle(parse_command(command, 2, constants.commands_delimetr, 2));
+ break;
+ case get_coodrinates:
+ device->send(intArrayToString(getCoordinates(), 2));
+ break;
+ default:
+ break;
+ }
 }
 
 void ServoController::setHorizontalAngle(int angle) {
-
  horizontalServo.write(angle);
+ delay(20);
 }
 
 void ServoController::setVerticalAngle(int angle) {
  verticalServo.write(angle);
+ delay(20);
 }
 
 void ServoController::setHorizontalAndVerticalAngle(int angleX, int angleY) {
  horizontalServo.write(angleX);
  verticalServo.write(angleY);
+ delay(20);
+}
+
+int* ServoController::getCoordinates() {
+ int* arr = new int[2];
+ arr[0] = getHorizontalAngel();
+ arr[1] = getVerticalAngel();
+ return arr;
+}
+
+void ServoController::setHorizontalAndVerticalAngle(int* arr) {
+ horizontalServo.write(arr[0]);
+ verticalServo.write(arr[1]);
+ delay(200);
 }
 
 int ServoController::getHorizontalAngel() {
@@ -38,7 +71,6 @@ int ServoController::getVerticalAngel() {
  return verticalServo.read();
 }
 
-
 ServoController::~ServoController()
 {
 }

code/Arduino/ServoController.h

@@ -2,8 +2,9 @@
 #include <Servo.h>
 #include "Constants.h"
 #include "ConnectingDevice.h"
+#include "MainController.h"
 
-class ServoController
+class ServoController : public MainController
 {
 private:
  Servo horizontalServo;
@@ -17,8 +18,10 @@ class ServoController
  void setHorizontalAngle(int angle);
  void setVerticalAngle(int angle);
  void setHorizontalAndVerticalAngle(int angleX, int angleY);
+ void setHorizontalAndVerticalAngle(int* arr);
  int getHorizontalAngel();
  int getVerticalAngel();
+ int* getCoordinates();
  ~ServoController();
 };
 

code/Arduino/trackPlatform.ino

@@ -4,17 +4,21 @@
 #include "WiFi.h"
 #include "SoftwareSerial.h"
 #include "CommandsController.h"
+#include "ServoController.h"
 
 Constants constants;
 Bluetooth bluetooth(constants.bluetooth_RX, constants.bluetooth_TX, constants.bluetooth_serial_speed);
 WiFi wifi(constants.wifi_RX, constants.wifi_TX, constants.wifi_serial_speed);
 ConnectingDevice *device;
 CommandsController controller;
+ServoController servoController;
 
 bool connected = false;
 
 void setup()
 {
+ servoController.init();
+
  while (!connected) {
  if (bluetooth.isActive()) {
  connected = true;
@@ -28,7 +32,12 @@ void setup()
 }
 
 void loop()
-{
- delay(90); //for sending commands from mobile 
- controller.handle(device, device->read());
+{ 
+ delay(100); //for sending commands from mobile 
+ String command = device->read();
+ if (command[0] == servoControllerID) {
+ servoController.exec(device, command);
+ } else {
+ controller.handle(device, device->read());
+ }
 }