/*-----------------------------------------------------------------------------------------
* This software is in the public domain, furnished "as is", without technical
* support, and with no warranty, express or implied, as to its usefulness for
* any purpose.
*
* DualDriveMix3CH.ino
*
* Dual Drive Mixer for a RC vehicles with two drives controlled with PWM motorshields
* based on BTS7960 dual-h-bridges. The adapted BTS7960 arduino Lib for the ESP32 architecture
* (BTS7960ESP32.h, BTS7960ESP32.ccp) must be available to build this binary.
*
* The functions need the RCPWMRead functions (based on pwmread_rcfailsafe.ino
* from Kelvin Nelson and Rc_Engine_Sound_ESP32.ino from TheDIYGuy999) to work.
* Pulse Width Modulation (PWM) decoding of RC Receiver (RCPWMRead.ino) must be placed
* in same directory to build the binary. Also the calibration steps of the RC inputs must
* be done as described in pwmread_rcfailsafe.ino
*
* 3 RC Channels are used for the mixer, because we have two different modes. At startup
* the manual mode is predefined to controll the two motors direct.
* The not used RC channel will trigger the mode switch if both motors are on neutral.
* In mixed mode one RC channel is controlling the direction and the other RC Channel is
* controlling the speed.
* Definitions for deadzones and agility of steering are forseen and should be adjusted.
*
* Author: Volker Frauenstein
* Date: 13/01/2021 Version 1.3: interface adaptation for blinking RGB-LED
*
* Date: 06/01/2021 Version 1.2: added function to interface to sound (power)
* Date: 05/01/2021 Version 1.1: migration to ESP32 architecture including
* ESP32 Lib for BTS7960, set_LEDstate calls added
* Date: 12/12/2020 Version 1.0: Dual Drive Mixer implementation for Arduino Nano
*/
// Definitions to handle output signals to BTS7960 motorshilds
#include "BTS7960ESP32.h" // include Lib to acces BST7960 Motorshilds
// definition PWM properties
const int freq = 2000; // Frequenz of pwm signal output, most efficient is 1-2kHz, but it could howl
// definition for pintout of motor shield 1 (for right hand side drive)
const uint8_t L_PWM_1 = 15; // must be a PWM output pin
const uint8_t L_pwmChannel_1 = 0; // with ESP32 a pwm channel must be defined
const uint8_t R_PWM_1 = 2; // must be a PWM output pin
const uint8_t R_pwmChannel_1 = 1; // with ESP32 a pwm channel must be defined
const uint8_t L_EN_1 = 4; // a digital output pin will do
const uint8_t R_EN_1 = 16; // a digital output pin will do
// definition motor shield 1 on right hand side -> must turn left to go forward
BTS7960ESP32 MotorR(L_EN_1, R_EN_1, L_PWM_1, R_PWM_1, freq, L_pwmChannel_1, R_pwmChannel_1);
// definition pitout of motor shield 2 (for left hand side drive)
const uint8_t L_PWM_2 = 17; // must be a PWM output pin
const uint8_t L_pwmChannel_2 = 2; // with ESP32 a pwm channel must be defined
const uint8_t R_PWM_2 = 5; // must be a PWM output pin
const uint8_t R_pwmChannel_2 = 3; // with ESP32 a pwm channel must be defined
const uint8_t L_EN_2 = 18; // a digital output pin will do
const uint8_t R_EN_2 = 19; // a digital output pin will do
// definition motor shield 2 on left hand side -> must turn right to go forward
BTS7960ESP32 MotorL(L_EN_2, R_EN_2, L_PWM_2, R_PWM_2, freq, L_pwmChannel_2, R_pwmChannel_2);
// definitions for adjustable values
const float deadzoneUp = 0.07; // upper RC Channel Deadzone of 7%
const float deadzoneDown = -0.07; // lower RC Channel Deadzone of 7%
const float TdeadzoneUp = 0.50; // upper RC Channel Deadzone for ModeChange trigger of 50%
const float TdeadzoneDown = -0.50; // lower RC Channel Deadzone for ModeChange trigger of 50%
const float agilityI = 2; // factor for MixedMode to adjust agility of steering for inner side drive (of turn); value > 1 means reverse rotation at higer steering angle
const float agilityO = 0.75; // factor for MixedMode to adjust agility of steering for outer side drive (of turn)
// generig definitions
// #define DEBUG_MIX 1
bool MixMode = false; // initialize for running mode of drive mixer to manuel-mode
bool ModeChangeTrigger = false; // flag to trigger Mode change after going back to zero on inactiv Channel
float nRC_t; // amount of RC throttel value for mixed mode calculations
float nRC_mp; // amount of mixing product (steering * throttel) for mixed mode calculations
float MixO; // calculated value for outer side drive in turn within mixed mode
float MixI; // calculated value for inner side drive in turn within mixed mode
bool ModeChange[3] = {false, false, false}; // flag per activ channel to trigger mode change -> both activ channels in deadzone plus positiv activation of inactiv channel
int countwait = 0; // counter to trigger ModeChange after triggertime, all flags must keep condition in waitperiod
void start_DualDriveMix(){ // set up function to enable the motors
MotorL.Enable();
MotorR.Enable();
}
void close_DualDrive(){ // set up function to enable the motors
MotorL.Disable();
MotorR.Disable();
}
// main function of DualDriveMixer for 3 channels
void run_DualDrive(int s, int tr, int tl){
// MixedMod combines the two drives controlled with one channel to steer left and right and on channel to act as throttle
// RC Channel 1 (RC_in[s]) is used for steering, RC Channel 4 (RC_in[tl]) ist used as throttel
// with RC Channel 2 (RC_in[tr]) the mode change can be triggerd
// the following algorithem is used with factor to controll the agility of cornering.
// The factor should be between (0,1 to 1) for soft agility and up to 2 for the inner side Drive to create reverse spinning
// Drive_Outerside = throttle + (factorOuterside * (throttle * steering))
// Drive_Innerside = throttle - (factorInnerside * (throttle * Steering))
// ************************************
// Mixed Mode Drive Controll
// ************************************
if (MixMode) { // RC channels have been updated, now check drive mode
nRC_t = abs(RC_in[tl]); // amount of throttel input precalculated
nRC_mp = abs(RC_in[s]) * nRC_t; // mixing product precalculated
MixO = nRC_t + (agilityO * nRC_mp); // mixed value for outer side motor
MixI = nRC_t - (agilityI * nRC_mp); // mixed value for inner side motor
if ( RC_in[s] < deadzoneDown ) { // we are in Mixed Mode: check if steering (channel 1) is to the right
ModeChange[0] = false; // modechange precondition 1 not fullfilled
if ( RC_in[tl] > deadzoneUp ) { // mix forward and right
ModeChange[1] = false; // modechange precondition 2 not fullfilled
if ( MixI >= 0 ) {
MotorR.TurnRight(normRC(MixI)); // right motor (inner side) must go slower, MotorR.TurnRight is forward
}
else {
MotorR.TurnLeft(normRC(MixI)); // or reverse (with agility factor > 1
}
MotorL.TurnLeft(normRC(MixO)); // left motor (outer side) must go faster, MotorL.TurnLeft is forward
}
else if ( RC_in[tl] < deadzoneDown ) {// mix reverse and right
ModeChange[1] = false; // modechange precondition 2 not fullfilled
if ( MixI >= 0 ) {
MotorR.TurnLeft(normRC(MixI)); // right motor (inner side) must go slower reverse, MotorR.TurnRight is forward
}
else {
MotorR.TurnRight(normRC(MixI)); // or forward (with agility factor > 1
}
MotorL.TurnRight(normRC(MixO)); // left motor (outer side) must go faster reverse
}
else { // speedcontroll (Channel 4) is neutral
ModeChange[1] = true; // modechange precondition 2 fullfillt and do not move motors
MotorR.TurnRight(0); // do not move motors
MotorL.TurnRight(0);
}
}
else if ( RC_in[s] > deadzoneUp ) { // check if steering (channel 1) ist to the left
ModeChange[0] = false; // modechange precondition 1 not fullfilled
if ( RC_in[tl] > deadzoneUp ) { // mix forward and left
ModeChange[1] = false; // modechange precondition 2 not fullfilled
if ( MixI >= 0 ) {
MotorL.TurnLeft(normRC(MixI)); // left motor (inner side) must go slower
}
else {
MotorL.TurnRight(normRC(MixI)); // or reverse (with agility factor > 1
}
MotorR.TurnRight(normRC(MixO)); // right motor (outer side) must go faster
}
else if ( RC_in[tl] < deadzoneDown ) {// mix reverse and left
ModeChange[1] = false; // modechange precondition 2 not fullfilled
if ( MixI >= 0 ) {
MotorL.TurnRight(normRC(MixI)); // left motor (inner side) must go slower reverse
}
else {
MotorL.TurnLeft(normRC(MixI)); // or forward (with agility factor > 1
}
MotorR.TurnLeft(normRC(MixO)); // right motor (outer side) must go faster reverse
}
else { // speedcontroll (Channel 4) is neutral
ModeChange[1] = true; // modechange precondition 2 fullfillt and dont move motors
MotorL.Stop(); // do not move motors
MotorR.Stop();
}
}
else { // ok not right or left, steering is in neutral
ModeChange[0] = true; // modechange precondition 1 fullfilled
if ( RC_in[tl] > deadzoneUp ) { // move forwared if above deadzone limit (channel 4)
ModeChange[1] = false; // modechange precondition 2 not fullfilled
MotorR.TurnRight(normRC(RC_in[tl])); // both motors with same speed forward
MotorL.TurnLeft(normRC(RC_in[tl]));
}
else if ( RC_in[tl] < deadzoneDown ) {// move reverse if below deadzone limit
ModeChange[1] = false; // modechange precondition 2 not fullfilled
MotorR.TurnLeft(normRC(RC_in[tl])); // both motors with same speed reverse
MotorL.TurnRight(normRC(RC_in[tl]));
}
else { // speedcontroll (Channel 4) is also in neutral
ModeChange[1] = true; // modechange precondition 2 fullfillt
MotorL.Stop(); // do not move motors
MotorR.Stop();
}
}
if ( TdeadzoneDown < RC_in[tr] < TdeadzoneUp ) {
ModeChange[2] = true; // Mixed Mode -> channel 2 used to trigger ModeChange
}
else ModeChange[2] = false;
// Set State LED to MixedMode color
if (!ModeChangeTrigger){ // if we are NOT in modechange process
if (ModeChange[0] && ModeChange[1]) { // modechanges condition fullfilled
set_LEDstate(4); // LED set to green blinking
}
else {
set_LEDstate(3); // LED set to green
}
}
MixMode = CheckModeChange(); // check for modechange
// mixed throttel for sound (0 to 500)
power = NormPower(250*(abs(MixO)+abs(MixI)));
#ifdef DEBUG_MIX // print DEBUG data to serial
Serial.print(" nRC_t: ");Serial.print(nRC_t);Serial.print(" , ");
Serial.print(" nRC_mp: ");Serial.print(nRC_mp);Serial.print(" , ");
Serial.print("power: ");Serial.print(power);Serial.print(" ");
Serial.print(" MixI: ");Serial.print(MixI);Serial.print(" , ");
Serial.print(" MixO: ");Serial.print(MixO);Serial.print(" , ");
Serial.print("Mode: ");Serial.print(MixMode);Serial.print(" ");
for (int i = 0; i<3; i++){
Serial.print(ModeChange[i]);Serial.print(",");
}
Serial.print(" ModeCount: ");Serial.print(countwait);
Serial.print(" Trigger: ");Serial.println(ModeChangeTrigger);
#endif
}
// ManuelMode is just parsing the values of two RC channels to both drives with deadzone and normation
// RC Channel 2 (RC_in[tr]) is used for throttel of right motor , RC channel 4 (RC_in[tl]) ist used as throttel of left motor
// with RC Channel 1 (RC_in[s]) the mode change can be triggerd
// ************************************
// Manuel Mode Drive Controll
// ************************************
else { // manuel mode is aktiv just send RC input values to motorshilds CH2->RC_in[tr]->MotorR, CH4->RC_in[tl]->MotorL
if ( RC_in[tr] > deadzoneUp ) { // Motor control right hand side: move forwared if above deadzone limit
MotorR.TurnRight(normRC(RC_in[tr])); // MotorR.TurnRight is forward
ModeChange[0] = false;
}
else if ( RC_in[tr] < deadzoneDown ) { // move reverse if below deadzone limit
MotorR.TurnLeft(normRC(RC_in[tr]));
ModeChange[0] = false;
}
else {
MotorR.Stop(); // do not move right motor
ModeChange[0] = true;
}
if ( RC_in[tl] > deadzoneUp ) { // Motor control left hand side: move forwared if above deadzone limit
MotorL.TurnLeft(normRC(RC_in[tl])); // MotorL.TurnLeft is forward
ModeChange[1] = false;
}
else if ( RC_in[tl] < deadzoneDown ) { // move reverse if below deadzone limit
MotorL.TurnRight(normRC(RC_in[tl]));
ModeChange[1] = false;
}
else {
MotorL.Stop(); // do not move left motor
ModeChange[1] = true;
}
if ( TdeadzoneDown < RC_in[s] < TdeadzoneUp ) {
ModeChange[2] = true; // manuel mode -> channel 1 used to trigger ModeChange
}
else ModeChange[2] = false;
// Set State LED to ManuelMode color
if (!ModeChangeTrigger){ // if we are NOT in modechange process
if (ModeChange[0] && ModeChange[1]) { // modechanges condition fullfilled
set_LEDstate(2); // LED set to blue blinking
}
else {
set_LEDstate(1); // LED set to blue
}
}
MixMode = CheckModeChange(); // check for modechange
// mixed throttel for sound (0 to 500)
power = NormPower (250*(abs(RC_in[tr])+ abs(RC_in[tl])));
#ifdef DEBUG_MIX // print DEBUG data to serial
for (int i = 0; i<4; i++){ // run through each RC channel
int CH = i+1;
Serial.print(" ch");Serial.print(CH);Serial.print(": ");
Serial.print(RC_in[i]);Serial.print(" -> ");Serial.print(normRC(RC_in[i]));Serial.print(" ");
}
Serial.print("power: ");Serial.print(power);Serial.print(" ");
Serial.print("Mode: ");Serial.print(MixMode);Serial.print(" ");
for (int i = 0; i<3; i++){
Serial.print(ModeChange[i]);Serial.print(",");
}
Serial.print(" ModeCount: ");Serial.print(countwait);
Serial.print(" Trigger: ");Serial.println(ModeChangeTrigger);
#endif
} // end Mode if
} // function run_DualDrive closed
uint8_t normRC(float RC_value){ // function to norm RC_in percentage into PWM motorshild value
// float (0 till |1|) to uint8 (0 till 255)
float Buffer;
uint8_t RCpower;
Buffer = abs(RC_value); // just the amount of the value
if (Buffer <= 1.0) { // compensate values above 1
Buffer = Buffer*255; // normation to uint8_t range
}
else {
Buffer = 255; // compensate values above 1
}
RCpower = Buffer; // copy to unit8
return RCpower; // return result
} // function normRC closed
bool CheckModeChange() { // function to check if mode change can be done
if (!ModeChangeTrigger){ // change is not triggered
if (ModeChange[0] && ModeChange[1] && ModeChange[2]) { // trigger condition flags fullfilled
if (countwait < 5) { // but we are waiting to be sure
countwait ++;
}
else { // trigger condition fully fullfilled
ModeChangeTrigger = true; // trigger set
set_LEDstate(5); // LED set to yellow
}
return MixMode;
}
else { // trigger condition not fullfilled
countwait = 0;
}
return MixMode;
}
else if (ModeChange[2]){ // trigger in progress but inactiv channel not to zero
return MixMode; // keep old mode
}
else { // Change was triggered and can be fullfilled now
ModeChangeTrigger = false;
set_LEDstate(6); // switch LED off
return !MixMode;
}
} // function ChecModeChange closed
int16_t NormPower (int16_t value) { // function to limit max power to 500
if (value >= 500) { // compensate values above 500
return 500;
}
if (value < (500*deadzoneUp)) { // check if value is inside deadzone
return 0;
}
else { // value is in range
return value; // just return
}
}