Der neue RP6-ESP
Hier soll der RP6 (genauer der RP6v2-Base) mit einem ESP kombiniert werden.
Hardware
- RP6v2
- Bumper-Board der MultiIO, mit zwei Bumpern und zwei Sharp-IR-Abstandssensoren sowie zugehörigem Transistor
- LED-Platinen der ArduiIO
- auf der Basis werden zwei BC847 aufgebaut, um die Scheinwerfer über die Status-LEDs der RP6-Base zu schalten; die Blinker werden direkt an die Status-LEDs der RP6-Base angeschlossen. Scheinwerfer Front/Heck innen (lila Kabel, IO2), Scheinwerfer Front/Heck außen (grünes Kabel, IO5), Blinker links (oranges Kabel, IO4) und Blinker rechts (gelbes Kabel, IO1).
- Die Bumper (ON-L und ON-R) sollen noch an die IO1 (rechts) und IO5 (links) angeschlossen werden. Außerdem der Transistor vor den Sharp-Abstandssensoren auf LIO1 (dort ist auch der rechte vordere Bumper angeschlossen). Und die beiden Sharp-Abstandssensoren kommen auf ADC0 und ADC1.
- ESP32 mit 4fach-Pegelwandler auf Erweiterung. Der ESP kommuniziert via UART (Rx2/Tx2) mit dem RP6. Die Baudrate beträgt 38.400.
- Der ESP ist wie folgt angeschlossen: Pin Rx2 (GPIO16) via Pegelwandler auf Tx des RP6; Pin Rx2 (GPIO17) via Pegelwandler auf Tx des RP6. Pin D21 auf ST2 des RP6. Pin D19 via Pegelwandler auf ST1 des RP6.
- ST1/ST2 des RP6 starten/resetten den RP6: zieht man ST1 auf +5V (HIGH) startet der RP6. Zieht man den ST2 auf GND (LOW) so resettet er.
Firmware RP6
Wie beschrieben kommt im Wesentlichen die Firmware für den RP6-Base von FabianE.'s Remotrol zum Einsatz. Hunzugefügt wurde in der RP6RobotBaseLib.c der Absatz zu den BackBumpers:
//*********NEW***********
// from: nil.at, RP6-Forum
uint8_t getBackBumperLeft(void)
{
PORTB &= ~SL4; //Schalte StatusLED4 (liegt auf PortB) ab
DDRB &= ~SL4; //Schalte StatusLED4 auf Eingang
nop(); //warte kurz
uint8_t tmp = PINB & SL4; //Lege den Wert von StatusLED4 auf "tmp"
if(statusLEDs.LED4) //wenn StatusLED4 vorher schon an war
{
DDRB |= SL4; //Schalte StatusLED4 auf Ausgang
PORTB |= SL4; //und schalte StatusLED4 wieder an
}
return tmp;
}
uint8_t getBackBumperRight(void)
{
PORTC &= ~SL1; //Schalte StatusLED1 (liegt auf PortC) ab
DDRC &= ~SL1; //Schalte StatusLED1 auf Eingang
nop(); //warte kurz
uint8_t tmp = PINC & SL1; //Lege den Wert von StatusLED1 auf "tmp"
if(statusLEDs.LED1) //wenn StatusLED1 vorher schon an war
{
DDRC |= SL1; //Schalte StatusLED1 auf Ausgang
PORTC |= SL1; //und schalte StatusLED1 wieder an
}
return tmp;
}
Entsprechend wurde auch der BUMPERS_stateChanged_DUMMY(void) angepasst und die RP6RobotBaseLib.h.
Software ESP32
Der auf dem ESP32 laufende Code findet sich am Ende dieses Artikels. Die Software des ESP32 ist derzeit grob mit RemoteXY gemacht. RemoteXY liefert ein schönes GUI für das Smartphone, jedoch mit allerlei Einschränkungen (nur gewisse Bausteine; keine Einbindung eines Kamerabildes; bei mehr als fünf Bausteinen wird es mit 12,99 Euro kostenpflichtig!). Der Restliche Code wurde von Fabian geschrieben und verarbeitet die Inputs des GUI aus RemoteXY und stellt entsprechend Befehle für den RP6 zusammen.
Änderungen und Erweiterungen
Einiges mehr soll noch kommen, hier ein paar Ideen/Pläne:
Hardware
- der ESP32 soll auf eine richtige Erweiterungsplatine, mit Pegelwandler, I2C- und UART-Zugang und USRBUS/XBUS
- Linienfolgermodul der RP6-MultiIO mit Servo, um es hoch- und herunterzufahren
- Akku mit Ladeeinheit
- Ultraschall-Abstandssensor mit Drehturm und Servo
- evtl. ESP32-CAM mit in den Drehturm
- evtl. Scheinwerfer mit in den Drehturm
- Gehäuse
- OLED-Display
- IMU-BreakoutBoard
Firmware RP6
Weitere Änderungen sollen sein: Wenn das ACS (Anti-Collision-System) an ist, soll automatisch beim Rückwärtsfahren das vordere ACS de- und das hintere (die beiden IR-Sharp-Sensoren) aktiviert werden; beim Vorwärtsfahren entsprechend das hintere de- und das vordere aktiviert. Bei einer Drehung auf der Stelle sollen beide aktiv sein.
Außerdem soll bei Kurven der jeweilige Blinker blinken und bei Drehungen auf der Stelle sowie bei Zusammenstößen soll der Warnblinker an gehen.
Software ESP32
- Reset des RP6 (dadurch Stop) nach Verbindungsabbruch
- Anzeige diverser Sensoren, evtl. auf mehreren Seiten/Reitern
- Einbindung des Kamerabildes?
- automatisches ACS, wodurch etwa bei Gefahr automatisch angehalten wird
- Routinen (Lichtfolger, Abstandhalter, Labyrinth, evtl. auch mit AI-Komponenten wie HIER)
Code auf dem ESP32
////////////////////////////////////////////////////////
// RP6 Master on ESP via UART //
////////////////////////////////////////////////////////
/*
RP6-Base Firmware mostly from FabianE. Remotroll V1.3
This software runs on an ESP32 and communicates with
the RP6-Base via UART. The RP6 has to run the
Remotrol-v1.3-Slave Demo from FabianE.!
Firmware changes on the RP6-Base Slave:
[+] added Bumpers left/right at the back to SL4 & SL1, respectively
[+] added Bumpers back and ADC0/ADC1 to UART-output
[-] indicators to SL1 & SL4 (automatic indicators when driving left/right and warning when turning)
[-] switching ACS/Sharps on/off when going forward/backward
Hardware:
[-] adding better battery
[-] adding US-sensor with Servo
[-] soadding IR-Sensors to the back, connected to ADC0/1
[+] adding lights/indicators
[-] housing
[-] line-follower with Servo
[-] maybe charging circuit (wireless?)
[-] Board for ESP32 with XBUS (+5V, GND, evtl INT&MRESET) und USBUS (ST1/ST2, RX/TX)
Pegelwandler, IMU (i2c), ADC (i2c),
Software on the ESP32:
[+] reads the UI from RemoteXY
[+] Sends commands for Speed and lights
[+] Switch for Starting/Stopping the RP6-Base (via IO on ST1 and ST2 pins)
[+] reading UART from RP6
[-] sending Info to RemoteXY (Battery, Bumpers, ACS, Speed, Power, Light,... )
[-] adding a beeper to the ESP32
[-] adding a display to the ESP32
[-] adding some routines (light-follow, line-follow, ...)
[-] ESP32-Cam??? Any possible?
[-] stop and/or reset RP6 when Connection lost/failed
*/
/*
-- New project --
This source code of graphical user interface
has been generated automatically by RemoteXY editor.
To compile this code using RemoteXY library 3.1.8 or later version
download by link http://remotexy.com/en/library/
To connect using RemoteXY mobile app by link http://remotexy.com/en/download/
- for ANDROID 4.11.1 or later version;
- for iOS 1.9.1 or later version;
This source code is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
//------------ Command Defines ------------
#define CMD_SET_SPEED 1
#define CMD_SET_SERVO 2
#define CMD_SET_LEDS 3 //#3:1:2:id* switches LED "2" on RP6Base ("1") ; #3:1:0:id*
#define CMD_SET_BEEP 4
#define CMD_SET_START_MELODY 5
#define CMD_SET_FEATURE 6
#define CMD_SET_STOP 7
#define CMD_SET_CONNECTION_SPEED 8
#define CMD_SET_MELODY 9
#define CMD_SET_ACSPOWER 10
#define CMD_SET_TEST 11
#define CMD_SET_CMD_SET_IRCOM 12
#define CMD_RESET_ID_COUNTER 99
#define CMD_DIR_FWD 0
#define CMD_DIR_BWD 1
#define CMD_DIR_TURN_L 2
#define CMD_DIR_TURN_R 3
#define CMD_FLAG_STP 0
#define CMD_FLAG_MOVE 1
#define CMD_RP6_BASE 1 // For LEDs, Servos, ...
#define CMD_RP6_M32 2 // For LEDs, Servos, ...
#define sLED1 1 // Status LED 1 (IO1) on RP6Base, Bumper back right & blinker right (yellow)
#define sLED2 2 // Status LED 2 (IO2) on RP6Base, nothing as input & light low (purple)
#define sLED3 4 // Status LED 3 (LIO1) on RP6Base, Bumper front right & IR Back on/off
#define sLED4 8 // Status LED 4 (IO5) on RP6Base, Bumper back left & blinker left (orange)
#define sLED5 16 // Status LED 5 (IO4) on RP6Base, nothing as input & light high (green)
#define sLED6 32 // Status LED 6 (LIO2) on RP6Base, Bumper front left & nothing as output
// UART2 defines; default ist RXD2=16, TXD2=17 but you can map it to any pin
#define RXD2 16 // for second uart
#define TXD2 17 // for second uart
//////////////////////////////////////////////
// RemoteXY include library //
//////////////////////////////////////////////
// RemoteXY select connection mode and include library
#define REMOTEXY_MODE__ESP32CORE_WIFI
#include <WiFi.h>
#include <RemoteXY.h>
// RemoteXY connection settings
#define REMOTEXY_WIFI_SSID "Anders"
#define REMOTEXY_WIFI_PASSWORD "stopsl.12"
#define REMOTEXY_SERVER_PORT 6377
// RemoteXY configurate
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] = // 61 bytes
{ 255, 4, 0, 22, 0, 54, 0, 16, 31, 0, 5, 47, 52, 14, 39, 39, 2, 26, 31, 66,
1, 11, 41, 7, 16, 2, 26, 67, 5, 29, 4, 43, 6, 2, 26, 21, 3, 131, 11, 16,
33, 12, 2, 26, 2, 1, 22, 45, 18, 8, 2, 26, 31, 31, 79, 78, 0, 79, 70, 70,
0
};
// this structure defines all the variables and events of your control interface
struct {
// input variables
int8_t joystick_x; // from -100 to 100
int8_t joystick_y; // from -100 to 100
uint8_t lights; // =0 if select position A, =1 if position B, =2 if position C, ...
uint8_t switch_main; // =1 if switch ON and =0 if OFF
// output variables
int8_t battery_level; // =0..100 level position
char text[21]; // string UTF8 end zero
// other variable
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
/////////////////////////////////////////////
// END RemoteXY include //
/////////////////////////////////////////////
// Variables for the RP6
int oldJoystick_x = 0;// Joystick x-Achse
int oldJoystick_y = 0;// Joystick y-Achse
int oldlights = 0; // Lichtschalter (dreifach: a=0, b=1, c=2)
int oldswitch_main = 0; // Hauptschalter, startet und resettet den RP6-Base (high auf ST1 startet, low auf ST2 resettet)
int id_counter = 0; // Counter for commands
int desSpeedL = 0; // desired speed left
int desSpeedR = 0; // desired speed right
int cmd_dir; // direction of movements
String command; // command that goes to the RP6 via UART (as a char/string)
int cmd_leds; // enthält den Wert (binär) der LEDs, 32 etwa entspricht 100000 und 1 entspricht 000001
const int startPin = 19; // high auf ST1 startet den RP6-Baser
const int stopPin = 21; // low auf ST2 stoppt den RP6-Base
// Variables for receiving data
String dataReceived;
int dataArray[15]; // array for the data from the RP6
enum state { //enumerate the items in the data from the RP6
Bat,
SpeedL,
SpeedR,
PowerL,
PowerR,
LightL,
LightR,
BumpL,
BumpR,
ObsL,
ObsR,
ADC0,
ADC1,
BumpHL,
BumpHR
};
void sendCommand(int cmd_flag)
{
switch (cmd_flag) {
case CMD_SET_STOP:
command = "#" + String(CMD_SET_STOP) + ":" + String(id_counter) + "*";
break;
case CMD_SET_SPEED:
command = "#" + String(CMD_SET_SPEED) + ":" + String(desSpeedL) + ":" + String(desSpeedR) + ":" + String(cmd_dir) + ":" + String(id_counter) + "*";
break;
case CMD_SET_LEDS:
command = "#" + String(CMD_SET_LEDS) + ":" + String(CMD_RP6_BASE) + ":" + String(cmd_leds) + ":" + String(id_counter) + "*";
break;
}
if (id_counter >= 99) {
command = "#" + String(CMD_RESET_ID_COUNTER) + ":" + String(99) + "*";
id_counter = 0;
}
id_counter++;
//Serial.println(command);
Serial2.println(command);
}
void ui_read_Handler()
{
if (abs(oldJoystick_x - RemoteXY.joystick_x) > 10 || abs(oldJoystick_y - RemoteXY.joystick_y) > 10) {
// nur wenn nennenswerte Änderung am Joystick ist; überschreibe dann erst mal die jeweilige Achse:
if (abs(oldJoystick_x - RemoteXY.joystick_x) > 10) {
oldJoystick_x = RemoteXY.joystick_x;
}
if (abs(oldJoystick_y - RemoteXY.joystick_y) > 10) {
oldJoystick_y = RemoteXY.joystick_y;
}
// dann Richtung definieren
if (RemoteXY.joystick_x > 10 && abs(RemoteXY.joystick_y) < 10) {
//Drehung rechts
cmd_dir = CMD_DIR_TURN_R;
desSpeedL = RemoteXY.joystick_x;
desSpeedR = RemoteXY.joystick_x;
sendCommand(CMD_SET_SPEED);
}
else if (RemoteXY.joystick_x < -10 && abs(RemoteXY.joystick_y) < 10) {
//Drehung links
cmd_dir = CMD_DIR_TURN_L;
abs(desSpeedL = abs(RemoteXY.joystick_x));
abs(desSpeedR = abs(RemoteXY.joystick_x));
sendCommand(CMD_SET_SPEED);
}
else if (RemoteXY.joystick_y >= 10) {
//Vorwärts, evtl. mit Kurve
cmd_dir = CMD_DIR_FWD;
desSpeedL = abs(RemoteXY.joystick_y + RemoteXY.joystick_x / 2);
desSpeedR = abs(RemoteXY.joystick_y - RemoteXY.joystick_x / 2);
sendCommand(CMD_SET_SPEED);
}
else if (RemoteXY.joystick_y <= -10) {
//Rückwärts, evtl. mit Kurve
cmd_dir = CMD_DIR_BWD;
desSpeedL = abs(RemoteXY.joystick_y - RemoteXY.joystick_x / 2);
desSpeedR = abs(RemoteXY.joystick_y + RemoteXY.joystick_x / 2);
sendCommand(CMD_SET_SPEED);
}
else {
// sonst stoppen!
sendCommand(CMD_SET_STOP);
}
}
if (RemoteXY.lights != oldlights) {
oldlights = RemoteXY.lights;
switch (RemoteXY.lights) {
case 0:
cmd_leds = 0;
break;
case 1:
cmd_leds = sLED2;
break;
case 2:
cmd_leds = sLED2 | sLED4;
break;
}
sendCommand(CMD_SET_LEDS);
}
if (RemoteXY.switch_main != oldswitch_main) {
oldswitch_main = RemoteXY.switch_main;
if (RemoteXY.switch_main) { //starte RP6
id_counter = 0; // id_counter zurücksetzen!
digitalWrite(startPin, HIGH);
delay(200);
digitalWrite(startPin, LOW);
}
else { //stoppe RP6
emergencyStop();
}
}
}
void emergencyStop(){
digitalWrite(stopPin, LOW);
delay(200);
digitalWrite(stopPin, HIGH);
}
// Proceed with new incoming data from Serial2
void readSerial(){
if(Serial2.available()){
char c = Serial2.read();
if(c == '\n'){
parseDataReceived(dataReceived);
dataReceived = "";
}
else {
dataReceived += c;
}
}
}
void parseDataReceived(String dataRec){
String part1;
String part2;
if (dataRec == "[DATA]" || dataRec == "[/DATA]"){
//nothing
}
else {
part1 = dataRec.substring(0, dataRec.indexOf(":"));
part2 = dataRec.substring(dataRec.indexOf(":")+1);
if (part1 == "Bat"){
dataArray[Bat] = part2.toInt();
} else if (part1 == "SpeedL"){
dataArray[SpeedL] = part2.toInt();
} else if (part1 == "SpeedR"){
dataArray[SpeedR] = part2.toInt();
} else if (part1 == "PowerL"){
dataArray[PowerL] = part2.toInt();
} else if (part1 == "PowerR"){
dataArray[PowerR] = part2.toInt();
} else if (part1 == "LightL"){
dataArray[LightL] = part2.toInt();
} else if (part1 == "LightR"){
dataArray[LightR] = part2.toInt();
} else if (part1 == "BumpL"){
dataArray[BumpL] = part2.toInt();
} else if (part1 == "BumpR"){
dataArray[BumpR] = part2.toInt();
} else if (part1 == "ObsL"){
dataArray[ObsL] = part2.toInt();
} else if (part1 == "ObsR"){
dataArray[ObsR] = part2.toInt();
} else if (part1 == "ADC0"){
dataArray[ADC0] = part2.toInt();
} else if (part1 == "ADC1"){
dataArray[ADC1] = part2.toInt();
} else if (part1 == "BumpHL"){
dataArray[BumpHL] = part2.toInt();
} else if (part1 == "BumpHR"){
dataArray[BumpHR] = part2.toInt();
}
data2ui();
}//end if-else
}
void data2ui(){
RemoteXY.battery_level = map(dataArray[LightL],0,1023,0,100);
}
void setup()
{
pinMode(startPin, OUTPUT);
pinMode(stopPin, OUTPUT);
digitalWrite(stopPin, HIGH);
digitalWrite(startPin, LOW);
RemoteXY_Init ();
Serial.begin(9600); //serial for PC-Com
Serial2.begin(38400, SERIAL_8N1, RXD2, TXD2); //second Serial on pins 16/17 for RP6-Com
}
void loop()
{
RemoteXY_Handler ();
if (RemoteXY.connect_flag == 0){
emergencyStop();
}
ui_read_Handler();
readSerial();
}