RoboCam + Andurino Uno + CH-05
- Войдите на сайт для отправки комментариев
Втр, 12/01/2021 - 00:17
Здравствуйте!
Хочу подключить своего робота к 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, которые приходят со смартфона. |
016 | const byte CMD_START = 0; //Команда "Старт". |
017 | const byte CMD_CALLSIGN = 1; //Позывной. |
018 | const byte CMD_CTRL = 2; //Изменение состояния контроллеров (джойстиков и/или клавиш). |
019 | const byte CMD_TEST = 3; //Тест соединения. |
020 | const byte CMD_STOP = 255; //Команда "Стоп". |
021 |
022 | //Оси джойтиков. |
023 | const byte AXIS_X = 0; |
024 | const byte AXIS_Y = 1; |
025 | const byte AXIS_W = 2; |
026 | const byte AXIS_Z = 3; |
027 | const byte AXIS_A = 4; |
028 | const byte AXIS_B = 5; |
029 | const byte AXIS_C = 6; |
030 | const byte AXIS_D = 7; |
031 |
032 | //Клавиши, которые нас интересуют. |
033 | //Клавиши вперёд. |
034 | const byte VK_W = 87; |
035 | const byte VK_UP = 38; |
036 | //Клавиши налево. |
037 | const byte VK_A = 65; |
038 | const byte VK_LEFT = 37; |
039 | //Клавиши назад. |
040 | const byte VK_S = 83; |
041 | const byte VK_DOWN = 40; |
042 | //Клавиши направо. |
043 | const byte VK_D = 68; |
044 | const byte VK_RIGHT = 39; |
045 | //Клавиша смотреть наверх. |
046 | const byte VK_Y = 89; |
047 | //Клавиша смотреть вниз. |
048 | const byte VK_H = 72; |
049 |
050 | //Состояние клавиши. |
051 | const byte KEY_PRESSED = 255; //Клавиша нажата. |
052 | const byte KEY_RELEASED = 254; //Клавиша не нажата. |
053 |
054 | unsigned int messageSize = 0; |
055 | unsigned int readMessageBytes = 0; |
056 | byte message[255]; |
057 | bool messageIsTooBig = false ; |
058 |
059 | //Считыает сообщение RoboCam из последовательного порта. |
060 | bool 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 | //Удаляет сообщение. |
087 | void deleteMessage() |
088 | { |
089 | messageSize = 0; |
090 | } |
091 |
092 | //Получает команду из сообщения. |
093 | byte getCommand() |
094 | { |
095 | return message[0]; |
096 | } |
097 |
098 | //Получает строку из сообщения. |
099 | String getString(unsigned int index) |
100 | { |
101 | return ( char *)&message[index]; |
102 | } |
103 |
104 | //Возвращает значение беззнакового байта. |
105 | byte getUByte(unsigned int index) |
106 | { |
107 | return message[index]; |
108 | } |
109 |
110 | //Возвращает значение знакового байта. |
111 | int getSByte(unsigned int index) |
112 | { |
113 | return message[index] > 127 ? message[index] - 256 : message[index]; |
114 | } |
115 |
116 | unsigned int controllerIndex = 1; |
117 |
118 | //Начинает перечисление контроллеров. |
119 | void beginControllerEnumeration() |
120 | { |
121 | controllerIndex = 1; |
122 | } |
123 |
124 | //Проверяет есть ли ещё контроллеры в перечислении. |
125 | bool hasController() |
126 | { |
127 | return controllerIndex < messageSize; |
128 | } |
129 |
130 | //Передвигается к следующему контроллеру в перечислении. |
131 | void gotoNextController() |
132 | { |
133 | if (isJoystickAxis() || isKey()) |
134 | controllerIndex += 2; |
135 | } |
136 |
137 | //Возвращает true, если контроллер - это клавиша. |
138 | bool isKey() |
139 | { |
140 | return message[controllerIndex] == KEY_PRESSED || message[controllerIndex] == KEY_RELEASED; |
141 | } |
142 |
143 | //Возвращает true, если клавиши нажата. |
144 | bool isKeyPressed() |
145 | { |
146 | return message[controllerIndex] == KEY_PRESSED; |
147 | } |
148 |
149 | //Возвращает true, если клавиши не нажата. |
150 | bool isKeyReleased() |
151 | { |
152 | return message[controllerIndex] == KEY_RELEASED; |
153 | } |
154 |
155 | //Возвращает код клавиши. |
156 | byte getKeyCode() |
157 | { |
158 | return isKey() ? message[controllerIndex + 1] : 0; |
159 | } |
160 |
161 | //Возвращает true, если текущий контроллер - это ось джойстика. |
162 | bool isJoystickAxis() |
163 | { |
164 | return message[controllerIndex] < 8; |
165 | } |
166 |
167 | //Возвращает идентификатор оси джойстика. |
168 | byte getJoystickAxisId() |
169 | { |
170 | return isJoystickAxis() ? message[controllerIndex] : -1; |
171 | } |
172 |
173 | String axisNames[] = { "x" , "y" , "w" , "z" , "a" , "b" , "c" , "d" }; |
174 |
175 | //Возвращает имя оси джойстика. |
176 | String getJoystickAxisName() |
177 | { |
178 | return getJoystickAxisId() >= 0 ? axisNames[getJoystickAxisId()] : "" ; |
179 | } |
180 |
181 | //Возвращает значение по оси джойстика. |
182 | int getJoystickAxisValue() |
183 | { |
184 | return isJoystickAxis() ? getSByte(controllerIndex + 1) : 0; |
185 | } |
186 |
187 |
188 | //------------------------------------------------------------------ |
189 | // Функции для формирования и отправки ответов на сообщения RoboCam. |
190 | //------------------------------------------------------------------ |
191 |
192 | unsigned int replySize = 0; //Размер ответа. |
193 | byte reply[20]; //Содержимое ответа. Длинных ответов у нас нет, поэтому хватит 20 байт. |
194 |
195 | //Создаёт новый ответ RoboCam. |
196 | void createReply() |
197 | { |
198 | replySize = 0; |
199 | } |
200 |
201 | //Записывает беззнаковый байт в ответ. |
202 | void writeUByte( byte _byte) |
203 | { |
204 | if (replySize < sizeof (reply)) |
205 | reply[replySize] = _byte; |
206 | replySize++; |
207 | } |
208 |
209 | //Записывает строку в ответ. |
210 | void 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 | //Посылает ответ. |
222 | void sendReply() |
223 | { |
224 | Serial .write(replySize & 0xFF); |
225 | Serial .write((replySize >> 8) & 0xFF); |
226 | Serial .write(reply, replySize); |
227 | } |
228 |
229 | //Создаёт и отправляет ответ, сообщая, что ошибок нет. |
230 | void createAndSendOKReply() |
231 | { |
232 | createReply(); //Создаём ответ. |
233 | writeUByte(0); //Ошибок нет. |
234 | sendReply(); //Отправляем ответ. |
235 | } |
236 |
237 |
238 | //------------------------------------------------------------------ |
239 | // Состояния джойстиков и клавиш. |
240 | //------------------------------------------------------------------ |
241 |
242 | int joystickAxisValues[8]; |
243 | bool pressedKeys[223]; |
244 |
245 |
246 | //------------------------------------------------------------------ |
247 | // Настройка моторов. |
248 | //------------------------------------------------------------------ |
249 |
250 | int rDirPin = 4; |
251 | int rSpeedPin = 5; |
252 |
253 | int lSpeedPin = 6; |
254 | int lDirPin = 7; |
255 |
256 | int maxSpeed = 100; |
257 |
258 | int lSpeed = 0; |
259 | int rSpeed = 0; |
260 | int back = HIGH; |
261 | int forward = LOW; |
262 |
263 | void setupMotorShield() |
264 | { |
265 | pinMode(rDirPin, OUTPUT); |
266 | pinMode(rSpeedPin, OUTPUT); |
267 | pinMode(lDirPin, OUTPUT); |
268 | pinMode(lSpeedPin, OUTPUT); |
269 | } |
270 |
271 | void 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 |
337 | void 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 |
355 | Servo servo; |
356 | int servoMinAngle = 40; //Минимальный угол (>=0) |
357 | int servoMaxAngle = 140; //Максимальный угол (<=179) |
358 | int servoStep = 5; |
359 | unsigned long lastUpdate = 0; |
360 | unsigned long updatePeriod = 100; |
361 | int posByKeyboard = 0; |
362 |
363 | void setupServo() |
364 | { |
365 | servo.attach(9); |
366 | servo.write((servoMaxAngle - servoMinAngle) / 2); |
367 | } |
368 |
369 | void 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 |
392 | void 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 |
401 | void setup () |
402 | { |
403 | clearStoredValues(); |
404 | setupMotorShield(); |
405 | setupServo(); |
406 | Serial .begin(9600); |
407 | } |
408 |
409 | void 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 | } |
300 рублей
Спасибо за готовность помочь! Не нашел, есть ли здесь ЛС. Моя почта: a-anufriev@"яндекс".
Написал. Всё хорошо.