Der Code erlaubt die Bluetooth Fernsteuerung von zweimotorigen Fahrzeugen am Beispiel eines Raddampfers, dessen beide Wasserräder zum Antrieb und zur Lenkung dienen.
Autor: Christian
/* BEERLECADA paddle steamer
* Product: P211106_paddle_steamer
Authors: Christian Rempel
Laurin Kettner
Licence: Beer(red wine) licence
Connections:
Arduino Module
D3 left PWM speed (BEERLECADA Motordriver)
D5 left direction LOW = FORWARD (BEERLECADA Motordriver)
D6 right PWM speed (BEERLECADA Motordriver)
D7 BT RXD PIN (HC05)
D8 BT TXN PIN (HC05)
D9 right direction LOW = FORWARD (BEERLECADA Motordriver)
We are working with the free controller app for android Arduino Joystick available in Google playstore
from uncia robotics.
It has a joystick and 4 buttons, but for this paddle weel ship we are only using
the joystick after all
*/
#include <SoftwareSerial.h>
// more than often two servos from the same manufatucer are working with different movement velocitys
// given the same volt. So here we are defining what bounderies it takes for them to spin in the same speed.
#define leftSpeedLow 56
#define leftSpeedHigh 100
#define rightSpeedLow 126
#define rightSpeedHigh 200
// defines if the arduino is trying to send Debug values to the serial interface
#define isDebugPrinting false
SoftwareSerial BTSerial(7, 8); // CONNECT BT RXD PIN TO ARDUINO 8 PIN | CONNECT BT TXN PIN TO ARDUINO 7 PIN
// after this amount of time without user input the servos auomaicly stop
// this is a nessesary secuite mesure to stop the ship if the bluetooth connection was lost
long joysticktimer;
long printTimer; // timer to print
int leftspeed = 0; // speed of left servo; goes from leftSpeedLow up to leftSpeedHigh
int rightspeed = 0; // speed of right servo; goes from rightSpeedLow up to rightSpeedHigh
int angle = 0; // angle of joystick, cached for median
int strength = 0; // strength of joystick, cached for median
void setup()
{
pinMode(3, OUTPUT);// left speed
pinMode(5, OUTPUT);// left direction LOW = FORWARD
pinMode(6, OUTPUT);// right speed
pinMode(9, OUTPUT);// right direction LOW = FORWARD
//stop everything and set direction to foreward
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
Serial.begin(9600);// serial interface
BTSerial.begin(9600); // HC-05 default speed in AT command more
joysticktimer = millis();
if (isDebugPrinting)
printTimer = millis();
delay(200);
}
void loop()
{
if (BTSerial.available() > 0)
{
joysticktimer = millis(); // reset timer, we got a new value
String val = BTSerial.readStringUntil('#'); // the app starts every new package with a "#"
if ((val.length() == 7) && (isValidNumber(val))) { // check if the message recived was valid
// reads angle and strength and smooths tem since the values highly fluctuate
angle = (int)(angle * 0.9 + val.substring(0, 3).toInt() * 0.1);
strength = (int)(strength * 0.9 + val.substring(3, 6).toInt() * 0.1);
handlejoystick(); //translate the angle and strength values to servo values
}
// the app is likely to send faster than we are working, trash all the
// old recived packages and start with a fresh one next loop
Serial.flush();
}
if ((millis() - joysticktimer) > 300) {
// stops the servos if no user input was given in a short amount of time
angle = 0;
strength = 0;
handlejoystick();
}
if (isDebugPrinting && ((millis() - printTimer) > 1000)) {
Serial.println("Speed left: " + (String)leftspeed + "; right: " + (String)rightspeed);
}
}
// translate the angle and strength values to motor values
// this is not a trivial task since we have only 2 spinning weels to steer
void handlejoystick(void) {
// if it should stop just skip all the calculating and stop imidiently
if (strength <= 0) { //with the app we are using strength goes from 0 to 100, 0 means the joystick is in resting position
leftspeed = 0;
rightspeed = 0;
analogWrite(3, 0);
analogWrite(6, 0);
return;
}
// pin 3 and 5 are the left motor, where 3 is the velocity and 5 the direction
// pin 6 and 9 are the right motor, where 6 is the velocity and 9 the direction
// tracks if we are in the upper / lower half of the joystick witch translates to foreward / backward
bool lower;
float cosinus;
lower = (angle >= 180);//determing the lower half = backwards; angle goes from 0° (leftsense) allaround to 360°
// start with right half of the joy stick pad
if (angle <= 90 || angle >= 270) {
leftspeed = strength;
cosinus = cos(2 * angle * PI / 180);
// if cosinus greater than zero, the right motor turns backwards
// i. e. smaller then 45° or greater then 315°
rightspeed = (int)(strength * (-cosinus));
} else {// left half
rightspeed = strength;
cosinus = cos(2 * angle * PI / 180);
// if the cosinus is greater then zero, the left motor turns backwards
// i. e. smaller greater then 135° and then 225°
leftspeed = (int)(strength * (-cosinus));
}
if (lower) { // moving backwards
leftspeed = -leftspeed;
rightspeed = -rightspeed;
}
// writing direction
if (leftspeed < 0) digitalWrite(5, HIGH);
else digitalWrite(5, LOW);
if (rightspeed < 0) digitalWrite(9, HIGH);
else digitalWrite(9, LOW);
/*
long map(long x, long in_min, long in_max, long out_min, long out_max) {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
*/
leftspeed = sign(leftspeed) * map(abs(leftspeed) * 10, 0, 1000, leftSpeedLow, leftSpeedHigh);
rightspeed = sign (rightspeed) * map(abs(rightspeed) * 10, 0, 1000, rightSpeedLow, rightSpeedHigh);
//writing velocity, while the global variable tracks the direction, only positiv volt values are possible
analogWrite(3, abs(leftspeed));
analogWrite(6, abs(rightspeed));
}
//returns signum of int num
int sign(int num) {
if (signbit(num)) return -1;
else return 1;
}
//returns only true if every char in String is a digit
boolean isValidNumber(String str) {
for (char c : str) {
if (!isDigit(c))
return false;
}
return true;
}