RoboCam + Andurino Uno + CH-05

Войдите на сайт для отправки комментариев

3 ответа [Последнее сообщение]

Втр, 12/01/2021 - 00:17

anufriev83

Offline

Зарегистрирован: 11.01.2021

Здравствуйте!

Хочу подключить своего робота к RoboCam http://www.proghouse.ru/article-box/128-robocam-arduino#

при компиляции скетча с этого сайта возникают ошибки:

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino: In function 'void loop()':

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino:437:35: warning: ISO C++ forbids converting a string constant to 'char*' [-Wwrite-strings]

writeString("Researcher");

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino: In function 'clearStoredValues()':

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino:396:27: warning: iteration 8 invokes undefined behavior [-Waggressive-loop-optimizations]

joystickAxisValues[i] = 0;

~~~~~~~~~~~~~~~~~~~~~~^~~

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino:395:21: note: within this loop

for (int i = 0; i < sizeof(joystickAxisValues); i++)

~~^~~~~~~~~

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino: In function 'clearStoredValues':

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino:396:27: warning: iteration 8 invokes undefined behavior [-Waggressive-loop-optimizations]

joystickAxisValues[i] = 0;

C:\ARDUINO\ROBO_Cam\ROBO_Cam.ino:395:21: note: within this loop

for (int i = 0; i < sizeof(joystickAxisValues); i++)

Нужна помощь специалиста.

За помощь в реализации подключения и управления как на сайте готов поблагодарить 500 руб. Или ваша цена.

Сам скетч:

001/*
002 * Автор: Алексей Валуев.
003 * Дата создания: 15.06.2017.
004 * Описание: Скрипт для управления колёсным Arduino-роботом с управляемым держателем смартфона с помощью приложения RoboCam.
005 * Приложение RoboCam в Google Play: <a href="https://play.google.com/store/apps/details?id=ru.proghouse.robocam" title="https://play.google.com/store/apps/details?id=ru.proghouse.robocam" rel="nofollow">https://play.google.com/store/apps/details?id=ru.proghouse.robocam</a>
006 * Статьи о приложении RoboCam: <a href="http://www.proghouse.ru/tags/robocam" title="http://www.proghouse.ru/tags/robocam" rel="nofollow">http://www.proghouse.ru/tags/robocam</a>
007*/
008 
009#include <math.h>
010 
011//------------------------------------------------------------------
012// Функции для чтения сообщений RoboCam.
013//------------------------------------------------------------------
014 
015//Команды RoboCam, которые приходят со смартфона.
016const byte CMD_START    = 0;    //Команда "Старт".
017const byte CMD_CALLSIGN = 1;    //Позывной.
018const byte CMD_CTRL     = 2;    //Изменение состояния контроллеров (джойстиков и/или клавиш).
019const byte CMD_TEST     = 3;    //Тест соединения.
020const byte CMD_STOP     = 255;  //Команда "Стоп".
021 
022//Оси джойтиков.
023const byte AXIS_X = 0;
024const byte AXIS_Y = 1;
025const byte AXIS_W = 2;
026const byte AXIS_Z = 3;
027const byte AXIS_A = 4;
028const byte AXIS_B = 5;
029const byte AXIS_C = 6;
030const byte AXIS_D = 7;
031 
032//Клавиши, которые нас интересуют.
033//Клавиши вперёд.
034const byte VK_W     = 87;
035const byte VK_UP    = 38;
036//Клавиши налево.
037const byte VK_A     = 65;
038const byte VK_LEFT  = 37;
039//Клавиши назад.
040const byte VK_S     = 83;
041const byte VK_DOWN  = 40;
042//Клавиши направо.
043const byte VK_D     = 68;
044const byte VK_RIGHT = 39;
045//Клавиша смотреть наверх.
046const byte VK_Y     = 89;
047//Клавиша смотреть вниз.
048const byte VK_H     = 72;
049 
050//Состояние клавиши.
051const byte KEY_PRESSED  = 255; //Клавиша нажата.
052const byte KEY_RELEASED = 254; //Клавиша не нажата.
053 
054unsigned int messageSize = 0;
055unsigned int readMessageBytes = 0;
056byte message[255];
057bool messageIsTooBig = false;
058 
059//Считыает сообщение RoboCam из последовательного порта.
060bool readMessage()
061{
062  //Читаем размер сообщения.
063  if (messageSize == 0 && Serial.available() > 1)
064  {
065    messageSize = Serial.read() + (Serial.read() << 8);
066    readMessageBytes = 0;
067  }
068 
069  //Считыаем сообщение.
070  //Сообщение забираем из Serial полностью, даже если оно больше массива "message".
071  //Если сообщение больше массива "message", выставляем флаг messageIsTooBig в true.
072  if (messageSize > 0 && readMessageBytes < messageSize && Serial.available() > 0)
073    while (readMessageBytes < messageSize && Serial.available() > 0)
074    {
075      byte b = Serial.read();
076      if (readMessageBytes < sizeof(message))
077        message[readMessageBytes] = b;
078      else
079        messageIsTooBig = true;
080      readMessageBytes++;
081    }
082 
083   return messageSize > 0 && readMessageBytes == messageSize;
084}
085 
086//Удаляет сообщение.
087void deleteMessage()
088{
089  messageSize = 0;
090}
091 
092//Получает команду из сообщения.
093byte getCommand()
094{
095  return message[0];
096}
097 
098//Получает строку из сообщения.
099String getString(unsigned int index)
100{
101  return (char*)&message[index];
102}
103 
104//Возвращает значение беззнакового байта.
105byte getUByte(unsigned int index)
106{
107  return message[index];
108}
109 
110//Возвращает значение знакового байта.
111int getSByte(unsigned int index)
112{
113  return message[index] > 127 ? message[index] - 256 : message[index];
114}
115 
116unsigned int controllerIndex = 1;
117 
118//Начинает перечисление контроллеров.
119void beginControllerEnumeration()
120{
121  controllerIndex = 1;
122}
123 
124//Проверяет есть ли ещё контроллеры в перечислении.
125bool hasController()
126{
127  return controllerIndex < messageSize;
128}
129 
130//Передвигается к следующему контроллеру в перечислении.
131void gotoNextController()
132{
133  if (isJoystickAxis() || isKey())
134    controllerIndex += 2;
135}
136 
137//Возвращает true, если контроллер - это клавиша.
138bool isKey()
139{
140  return message[controllerIndex] == KEY_PRESSED || message[controllerIndex] == KEY_RELEASED;
141}
142 
143//Возвращает true, если клавиши нажата.
144bool isKeyPressed()
145{
146  return message[controllerIndex] == KEY_PRESSED;
147}
148 
149//Возвращает true, если клавиши не нажата.
150bool isKeyReleased()
151{
152  return message[controllerIndex] == KEY_RELEASED;
153}
154 
155//Возвращает код клавиши.
156byte getKeyCode()
157{
158  return isKey() ? message[controllerIndex + 1] : 0;
159}
160 
161//Возвращает true, если текущий контроллер - это ось джойстика.
162bool isJoystickAxis()
163{
164  return message[controllerIndex] < 8;
165}
166 
167//Возвращает идентификатор оси джойстика.
168byte getJoystickAxisId()
169{
170  return isJoystickAxis() ? message[controllerIndex] : -1;
171}
172 
173String axisNames[] = {"x", "y", "w", "z", "a", "b", "c", "d"};
174 
175//Возвращает имя оси джойстика.
176String getJoystickAxisName()
177{
178  return getJoystickAxisId() >= 0 ? axisNames[getJoystickAxisId()] : "";
179}
180 
181//Возвращает значение по оси джойстика.
182int getJoystickAxisValue()
183{
184  return isJoystickAxis() ? getSByte(controllerIndex + 1) : 0;
185}
186 
187 
188//------------------------------------------------------------------
189// Функции для формирования и отправки ответов на сообщения RoboCam.
190//------------------------------------------------------------------
191 
192unsigned int replySize = 0; //Размер ответа.
193byte reply[20]; //Содержимое ответа. Длинных ответов у нас нет, поэтому хватит 20 байт.
194 
195//Создаёт новый ответ RoboCam.
196void createReply()
197{
198  replySize = 0;
199}
200 
201//Записывает беззнаковый байт в ответ.
202void writeUByte(byte _byte)
203{
204  if (replySize < sizeof(reply))
205    reply[replySize] = _byte;
206   replySize++;
207}
208 
209//Записывает строку в ответ.
210void writeString(char* str)
211{
212  int index = 0;
213  while (str[index] != 0)
214  {
215    writeUByte((byte)str[index]);
216    index++;
217  }
218  writeUByte(0);
219}
220 
221//Посылает ответ.
222void sendReply()
223{
224  Serial.write(replySize & 0xFF);
225  Serial.write((replySize >> 8) & 0xFF);
226  Serial.write(reply, replySize);
227}
228 
229//Создаёт и отправляет ответ, сообщая, что ошибок нет.
230void createAndSendOKReply()
231{
232  createReply(); //Создаём ответ.
233  writeUByte(0); //Ошибок нет.
234  sendReply(); //Отправляем ответ.
235}
236 
237 
238//------------------------------------------------------------------
239// Состояния джойстиков и клавиш.
240//------------------------------------------------------------------
241 
242int joystickAxisValues[8];
243bool pressedKeys[223];
244 
245 
246//------------------------------------------------------------------
247// Настройка моторов.
248//------------------------------------------------------------------
249 
250int rDirPin = 4;
251int rSpeedPin = 5;
252 
253int lSpeedPin = 6;
254int lDirPin = 7;
255 
256int maxSpeed = 100;
257 
258int lSpeed = 0;
259int rSpeed = 0;
260int back = HIGH;
261int forward = LOW;
262 
263void setupMotorShield()
264{
265    pinMode(rDirPin, OUTPUT);
266    pinMode(rSpeedPin, OUTPUT);
267    pinMode(lDirPin, OUTPUT);
268    pinMode(lSpeedPin, OUTPUT);
269}
270 
271void updateMotorStates()
272{
273  //Считаем скорость моторов в зависимости от координат по осям джойстиков x и y.
274  int x = joystickAxisValues[AXIS_X];
275  int y = joystickAxisValues[AXIS_Y];
276  double angle = atan2(y, x) * 180.0 / M_PI;
277  double d = sqrt(x * x + y * y); //Расстояние от центра окружности до точки
278  if (d > 100)
279      d = 100;
280  double L = 0, R = 0;
281  if (angle >= 0 && angle <= 90) {
282      R = angle / 90 * 201 - 100;
283      L = 100;
284  } else if (angle < 0 && angle >= -90) {
285      L = (angle / 90 * -201 - 100) * -1;
286      R = -100;
287  } else if (angle > 90 && angle <= 180) {
288      L = ((angle - 90) / 90 * 201 - 100) * -1;
289      R = 100;
290  } else if (angle < -90 && angle >= -180) {
291      R = (angle + 90) / 90 * -201 - 100;
292      L = -100;
293  }
294  rSpeed = (int) round(maxSpeed * R / 100 * d / 100);
295  lSpeed = (int) round(maxSpeed * L / 100 * d / 100);
296  //Считаем скорость в зависимости от нажатых клавиш.
297  int rSpeedBtn = 0;
298  int lSpeedBtn = 0;
299  bool up   = pressedKeys[VK_W] || pressedKeys[VK_UP];
300  bool down = pressedKeys[VK_S] || pressedKeys[VK_DOWN];
301  bool left = pressedKeys[VK_A] || pressedKeys[VK_LEFT];
302  bool right = pressedKeys[VK_D] || pressedKeys[VK_RIGHT];
303  if (up)
304  {
305    rSpeedBtn += maxSpeed;
306    lSpeedBtn += maxSpeed;
307  }
308  if (down)
309  {
310    rSpeedBtn -= maxSpeed;
311    lSpeedBtn -= maxSpeed;
312  }
313  if (left && !right)
314    if (up || down)
315      lSpeedBtn = 0;
316    else
317    {
318      lSpeedBtn -= maxSpeed;
319      rSpeedBtn += maxSpeed;
320    }
321  if (right && !left)
322    if (up || down)
323      rSpeedBtn = 0;
324    else
325    {
326      lSpeedBtn += maxSpeed;
327      rSpeedBtn -= maxSpeed;
328    }
329  rSpeed += rSpeedBtn;
330  lSpeed += lSpeedBtn;
331  rSpeed = max(min(rSpeed, maxSpeed), -maxSpeed);
332  lSpeed = max(min(lSpeed, maxSpeed), -maxSpeed);
333  updateMotorState(rDirPin, rSpeedPin, rSpeed);
334  updateMotorState(lDirPin, lSpeedPin, lSpeed * -1);
335}
336 
337void updateMotorState(int dirPin, int speedPin, int speed)
338{
339  if (speed == 0)
340    analogWrite(speedPin, 0);
341  else
342  {
343    analogWrite(speedPin, abs(speed));
344    digitalWrite(dirPin, speed > 0 ? forward : back);
345  }
346}
347 
348 
349//------------------------------------------------------------------
350// Настройка сервомотора.
351//------------------------------------------------------------------
352 
353#include <Servo.h>
354 
355Servo servo;
356int servoMinAngle = 40;//Минимальный угол (>=0)
357int servoMaxAngle = 140;//Максимальный угол (<=179)
358int servoStep = 5;
359unsigned long lastUpdate = 0;
360unsigned long updatePeriod = 100;
361int posByKeyboard = 0;
362 
363void setupServo()
364{
365  servo.attach(9);
366  servo.write((servoMaxAngle - servoMinAngle) / 2);
367}
368 
369void updateServoState()
370{
371  unsigned long currentTime = millis();
372  //Считаем угол поворота сервомотора, в зависимости от координат джойстика.
373  int posByJoystick = round((joystickAxisValues[AXIS_Z] + 100.0) * (servoMaxAngle - servoMinAngle) / 200.0) + servoMinAngle;
374  //Если нажата клавиша Y или H и прошло более updatePeriod миллисекунд, считаем меняем относительный угол поворота.
375  if (((pressedKeys[VK_Y] || pressedKeys[VK_H]) && pressedKeys[VK_Y] != pressedKeys[VK_H])
376      && (lastUpdate == 0 || lastUpdate > currentTime || currentTime - lastUpdate > updatePeriod))
377  {
378    lastUpdate = currentTime;
379    posByKeyboard += pressedKeys[VK_Y] ? servoStep : -servoStep;
380    posByKeyboard = max(min(posByKeyboard, (servoMaxAngle - servoMinAngle) / 2), (servoMinAngle - servoMaxAngle) / 2);
381  }
382  //Считаем новый угол поворота сервомотора.
383  int newPos = max(min(posByJoystick + posByKeyboard, servoMaxAngle), servoMinAngle);
384  //Поворачиваем сервомотор.
385  servo.write(newPos);
386}
387 
388//------------------------------------------------------------------
389// Основной код.
390//------------------------------------------------------------------
391 
392void clearStoredValues()
393{
394  posByKeyboard = 0;
395  for (int i = 0; i < sizeof(joystickAxisValues); i++)
396    joystickAxisValues[i] = 0;
397  for (int i = 0; i < sizeof(pressedKeys); i++)
398    pressedKeys[i] = false;
399}
400 
401void setup()
402{
403  clearStoredValues();
404  setupMotorShield();
405  setupServo();
406  Serial.begin(9600);
407}
408 
409void loop() 
410{
411  if (readMessage())
412  {
413    switch (getCommand())
414    {
415      case CMD_START: //Начало работы.
416        //Создаём ответ.
417        createReply();
418        //Ошибок нет.
419        writeUByte(0);
420        //Контрольный байт. Нужно вернуть полученное значение увеличенное на 1.
421        writeUByte(message[1] + 1);
422        //Версия протокола общения, которую мы будем использовать.
423        writeUByte(1);
424        //Кодировка для строк US-ASCII.
425        writeUByte(0);
426        //Отправляем ответ.
427        sendReply();
428        break;
429      case CMD_CALLSIGN: //Запрос отзыва на позывной.
430        //Создаём ответ.
431        createReply();
432        if (getString(1) == "RoboCam") //Если позывной "RoboCam"...
433        {
434          //Ошибок нет.
435          writeUByte(0);
436          //Ответ "Researcher".
437          writeString("Researcher");
438        }
439        else
440          //Ошибка, т.к. получен неправильный позывной.
441          writeUByte(1);
442        //Отправляем ответ.
443        sendReply();
444        break;
445      case CMD_CTRL: //Изменения состояний контроллеров.
446        beginControllerEnumeration(); //Создаём перечисление состояний контроллеров.
447        while (hasController()) //Если есть контроллер.
448        {
449          if (isJoystickAxis()) //Если контроллер - это ось джойстика.
450            joystickAxisValues[getJoystickAxisId()] = getJoystickAxisValue();
451          else if (isKey()) //Если контроллер - это клавиша.
452          {
453            if (isKeyPressed()) //Если клавиша нажата.
454              pressedKeys[getKeyCode()] = true;
455            else if (isKeyReleased()) //Если клавиша не нажата.
456              pressedKeys[getKeyCode()] = false;
457          }
458          gotoNextController(); //Переходим к следующему контролеру.
459        }
460        //Обновляем состояния моторов.
461        updateMotorStates();
462        //Отправляем ответ.
463        createAndSendOKReply();
464        break;
465      case CMD_TEST: //Проверка подключения.
466        //Отправляем ответ.
467        createAndSendOKReply();
468        break;
469      case CMD_STOP: //Остановка робота.
470        //Обнуляем координаты осей джойстиков и состояния клавиш.
471        clearStoredValues();
472        //Обновляем состояния моторов.
473        updateMotorStates();
474        //Отправляем ответ.
475        createAndSendOKReply();
476        break;
477    }
478    //Удаляем сообщение.
479    deleteMessage();
480  }
481  //Обновляем состояние сервомотора.
482  updateServoState();
483}

Втр, 12/01/2021 - 00:47

Бармалей

Offline

Зарегистрирован: 23.09.2019

300 рублей

Втр, 12/01/2021 - 10:01

anufriev83

Offline

Зарегистрирован: 11.01.2021

Спасибо за готовность помочь! Не нашел, есть ли здесь ЛС. Моя почта: a-anufriev@"яндекс".

Втр, 12/01/2021 - 23:01

Бармалей

Offline

Зарегистрирован: 23.09.2019

Написал. Всё хорошо.

Arduino.ru

Регистрация новых пользователей и создание новых тем теперь только на новом форуме https://forum.arduino.ru

RoboCam + Andurino Uno + CH-05