diff --git a/.DS_Store b/.DS_Store index 37e17f7..57c24af 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/include/.DS_Store b/include/.DS_Store new file mode 100644 index 0000000..0cb0318 Binary files /dev/null and b/include/.DS_Store differ diff --git a/platformio.ini b/platformio.ini index 7804d6c..9f7e04c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -17,6 +17,6 @@ debug_tool = stlink board_build.f_cpu = 72000000L monitor_speed = 115200 lib_deps = - danya0x07/NRF24L01_SimpleLibrary@^1.0.0 - pkourany/MPU6050@^1.0.3 - sstaub/Ticker@^3.2.0 +# danya0x07/NRF24L01_SimpleLibrary@^1.0.0 + + diff --git a/rc-micro-car.code-workspace b/rc-micro-car.code-workspace new file mode 100644 index 0000000..0a6fbf3 --- /dev/null +++ b/rc-micro-car.code-workspace @@ -0,0 +1,26 @@ +{ + "folders": [ + { + "path": "." + } + ], + "settings": { + "files.associations": { + "__bit_reference": "cpp", + "__hash_table": "cpp", + "__split_buffer": "cpp", + "array": "cpp", + "deque": "cpp", + "filesystem": "cpp", + "initializer_list": "cpp", + "iterator": "cpp", + "string": "cpp", + "string_view": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "ostream": "cpp", + "*.tcc": "cpp", + "ios": "cpp" + } + } +} \ No newline at end of file diff --git a/src/MC34933.cpp b/src/MC34933.cpp deleted file mode 100644 index e73fa6f..0000000 --- a/src/MC34933.cpp +++ /dev/null @@ -1,146 +0,0 @@ -#include -#include "MC34933.h" - -void MC34933::begin(void) -{ - pinMode(m_MA, OUTPUT); - pinMode(m_MB, OUTPUT); - //init pins Hi-Z - digitalWrite(m_MA, 1); - digitalWrite(m_MB, 1); - - -} - -void MC34933::setDirection(e_direction dir) -{ - m_prevDirection = m_direction; - m_direction = dir; -} - -void MC34933::start( void ) -{ - m_startCmd = true; -} - -bool MC34933::start( int speed ) -{ - if(setSpeed(speed)) - { - m_startCmd = true; - return true; - } - return false; -} - -bool MC34933::start( int speed, int duration_ms) -{ - if(setSpeed(speed)) - { - m_startCmd = true; - return true; - } - return false; -} - -bool MC34933::setSpeed(int speed) -{ - if(speed <= 255) - { - m_speed = speed; - return true; - } - return false; -} - -void MC34933::stop ( void ) -{ - m_stopCmd = true; -} - -void MC34933::forward(int speed) -{ - setDirection(LEFT); - if(setSpeed(speed)) - { - start(); - } - -} - -void MC34933::backward(int speed) -{ - setDirection(RIGHT); - if(setSpeed(speed)) - { - start(); - } -} - - -//motor statemachine -void MC34933::run( void ) -{ - switch(m_status) - { - case STARTING: - { - m_startCmd = false; //clear start flag - if(m_direction == LEFT) - { - analogWrite(m_MA, m_speed); - digitalWrite(m_MB, 1); - } - if(m_direction == RIGHT) - { - analogWrite(m_MB, m_speed); - digitalWrite(m_MA, 1); - } - m_status = RUN; - } - break; - - case RUN: - { - if(m_stopCmd ) - { - m_status = STOPPING; - - } - - if(m_direction != m_prevDirection) - { - m_status = STOPPING; - m_startCmd = true; - } - } - break; - - case STOPPING: - { - m_stopCmd = false; //clear stopflag - //set both pins to break; - analogWrite(m_MA, 0); - analogWrite(m_MB, 0); - digitalWrite(m_MA, 0); - digitalWrite(m_MB, 0); - m_status = IDLE; - } - break; - - default: - case IDLE: - { - if(m_startCmd) - { - m_status = STARTING; - } - //write pins Hi-z - analogWrite(m_MA, 255); - analogWrite(m_MB, 255); - digitalWrite(m_MA, 1); - digitalWrite(m_MB, 1); - } - break; - } -} \ No newline at end of file diff --git a/src/MC34933.h b/src/MC34933.h deleted file mode 100644 index 0742b4e..0000000 --- a/src/MC34933.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef MC34933MOTORH -#define MC34933MOTORH - -enum e_direction -{ - LEFT, - RIGHT -}; - -enum e_status -{ - IDLE, - STARTING, - RUN, - STOPPING -}; - -class MC34933 -{ - private: - const int m_MA; - const int m_MB; - e_direction m_direction = LEFT; - e_direction m_prevDirection; - int m_speed; - e_status m_status; - bool m_startCmd = false; - bool m_stopCmd = false; - - - public: - MC34933( int pin_ma, int pin_mb) - : m_MA{pin_ma}, m_MB{pin_mb} {} - - void begin( void ); - void setDirection( e_direction dir); - e_direction getDirection( void ) {return m_direction;} - bool setSpeed( int speed ); - int getSpeed( void ) {return m_speed;} - void start( void ); - bool start( int speed ); - bool start( int speed, int duration_ms); - void stop ( void ); - void forward(int speed); - void backward(int speed); - e_status getStatus( void ) { return m_status;} - - void run(void); - -}; - -#endif //MC34933MOTORH \ No newline at end of file diff --git a/src/MPU6050.cpp b/src/MPU6050.cpp new file mode 100644 index 0000000..a2d2ebf --- /dev/null +++ b/src/MPU6050.cpp @@ -0,0 +1,241 @@ +#include "Arduino.h" +#include "MPU6050.h" +#include +#include "vehicleConfig.h" +//#include "radio.h" + + + +/* This code is based on Joop Brokkings excellent work: + http://www.brokking.net/imu.html + https://www.youtube.com/watch?v=4BoIE8YQwM8 + https://www.youtube.com/watch?v=j-kE0AMEWy4 + + I (willumpie82) forked the code from TheDIYGuy999 and have modified it to fit my needs + + This header file is required for the "balancing" (vehicleType = 4) or "MRSC" (vehicleType = 5) options in the vehicleConfig.h + Connect an MPU-6050 sensor to GND, VDD (3.3 and 5V compatible), SCL and SDA. + Mount it as close as possible to the pivot point of your vehicle + + -->> Note: + - The receiver will not work, if vehicleType is set to 4 or 5 and no MPU-6050 sensor is wired up!! + - !! Don't move your vehicle during gyro calibration! (about 6s after powering up) !! + - The MPU-6050 requires about 20s to stabilize (finding the exact zero point) after powering on! + - The measurements are taken with 125Hz (8ms) refresh rate. Reason: processing all the code requires up to + 7ms loop time with 8MHz MCU clock. --> You can measure your loop time with loopDuration() +*/ + +// +// ======================================================================================================= +// GLOBAL VARIABLES +// ======================================================================================================= +// + +// 6050 variables +int gyro_x, gyro_y, gyro_z; +long acc_x_raw, acc_y_raw, acc_z_raw; +long acc_x, acc_y, acc_z, acc_total_vector; +int temperature; +long gyro_x_cal, gyro_y_cal, gyro_z_cal; +long loop_timer; +float angle_pitch, angle_roll; +bool set_gyro_angles; +float angle_roll_acc, angle_pitch_acc; +float yaw_rate; + + +// configuration variables (you may have to change them) +const int calibrationPasses = 500; // 500 is useful + +// +// ======================================================================================================= +// PRINT DEBUG DATA +// ======================================================================================================= +// + +void writeDebug() { +#ifdef DEBUG + static unsigned long lastPrint; + if (millis() - lastPrint >= 250) { + lastPrint = millis(); + + Serial.print("P: "); + Serial.print(angle_pitch); //Print pitch + Serial.print(" R: "); + Serial.print(angle_roll); //Print roll + Serial.print(" Motor: "); + Serial.println(angleOutput); //Print Motor output + } +#endif +} + +// +// ======================================================================================================= +// PROCESS MPU 6050 DATA SUBFUNCTION +// ======================================================================================================= +// + +void processMpu6050Data( void ) { + + vehicleConfig* cfg = getConfigptr(); + + gyro_x -= gyro_x_cal; // Subtract the offset calibration value from the raw gyro_x value + gyro_y -= gyro_y_cal; // Subtract the offset calibration value from the raw gyro_y value + gyro_z -= gyro_z_cal; // Subtract the offset calibration value from the raw gyro_z value + + //Gyro angle calculations + //0.0000611 * 4 = 1 / (125Hz / 65.5) + angle_pitch += gyro_x * 0.0004885; // Calculate the traveled pitch angle and add this to the angle_pitch variable + angle_roll += gyro_y * 0.0004885; // Calculate the traveled roll angle and add this to the angle_roll variable + yaw_rate = gyro_z * 0.0004885; // Yaw rate in degrees per second + + if (cfg->getvehicleType() == balancingthing) { // Those calculations are only required for the self balancing robot. Otherwise we can save some processing time. + //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians (not required in this application) + //angle_pitch += angle_roll * sin(gyro_z * 0.000001066); // If the IMU has yawed transfer the roll angle to the pitch angle + //angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); // If the IMU has yawed transfer the pitch angle to the roll angle + + //Accelerometer reading averaging to improve vibration resistance (added by TheDIYGuy999) + acc_x = (acc_x * 9 + acc_x_raw) / 10; + acc_y = (acc_y * 9 + acc_y_raw) / 10; + acc_z = (acc_z * 9 + acc_z_raw) / 10; + + //Accelerometer angle calculations + acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); // Calculate the total accelerometer vector + //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians + if (abs(acc_y) < acc_total_vector) { //Prevent the asin function to produce a NaN + angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296; //Calculate the pitch angle. + } + if (abs(acc_x) < acc_total_vector) { //Prevent the asin function to produce a NaN + angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296; //Calculate the roll angle. + } + + if (set_gyro_angles) { // If the IMU is already started (zero drift protection) + angle_pitch = angle_pitch * 0.99 + angle_pitch_acc * 0.01; // Correct the drift of the gyro pitch angle with the accelerometer pitch angle + angle_roll = angle_roll * 0.99 + angle_roll_acc * 0.01; // Correct the drift of the gyro roll angle with the accelerometer roll angle + } + else { // At first start + angle_pitch = angle_pitch_acc; // Set the gyro pitch angle equals to the accelerometer pitch angle + angle_roll = angle_roll_acc; // Set the gyro roll angle equals to the accelerometer roll angle + set_gyro_angles = true; // Set the IMU started flag + } + } +} + +// +// ======================================================================================================= +// READ MPU 6050 RAW DATA FUNCTION +// ======================================================================================================= +// + +// Sub function allows setup to call it without delay +void readMpu6050Raw() { + Wire.beginTransmission(0x68); // Start communicating with the MPU-6050 + Wire.write(0x3B); // Send the requested starting register + Wire.endTransmission(); // End the transmission + Wire.requestFrom(0x68, 14); // Request 14 bytes from the MPU-6050 + while (Wire.available() < 14); // Wait until all the bytes are received + acc_x_raw = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_x variable + acc_y_raw = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_y variable + acc_z_raw = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_z variable + temperature = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the temperature variable + gyro_x = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_x variable + gyro_y = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_y variable + gyro_z = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_z variable +} + +// Main function +void readMpu6050Data() { + static unsigned long lastReading; + if (micros() - lastReading >= 8000) { // Read the data every 8000us (equals 125Hz) + lastReading = micros(); + + readMpu6050Raw(); // Read RAW data + + processMpu6050Data(); // Process the MPU 6050 data + } +} + +// +// ======================================================================================================= +// MPU 6050 SETUP +// ======================================================================================================= +// + +void setupMpu6050() { + + vehicleConfig* cfg = getConfigptr(); + + Wire.begin(); // Start I2C as master + + // Is an an MPU-6050 (address 0x68) on the I2C bus present? + // Allows to use the receiver without an MPU-6050 plugged in (not in balancing mode, vehicleType 4) + Wire.beginTransmission(0x68); + byte error = Wire.endTransmission(); // Read the I2C error byte + if (cfg->getvehicleType() == carwithMRSC && error > 0) { // If MRSC vehicle (5) is active and there is a bus error + cfg->setvehicleType(car); // Fall back to vehicle type 0 (car without MPU-6050) + return; // Cancel the MPU-6050 setup + } + + // Activate the MPU-6050 + Wire.beginTransmission(0x68); // Start communicating with the MPU-6050 + Wire.write(0x6B); // Send the requested starting register + Wire.write(0x00); // Set the requested starting register + Wire.endTransmission(); // End the transmission + // Configure the accelerometer (+/-8g) + Wire.beginTransmission(0x68); // Start communicating with the MPU-6050 + Wire.write(0x1C); // Send the requested starting register + Wire.write(0x10); // Set the requested starting register + Wire.endTransmission(); // End the transmission + // Configure the gyro (2000° per second full scale) + Wire.beginTransmission(0x68); // Start communicating with the MPU-6050 + Wire.write(0x1B); // Send the requested starting register + Wire.write(0x18); // Set the requested starting register + Wire.endTransmission(); // End the transmission + + // Calibrate the gyro (the vehicle must stay steady during this time!) + delay(1500); // Delay 1.5 seconds to display the text +#ifdef DEBUG + Serial.println("Calibrating gyro"); // Print text to console +#endif + + int cal_int = 0; + while (cal_int < calibrationPasses) { // Run the calibrating code X times + static unsigned long lastGyroCal; + if (micros() - lastGyroCal >= 8000) { // Read the data every 8000us (equals 125Hz) +#ifdef DEBUG + if (cal_int % (calibrationPasses / 32) == 0)Serial.print("."); // Print a dot every X readings +#endif + readMpu6050Raw(); // Read the raw acc and gyro data from the MPU-6050 + gyro_x_cal += gyro_x; // Add the gyro x-axis offset to the gyro_x_cal variable + gyro_y_cal += gyro_y; // Add the gyro y-axis offset to the gyro_y_cal variable + gyro_z_cal += gyro_z; // Add the gyro z-axis offset to the gyro_z_cal variable + lastGyroCal = micros(); + cal_int ++; + } + } + gyro_x_cal /= calibrationPasses; // Divide the gyro_x_cal variable by X to get the avarage offset + gyro_y_cal /= calibrationPasses; // Divide the gyro_y_cal variable by X to get the avarage offset + gyro_z_cal /= calibrationPasses; // Divide the gyro_z_cal variable by X to get the avarage offset + +#ifdef DEBUG + + Serial.print("X cal: "); + Serial.print(gyro_x_cal); + Serial.print(" Y cal: "); + Serial.print(gyro_y_cal); + Serial.print(" Z cal: "); + Serial.println(gyro_z_cal); + + Serial.println("done!"); +#endif +} + +float Mpu6050_getangle_pitch( void ) +{ + return angle_pitch; +} + +float Mpu6050_getyaw_rate( void ) +{ + return yaw_rate; +} \ No newline at end of file diff --git a/src/MPU6050.h b/src/MPU6050.h new file mode 100644 index 0000000..12f2054 --- /dev/null +++ b/src/MPU6050.h @@ -0,0 +1,15 @@ +#ifndef MPU6050H +#define MPU6050H + +//#include "servos.h" + +void writeDebug(); +void readMpu6050Data(); +void setupMpu6050(); + +float Mpu6050_getangle_pitch( void ); +float Mpu6050_getyaw_rate( void ); + + + +#endif //MPU6050H \ No newline at end of file diff --git a/src/balancing.cpp b/src/balancing.cpp new file mode 100644 index 0000000..d0e571e --- /dev/null +++ b/src/balancing.cpp @@ -0,0 +1,173 @@ +#include +#include "balancing.h" + +#include "vehicleConfig.h" +#include "MPU6050.h" +#include "radio.h" +#include <../lib/Arduino-PID-Library/PID_v1.h> // https://github.com/br3ttb/Arduino-PID-Library/ +#include "motors.h" + +int speedPot; + +int speedAveraged; + + +//Define PID Variables +double speedTarget, speedMeasured, speedOutput; +double angleTarget, angleMeasured, angleOutput; + +double speedAverage; + +// PID controllers (you may have to change their parameters in balancing() ) +//Kp: proportional (instantly), Ki: integral (slow, precise), Kd: deriative (speed of difference) +PID speedPid(&speedMeasured, &speedOutput, &speedTarget, 0.0, 0.0, 0.0, DIRECT); // Speed: outer, slow control loop +PID anglePid(&angleMeasured, &angleOutput, &angleTarget, 0.0, 0.0, 0.0, DIRECT); // Angle: inner, fast control loop + + + +// +// ======================================================================================================= +// PID SETUP +// ======================================================================================================= +// + +void setupPid() { + + // Speed control loop + speedPid.SetSampleTime(8); // calcualte every 4ms = 250Hz + speedPid.SetOutputLimits(-33, 33); // output range from -33 to 33 (same as in balancing() ) + speedPid.SetMode(AUTOMATIC); + + // Angle control loop + anglePid.SetSampleTime(8); // calcualte every 4ms = 250Hz + anglePid.SetOutputLimits(-43, 43); // output range from -43 to 43 for motor + anglePid.SetMode(AUTOMATIC); +} + + +// +// ======================================================================================================= +// BALANCING CALCULATIONS +// ======================================================================================================= +// + +void balancing() { + + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + // Read sensor data + readMpu6050Data(); + + angleMeasured = Mpu6050_getangle_pitch() - cfg->gettiltCalibration(); + + // Read speed pot with 0.2s fader + static unsigned long lastPot; + if (millis() - lastPot >= 40) { // 40ms + lastPot = millis(); + speedPot = (speedPot * 4 + data->axis3) / 5; // 1:5 + } + + // PID Parameters (Test) + double speedKp = 0.9, speedKi = 0.03, speedKd = 0.0; + double angleKp = data->pot1 / 8.0, angleKi = 25.0, angleKd = 0.12; // /You need to connect a potentiometer to the transmitter analog input A6 + + // PID Parameters (Working) + //double speedKp = 0.9, speedKi = 0.03, speedKd = 0.0; + //double angleKp = data.pot1 / 8.0, angleKi = 25.0, angleKd = 0.12; // You need to connect a potentiometer to the transmitter analog input A6 + + // Speed PID controller (important to protect the robot from falling over at full motor rpm!) + speedTarget = ((float)speedPot - 50.0) / 1.51; // (100 - 50) / 1.51 = Range of about +/- 33 (same as in setupPid() !) + speedMeasured = speedAveraged * 1.3; //angleOutput; // 43 / 33 = 1.3 + speedPid.SetTunings(speedKp, speedKi, speedKd); + speedPid.Compute(); + + // Angle PID controller + angleTarget = speedOutput / -8.25; // 33.0 (from above) / 8.25 = Range of about +/- 4.0° tilt angle + // angleTarget = (speedPot - 50) / -12.5; // 50 / 12.5 = Range of about +/- 4.0° tilt angle + anglePid.SetTunings(angleKp, angleKi, angleKd); + anglePid.Compute(); + + // Send the calculated values to the motors + driveMotorsBalancing(); + + // Display PID variables on the transmitter OLED (for debugging only, comment out the corresponding variables in checkBattery() in this case) + //loopDuration(); // compute the loop time + //payload.vcc = loopTime; + //payload.vcc = angleMeasured; + //payload.vcc = speedTarget; + //payload.batteryVoltage = speedOutput; + //payload.batteryVoltage = speedMeasured; +} + +// +// ======================================================================================================= +// MRSC (MICRO RC STABILITY CONTROL) CALCULATIONS +// ======================================================================================================= +// For cars with stability control (steering overlay depending on gyro yaw rate) + +void mrsc() { + + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + //MC34933* Motor1 = getMotorptr(MOTOR1); + MC34933* Motor2 = getMotorptr(MOTOR2); + + // Read sensor data + readMpu6050Data(); + + // If the MRSC gain is a fixed value, read it! +#ifdef MRSC_FIXED + data.pot1 = mrscGain; +#endif + + // Compute steering compensation overlay + int turnRateSetPoint = data->axis1 - 50; // turnRateSetPoint = steering angle (0 to 100) - 50 = -50 to 50 + int turnRateMeasured = Mpu6050_getyaw_rate() * abs(data->axis3 - 50); // degrees/s * speed + int steeringAngle = turnRateSetPoint + (turnRateMeasured * data->pot1 / 100); // Compensation depending on the pot value + + steeringAngle = constrain (steeringAngle, -50, 50); // range = -50 to 50 + + // Control steering servo (MRSC mode only) +#ifdef CONFIG_HAS_SERVO + servo1.write(map(steeringAngle, 50, -50, lim1L, lim1R) ); // 45 - 135° +#endif //CONFIG_HAS_SERVO + // Control motor 2 (steering, not on "High Power" board type) + if (!cfg->getHP()) { + Motor2->drive((steeringAngle + 50), 0, cfg->getsteeringTorque(), 0, false); // The steering motor (if the original steering motor is reused instead of a servo) + } + + // Control motors + driveMotorsCar(); + +} + + +void driveMotorsBalancing() +{ + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + MC34933* Motor1 = getMotorptr(MOTOR1); + MC34933* Motor2 = getMotorptr(MOTOR2); + + // The steering overlay is in degrees per second, controlled by the MPU 6050 yaw rate and yoystick axis 1 + int steering = ((data->axis1 - 50) / 7) - Mpu6050_getyaw_rate(); // -50 to 50 / 8 = 7°/s - yaw_rate + steering = constrain(steering, -7, 7); + int speed = angleOutput + 50; + + // Calculate averaged motor power (speed) for speed controller feedback + static unsigned long lastSpeed; + if (millis() - lastSpeed >= 8) { // 8ms + speedAveraged = (speedAveraged * 3.0 + angleOutput) / 4.0; // 1:4 (1:8) + } + + speed = constrain(speed, 7, 93); // same range as in setupPID() + 50 offset from above! + + if (angleMeasured > -20.0 && angleMeasured < 20.0) { // Only drive motors, if robot stands upright + Motor1->drive(speed - steering, cfg->getminPWM(), cfg->getmaxPWMfull(), 0, false); // left caterpillar, 0ms ramp! 50 = neutral! + Motor2->drive(speed + steering, cfg->getminPWM(), cfg->getmaxPWMfull(), 0, false); // right caterpillar + } + else { // keep motors off + Motor1->drive(50, cfg->getminPWM(), cfg->getmaxPWMfull(), 0, false); // left caterpillar, 0ms ramp! + Motor2->drive(50, cfg->getminPWM(), cfg->getmaxPWMfull(), 0, false); // right caterpillar + } +} \ No newline at end of file diff --git a/src/balancing.h b/src/balancing.h new file mode 100644 index 0000000..79fd8c3 --- /dev/null +++ b/src/balancing.h @@ -0,0 +1,10 @@ +#ifndef BALANCINGH +#define BALANCINGH + +void balancing( void ); +void driveMotorsBalancing( void ); +void setupPid( void ); +void mrsc(); + + +#endif //BALANCINGH \ No newline at end of file diff --git a/src/battery.cpp b/src/battery.cpp new file mode 100644 index 0000000..af55b6a --- /dev/null +++ b/src/battery.cpp @@ -0,0 +1,92 @@ +#include +#include "battery.h" +#include "vehicleConfig.h" +#include "rc_board.h" +#include "radio.h" +#include "motors.h" +#include "lights.h" + + +bool battSense; + +void checkBattery() +{ + vehicleConfig* cfg = getConfigptr(); + ackPayload* payload = getPayloadptr(); + + if (cfg->getboardVersion() < 1.2) battSense = false; + else battSense = true; + + // switch between load and no load contition + if (millis() - getMillisLightsOff() >= 1000) { // one s after the vehicle did stop + setIsDriving(false);// = false; // no load + } + else { + setIsDriving(true);// = true; // under load + } + + // Every 1000 ms, take measurements + static unsigned long lastTrigger; + if (millis() - lastTrigger >= 1000) { + lastTrigger = millis(); + + // Read both averaged voltages + payload->batteryVoltage = batteryAverage(); + payload->vcc = vccAverage(); + + if (battSense) { // Observe battery voltage + if (payload->batteryVoltage <= cfg->getcutoffVoltage()) payload->batteryOk = false; + } + else { // Observe vcc voltage + if (payload->vcc <= cfg->getcutoffVoltage()) payload->batteryOk = false; + } + } +} + +// Voltage read & averaging subfunctions ----------------------------------------- +// vcc ---- +float vccAverage() +{ + static int raw[6]; + + if (raw[0] == 0) { + for (int i = 0; i <= 5; i++) { + raw[i] = readVcc(); // Init array + } + } + + raw[5] = raw[4]; + raw[4] = raw[3]; + raw[3] = raw[2]; + raw[2] = raw[1]; + raw[1] = raw[0]; + raw[0] = readVcc(); + float average = (raw[0] + raw[1] + raw[2] + raw[3] + raw[4] + raw[5]) / 6000.0; + return average; +} + +// battery ---- +float batteryAverage() +{ + + vehicleConfig* cfg = getConfigptr(); + static int raw[6]; + + if (!battSense) return 0; + + if (raw[0] == 0) { + for (int i = 0; i <= 5; i++) { + raw[i] = analogRead(PIN_ADC_VBAT); // Init array + } + } + + raw[5] = raw[4]; + raw[4] = raw[3]; + raw[3] = raw[2]; + raw[2] = raw[1]; + raw[1] = raw[0]; + if (getIsDriving() && cfg->getHP()) raw[0] = (analogRead(PIN_ADC_VBAT) + 31); // add 0.3V while driving (HP version only): 1023 steps * 0.3V / 9.9V = 31 + else raw[0] = analogRead(PIN_ADC_VBAT); // else take the real voltage (compensates voltage drop while driving) + float average = (raw[0] + raw[1] + raw[2] + raw[3] + raw[4] + raw[5]) / 619.999; // 1023steps / 9.9V * 6 = 619.999 + return average; +} \ No newline at end of file diff --git a/src/battery.h b/src/battery.h new file mode 100644 index 0000000..af91b0a --- /dev/null +++ b/src/battery.h @@ -0,0 +1,10 @@ +#ifndef BATTERYH +#define BATTERYH + +void checkBattery(); +float vccAverage(); +float batteryAverage(); +long readVcc(); + + +#endif //BATTERYH \ No newline at end of file diff --git a/src/helper.h b/src/helper.h new file mode 100644 index 0000000..33ad5c9 --- /dev/null +++ b/src/helper.h @@ -0,0 +1,23 @@ +#ifndef helper_h +#define helper_h + +#include "Arduino.h" + +// +// ======================================================================================================= +// LOOP TIME MEASUREMENT +// ======================================================================================================= +// + +unsigned int loopTime; + + + +void loopDuration() { + static unsigned long timerOld; + unsigned long timer = millis(); + loopTime = timer - timerOld; + timerOld = timer; +} + +#endif diff --git a/src/lights.cpp b/src/lights.cpp new file mode 100644 index 0000000..dc0a6a6 --- /dev/null +++ b/src/lights.cpp @@ -0,0 +1,180 @@ +#include +#include "lights.h" +#include "radio.h" +#include "vehicleConfig.h" +#include "motors.h" + +#include <../lib/statusLED/statusLED.h> // https://github.com/TheDIYGuy999/statusLED + + +// Headlight off delay +unsigned long millisLightOff = 0; + + +// Status LED objects +statusLED tailLight(false); // "false" = output not inverted +statusLED headLight(false); +statusLED indicatorL(false); +statusLED indicatorR(false); +statusLED beaconLights(false); + +// Indicators +bool left; +bool right; +bool hazard; + +bool getLeft( void ) +{ + return left; +} + +bool getRight( void ) +{ + return right; +} + +bool getHazard( void ) +{ + return hazard; +} + +void SetmillisLightOff( unsigned long millis) +{ + millisLightOff = millis; +} + +void UpdatemillisLightOff( void ) +{ + millisLightOff = millis(); +} + + +unsigned long getMillisLightsOff( void ) +{ + return millisLightOff; +} + + +// +// ======================================================================================================= +// LED +// ======================================================================================================= +// + +// Brake light subfunction for ESC vehicles +bool escBrakeActive() { + RcData* data = getDataptr(); + static byte driveState; + bool brake= false; + + switch (driveState) { // 0 = neutral, 1 = forward, 2 = reverse, 3 = brake + + case 0: // neutral + if (data->axis3 > 55) driveState = 1; // forward + if (data->axis3 < 45) driveState = 2; // reverse + brake = false; + break; + + case 1: // forward + if (data->axis3 < 45) driveState = 3; // brake + brake = false; + break; + + case 2: // reverse + if (data->axis3 > 55) driveState = 1; // forward + brake = false; + break; + + case 3: // brake + if (data->axis3 > 45) driveState = 2; // go to reverse, if above neutral + brake = true; + break; + default: + { + brake = false; + } + + } + return brake; +} + +void led() +{ + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + // Lights are switching off 10s after the vehicle did stop + if (millis() - millisLightOff >= 10000) { + headLight.off(); // Headlight off + tailLight.off(); // Taillight off + beaconLights.off(); // Beacons off + } + else { + if (!cfg->getescBrakeLights() && ((!cfg->getHP() && getMotorptr(MOTOR1)->brakeActive()) || (cfg->getHP() && getMotorptr(MOTOR2)->brakeActive()) )) { // if braking detected from TB6612FNG motor driver + tailLight.on(); // Brake light (full brightness) + } + + else if (cfg->getescBrakeLights() && escBrakeActive() ) { // or braking detected from ESC + tailLight.on(); // Brake light (full brightness) + } + + else { + tailLight.flash(10, 14, 0, 0); // Taillight: 10 on / 14 off = about 40% brightness (soft PWM) + } + beaconLights.flash(50, 650, 0, 0); // Simulate rotating beacon lights with short flashes + headLight.on(); // Headlight on + } + + // Indicator lights ---- + if (cfg->getindicators()) { + // Set and reset by lever + if (data->axis4 < 5) left = true; + if (data->axis4 > 55) left = false; + + if (data->axis4 > 95) right = true; + if (data->axis4 < 45) right = false; + + // Reset by steering + static int steeringOld; + + if (data->axis1 > steeringOld + 10) { + left = false; + steeringOld = data->axis1; + } + + if (data->axis1 < steeringOld - 10) { + right = false; + steeringOld = data->axis1; + } + + // Lights + if (left) { // Left indicator + right = false; + indicatorL.flash(375, 375, 0, 0); + indicatorR.off(); + } + + if (right) { // Right indicator + left = false; + indicatorR.flash(375, 375, 0, 0); + indicatorL.off(); + } + + if (hazard) { // Hazard lights + if (left) { + left = false; + indicatorL.off(); + } + if (right) { + right = false; + indicatorR.off(); + } + indicatorL.flash(375, 375, 0, 0); + indicatorR.flash(375, 375, 0, 0); + } + + if (!hazard && !left && !right) { + indicatorL.off(); + indicatorR.off(); + } + } +} \ No newline at end of file diff --git a/src/lights.h b/src/lights.h new file mode 100644 index 0000000..c649938 --- /dev/null +++ b/src/lights.h @@ -0,0 +1,14 @@ +#ifndef LIGHTSH +#define LIGHTSH + + +void SetmillisLightOff( unsigned long millis ); +void UpdatemillisLightOff( void ); +unsigned long getMillisLightsOff( void ); +void led(); + +bool getLeft( void ); +bool getRight( void ); +bool getHazard( void ); + +#endif //LIGHTSH \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index d9c12c8..cb51f66 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,177 +1,225 @@ -/* - * Blink - * Turns on an LED on for one second, - * then off for one second, repeatedly. - */ +// 4 Channel "Micro RC" Receiver with 4 standard RC Servo Outputs +// ATMEL Mega 328P TQFP 32 soldered directly to the board, 8MHz external resonator, +// 2.4GHz NRF24L01 SMD radio module, TB6612FNG dual dc motor driver +// An MPU-6050 gyro / accelerometer can be used for MRSC stability control or self balancing robots +// SBUS output on pin TXO -#include -#include +// See: https://www.youtube.com/playlist?list=PLGO5EJJClJBCjIvu8frS7LrEU3H2Yz_so + +// * * * * N O T E ! The vehicle specific configurations are stored in "vehicleConfig.h" * * * * + +const float codeVersion = 3.4; // Software revision (see https://github.com/TheDIYGuy999/Micro_RC_Receiver/blob/master/README.md) + +// +// ======================================================================================================= +// BUILD OPTIONS (comment out unneeded options) +// ======================================================================================================= +// + +//#define DEBUG // if not commented out, Serial.print() is active! For debugging only!! + +// +// ======================================================================================================= +// INCLUDE LIRBARIES +// ======================================================================================================= +// + +// Libraries +#include // I2C library (for the MPU-6050 gyro /accelerometer) +//#include // Installed via Tools > Board > Boards Manager > Type RF24 +#include +#include "vehicleConfig.h" +// Tabs (header files in sketch directory) +#include "readVCC.h" +#include "steeringCurves.h" +#include "tone.h" +#include "MPU6050.h" +#include "helper.h" +#include "radio.h" +#include "motors.h" +#include "battery.h" #include "hall.h" #include "rc_board.h" -#include -#include -#include -//#include "Wire.h" -#include "MC34933.h" +#include "servos.h" +#include "balancing.h" +#include "lights.h" -bool ms_task_flag = false; -bool decims_task_flag = false; -bool second_task_flag = false; -//function prototypes -void Tick_handle_msTask (void) + + + +// +// ======================================================================================================= +// MAIN ARDUINO SETUP (1x during startup) +// ======================================================================================================= +// + +void setup() { - ms_task_flag = true; -} -void Tick_handle_10msTask( void ) -{ - decims_task_flag = true; -} -void Tick_handle_1sTask( void ) -{ - second_task_flag = true; + + vehicleConfig* cfg = getConfigptr(); + cfg->begin(COKECANCAR); + + RcData* data = getDataptr(); + +#ifdef DEBUG + Serial.begin(115200); + printf_begin(); + serialCommands = false; + delay(3000); +#endif + +#ifndef DEBUG + // If TXO pin or RXI pin is used for other things, disable Serial + if (cfg->getTXO_momentary1() || cfg->getTXO_toggle1() || cfg->getheadLights()) + { + Serial.end(); // make sure, serial is off! + #ifndef STM32F1xx + UCSR0B = 0b00000000; + #endif + cfg->serialCommands = false; + } + else + { // Otherwise use it for serial commands to the light and sound controller + +#ifdef CONFIG_HAS_SBUS + SBUS* x8r = getSBUSptr(); + x8r->begin(); // begin the SBUS communication +#else + Serial.begin(115200); // begin the standart serial communication +#endif + cfg->serialCommands = true; + } +#endif + + if(cfg->gettoneOut()) + { + R2D2_tell(); + } + + + // LED setup +#ifdef CONFIG_HAS_LIGHTS + if (vehicleType == 4 || vehicleType == 5 ) indicators = false; // Indicators use the same pins as the MPU-6050, so they can't be used in vehicleType 4 or 5! + + if (tailLights) tailLight.begin(A1); // A1 = Servo 2 Pin + if (headLights) headLight.begin(0); // 0 = RXI Pin + if (indicators) { + indicatorL.begin(A4); // A4 = SDA Pin + indicatorR.begin(A5); // A5 = SCL Pin + } + if (beacons) beaconLights.begin(A3); // A3 = Servo 4 Pin +#endif //CONFIG_HAS_LIGHTS + +// Radio setup +#ifdef CONFIG_HAS_NRF24 + setupRadio(); +#endif //CONFIG_HAS_NRF24 + + // Servo pins +#ifdef CONFIG_HAS_SERVO + servo1.attach(A0); + if (!tailLights) servo2.attach(A1); + if (!engineSound && !toneOut) servo3.attach(A2); + if (!beacons) servo4.attach(A3); +#endif //CONFIG_HAS_SERVO + + // All axes to neutral position + data->axis1 = 50; + data->axis2 = 50; + data->axis3 = 50; + data->axis4 = 50; + data->pot1 = 50; // Added in v3.32 + + + + // Special functions + if (cfg->getTXO_momentary1() || cfg->getTXO_toggle1()) pinMode(PIN_SPARE1, OUTPUT); + + // Motor driver setup + setupMotors(); + + if (cfg->getvehicleType() == balancingthing || cfg->getvehicleType() == carwithMRSC) { // Only for self balancing vehicles and cars with MRSC + // MPU 6050 accelerometer / gyro setup + setupMpu6050(); + + // PID controller setup + setupPid(); + } } -Ticker ms_ticker(Tick_handle_msTask, 1, 0, MILLIS); -Ticker decims_ticker(Tick_handle_10msTask, 10, 0, MILLIS); -Ticker second_ticker(Tick_handle_1sTask, 1000,0, MILLIS); +// +// ======================================================================================================= +// WRITE DIGITAL OUTPUTS (SPECIAL FUNCTIONS) +// ======================================================================================================= +// +void digitalOutputs() { -MPU6050 mpu; -MC34933 motor1(PIN_M1A,PIN_M1B); -MC34933 steering(PIN_M2A, PIN_M2B); + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + static bool wasPressed; -bool blinkState = false; -// Variables will change: -int ledState = LOW; // ledState used to set the LED -unsigned long previousMillis = 0; // will store last time LED was updated -const long interval = 1000; // interval at which to blink (milliseconds) -const long main_loop_interval = 10; -const long second_interval = 100; //number of decims-ticks -unsigned long decims_counter = 0; - -#define MOTORSPEED 1 - - -// ================================================================ -// === INITIAL SETUP === -// ================================================================ - -void handle_msTask (void) -{ -ms_task_flag = true; -} - -void handle_10msTask( void ) -{ - motor1.run(); -} - -void handle_1sTask (void ) -{ - if (ledState == LOW) - { - ledState = HIGH; - motor1.forward(MOTORSPEED); - } - else - { - ledState = LOW; - motor1.backward(MOTORSPEED); + if (cfg->getTXO_momentary1()) { // only, if momentary function is enabled in vehicle configuration + if (data->momentary1) { + digitalWrite(PIN_SPARE1, HIGH); + #ifdef CONFIG_HAS_TONE + R2D2_tell(); + #endif } - Serial.print("."); - digitalWrite(PIN_SPARE2, ledState); -} - -void init_mpu( void ) -{ -// //MPU6050 init -// Serial.println("Initializing I2C devices..."); -// mpu.initialize(); - -// Serial.println(F("Initializing DMP...")); -// devStatus = mpu.dmpInitialize(); - -// // supply your own gyro offsets here, scaled for min sensitivity -// mpu.setXGyroOffset(220); -// mpu.setYGyroOffset(76); -// mpu.setZGyroOffset(-85); -// mpu.setZAccelOffset(1788); // 1688 factory default for my test chip - -// // make sure it worked (returns 0 if so) -// if (devStatus == 0) { -// // turn on the DMP, now that it's ready -// Serial.println(F("Enabling DMP...")); -// mpu.setDMPEnabled(true); - -// // enable Arduino interrupt detection -// Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); - -// attachInterrupt(0, dmpDataReady, RISING); - -// mpuIntStatus = mpu.getIntStatus(); - -// // set our DMP Ready flag so the main loop() function knows it's okay to use it -// Serial.println(F("DMP ready! Waiting for first interrupt...")); -// dmpReady = true; - -// // get expected DMP packet size for later comparison -// packetSize = mpu.dmpGetFIFOPacketSize(); -// } else { -// // ERROR! -// // 1 = initial memory load failed -// // 2 = DMP configuration updates failed -// // (if it's going to break, usually the code will be 1) -// Serial.print(F("DMP Initialization failed (code ")); -// Serial.print(devStatus); -// Serial.println(F(")")); -// } -} - - -void setup() -{ - hall_init(); - - motor1.begin(); - delay(5000); - - - - - - ms_ticker.start(); - decims_ticker.start(); - second_ticker.start(); -} - - -void loop() -{ - - ms_ticker.update(); - decims_ticker.update(); - second_ticker.update(); - - if( ms_task_flag) - { - ms_task_flag = false; - handle_msTask(); + else digitalWrite(PIN_SPARE1, LOW); } - if( decims_task_flag) - { - decims_task_flag = false; - handle_10msTask(); - } + if (cfg->getTXO_toggle1()) { // only, if toggle function is enabled in vehicle configuration - if( second_task_flag) - { - second_task_flag = false; - handle_1sTask(); + if (data->momentary1 && !wasPressed) { + digitalWrite(PIN_SPARE1, !digitalRead(PIN_SPARE1)); + wasPressed = true; + } + if (!data->momentary1) wasPressed = false; } - } + + + + + + +// +// ======================================================================================================= +// MAIN LOOP +// ======================================================================================================= +// + +void loop() { + // Read radio data from transmitter + readRadio(); + + // Write the servo positions + writeServos(); + + // Drive the motors + drive(); + + // Battery check + checkBattery(); + + // Digital Outputs (special functions) + digitalOutputs(); + + // LED + led(); + + // Send serial commands +#ifdef CONFIG_HAS_SBUS + // Serial commands are transmitted in SBUS standard + sendSbusCommands(); +#else + // Normal protocol (for ESP32 engine sound controller only) + sendSerialCommands(); +#endif +} + diff --git a/src/motors.cpp b/src/motors.cpp new file mode 100644 index 0000000..b028dd1 --- /dev/null +++ b/src/motors.cpp @@ -0,0 +1,326 @@ +#include +#include "motors.h" +#include "vehicleConfig.h" +#include "rc_board.h" +#include "radio.h" +#include "lights.h" +#include "steeringCurves.h" +#include "balancing.h" + + +#include "../lib/MC34933/MC34933.h" // https://github.com/willumpie82/MC34933 +#include <../lib/PWMFrequency/PWMFrequency.h> // https://github.com/TheDIYGuy999/PWMFrequency + +bool isDriving; // is the vehicle driving? +// Motor objects +MC34933 Motor1; +MC34933 Motor2; + + +// This array is intended for the "vhctype_semi caterpillar" mode. The inner wheel can max. slow down to 60% of the +// outer wheels RPM +float curvevhctype_semi[][2] = { // see excel sheet! + {0, 60} // {input value, output value} + , {25, 70} + , {50, 80} + , {75, 90} + , {100, 100} +}; + +// This array is intended for the "Caterpillar" mode. The inner wheel can spin backwars up to 100% of the +// outer wheels RPM. That allows for turning the vehicle "on place" +float curveFull[][2] = { + {0, -100} // {input value, output value} + , {7, 0} + , {20, 45} + , {60, 85} + , {100, 100} +}; + +// This array is intended for the "Differential Thrust" mode. The inner motor can max. slow down to 20% of the +// outer motors RPM +float curveThrust[][2] = { // see excel sheet! + {0, 20} // {input value, output value} + , {25, 40} + , {50, 60} + , {75, 80} + , {100, 100} +}; + +// +// ======================================================================================================= +// MOTOR DRIVER SETUP +// ======================================================================================================= +// + +void setupMotors() +{ + vehicleConfig* cfg = getConfigptr(); + // MC34933 H-Bridge pins + const byte motor1_in1 = PIN_MOTOR_M1A; + const byte motor1_in2 = PIN_MOTOR_M1B; + + const byte motor2_in1 = PIN_MOTOR_M2A; + const byte motor2_in2 = PIN_MOTOR_M2B; + + + // SYNTAX: IN1, IN2, PWM, min. input value, max. input value, neutral position width + // invert rotation direction true or false + Motor1.begin(motor1_in1, motor1_in2, 0, 100, 4, false); // Drive motor + Motor2.begin(motor2_in1, motor2_in2, 0, 100, 4, false); // Steering motor (Drive in "HP" version) + + // Motor PWM frequency prescalers (Requires the PWMFrequency.h library) + // Differential steering vehicles: locked to 984Hz, to make sure, that both motors use 984Hz. + if (cfg->getvehicleType() == vhctype_semi || cfg->getvehicleType() == vhctype_unknown || cfg->getvehicleType() == simpledualmotorplane) + { + cfg->setpwmPrescaler2(32); + } + // ----------- IMPORTANT!! -------------- + // Motor 1 always runs @ 984Hz PWM frequency and can't be changed, because timers 0 an 1 are in use for other things! + // Motor 2 (pin 3) can be changed to the following PWM frequencies: 32 = 984Hz, 8 = 3936Hz, 1 = 31488Hz + setPWMPrescaler(3, cfg->getpwmPrescaler2()); // pin 3 is hardcoded, because we can't change all others anyway +} + + +MC34933* getMotorptr(motor_t motor) +{ + if(motor==1) + { + return &Motor1; + } + else + { + return &Motor2; + } + +} + +// +// ======================================================================================================= +// DRIVE MOTORS CAR (for cars, one motor for driving, one for steering) +// ======================================================================================================= +// + +void drive() +{ + vehicleConfig* cfg = getConfigptr(); + + switch(cfg->getvehicleType()) + { + case car: + { + driveMotorsCar(); + + } + break; + + case carwithMRSC: + { + mrsc(); // Car with MSRC stabilty control + } + break; + + case forklift: + { + driveMotorsForklift(); // Forklift + } + break; + + case balancingthing: + { + balancing(); // Self balancing robot + } + break; + + default: + { + driveMotorsSteering(); // Caterpillar and half caterpillar vecicles + } + break; + + + } + + +} + + +void driveMotorsCar() { + + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + ackPayload* payload = getPayloadptr(); + + int maxPWM; + byte maxAcceleration; + + // Speed limitation (max. is 255) + if (data->mode1) { + maxPWM = cfg->getmaxPWMlimited(); // Limited + } else { + maxPWM = cfg->getmaxPWMfull(); // Full + } + + if (!payload->batteryOk && cfg->getliPo()) data->axis3 = 50; // Stop the vehicle, if the battery is empty! + + // Acceleration & deceleration limitation (ms per 1 step input signal change) + if (data->mode2) { + maxAcceleration = cfg->getmaxAccelerationLimited(); // Limited + } else { + maxAcceleration = cfg->getmaxAccelerationFull(); // Full + } + + // ***************** Note! The ramptime is intended to protect the gearbox! ******************* + // SYNTAX: Input value, max PWM, ramptime in ms per 1 PWM increment + // false = brake in neutral position inactive + + if (!cfg->getHP()) { // Two channel version: ---- + if (Motor1.drive(data->axis3, cfg->getminPWM(), maxPWM, maxAcceleration, true) ) { // The drive motor (function returns true, if not in neutral) + UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving! + } + if (cfg->getvehicleType() != 5) { // If not car with MSRC stabilty control + Motor2.drive(data->axis1, 0, cfg->getsteeringTorque(), 0, false); // The steering motor (if the original steering motor is reused instead of a servo) + } + } + else { // High Power "HP" version. Motor 2 is the driving motor, no motor 1: ---- + if (Motor2.drive(data->axis3, cfg->getminPWM(), maxPWM, maxAcceleration, true) ) { // The drive motor (function returns true, if not in neutral) + UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving! + } + } +} + +// +// ======================================================================================================= +// DRIVE MOTORS FORKLIFT (for forklifts, one motor for driving, one for lifting) +// ======================================================================================================= +// + +void driveMotorsForklift() { + + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + ackPayload* payload = getPayloadptr(); + + int maxPWM; + byte maxAcceleration; + + // Speed limitation (max. is 255) + if (data->mode1) { + maxPWM = cfg->getmaxPWMlimited(); // Limited + } else { + maxPWM = cfg->getmaxPWMfull(); // Full + } + + if (!payload->batteryOk && cfg->getliPo()) data->axis3 = 50; // Stop the vehicle, if the battery is empty! + + // Acceleration & deceleration limitation (ms per 1 step input signal change) + if (data->mode2) { + maxAcceleration = cfg->getmaxAccelerationLimited(); // Limited + } else { + maxAcceleration = cfg->getmaxAccelerationFull(); // Full + } + + // ***************** Note! The ramptime is intended to protect the gearbox! ******************* + // SYNTAX: Input value, max PWM, ramptime in ms per 1 PWM increment + // false = brake in neutral position inactive + + + if (Motor1.drive(data->axis3, cfg->getminPWM(), maxPWM, maxAcceleration, true) ) { // The drive motor (function returns true, if not in neutral) + UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving! + } + Motor2.drive(data->axis2, 0, cfg->getsteeringTorque(), 0, false); // The fork lifting motor (the steering is driven by servo 1) +} + +// +// ======================================================================================================= +// "STEERING" MOTOR DRIVING FUNCTION (throttle & steering overlay for caterpillar vehicles) +// ======================================================================================================= +// + +bool getIsDriving() +{ + return isDriving; +} + +void setIsDriving(bool driveing) +{ + isDriving = driveing; +} + + +void driveMotorsSteering() { + + int pwm[2]; + + vehicleConfig* cfg = getConfigptr(); + RcData* data = getDataptr(); + ackPayload* payload = getPayloadptr(); + + // The steering overlay + int steeringFactorLeft=0; + int steeringFactorRight=0; + int steeringFactorLeft2=0; + int steeringFactorRight2 =0; + + // The input signal range + const int servoMin = 0; + const int servoMax = 100; + const int servoNeutralMin = 48; + const int servoNeutralMax = 52; + + // Compute steering overlay: + // The steering signal is channel 1 = data.axis1 + // 100% = wheel spins with 100% of the requested speed forward + // -100% = wheel spins with 100% of the requested speed backward + if (data->axis1 <= servoNeutralMin) { + steeringFactorLeft = map(data->axis1, servoMin, servoNeutralMin, 0, 100); + steeringFactorLeft = constrain(steeringFactorLeft, 0, 100); + } + else { + steeringFactorLeft = 100; + } + + if (data->axis1 >= servoNeutralMax) { + steeringFactorRight = map(data->axis1, servoMax, servoNeutralMax, 0, 100); + steeringFactorRight = constrain(steeringFactorRight, 0, 100); + } + else { + steeringFactorRight = 100; + } + + // Nonlinear steering overlay correction + if (cfg->getvehicleType() == 6) { + steeringFactorLeft2 = reMap(curveThrust, steeringFactorLeft); // Differential thrust mode + steeringFactorRight2 = reMap(curveThrust, steeringFactorRight); + data->axis3 = constrain(data->axis3, 50, 100); // reverse locked! + } + if (cfg->getvehicleType() == 2) { + steeringFactorLeft2 = reMap(curveFull, steeringFactorLeft); // Caterpillar mode + steeringFactorRight2 = reMap(curveFull, steeringFactorRight); + } + if (cfg->getvehicleType() == 1) { + steeringFactorLeft2 = reMap(curvevhctype_semi, steeringFactorLeft); // vhctype_semi caterpillar mode + steeringFactorRight2 = reMap(curvevhctype_semi, steeringFactorRight); + } + + // Drive caterpillar motors + // The throttle signal (for both caterpillars) is channel 3 = data.axis3 + // -100 to 100% + pwm[0] = map(data->axis3, servoMin, servoMax, 100, -100) * steeringFactorRight2 / 100; + pwm[1] = map(data->axis3, servoMin, servoMax, 100, -100) * steeringFactorLeft2 / 100; + + pwm[0] = map(pwm[0], 100, -100, 100, 0); // convert -100 to 100% to 0-100 for motor control + pwm[1] = map(pwm[1], 100, -100, 100, 0); + + if (!payload->batteryOk && cfg->getliPo()) { // Both motors off, if LiPo battery is empty! + pwm[0] = 0; + pwm[1] = 0; + } + + Motor1.drive(pwm[0], 0, 255, 0, false); // left caterpillar, 0ms ramp! + Motor2.drive(pwm[1], 0, 255, 0, false); // right caterpillar + + if (pwm[0] < 40 || pwm[0] > 60 || pwm[1] < 40 || pwm[1] > 60) { + UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving! + } +} \ No newline at end of file diff --git a/src/motors.h b/src/motors.h new file mode 100644 index 0000000..5801640 --- /dev/null +++ b/src/motors.h @@ -0,0 +1,25 @@ +#ifndef MOTORSH +#define MOTORSH + +#include "../lib/MC34933/MC34933.h" // https://github.com/willumpie82/MC34933 + +typedef enum motor_e +{ + MOTOR1, + MOTOR2 +}motor_t; + + +// Motors +bool getIsDriving(); +void setIsDriving(bool driveing); + +void setupMotors(); +void driveMotorsCar(); +void driveMotorsForklift(); +void driveMotorsSteering(); +void drive(); + +MC34933* getMotorptr(motor_t motor); + +#endif //MOTORSH \ No newline at end of file diff --git a/src/radio.cpp b/src/radio.cpp new file mode 100644 index 0000000..d901254 --- /dev/null +++ b/src/radio.cpp @@ -0,0 +1,217 @@ +#include +#include "radio.h" +#include "lights.h" +#include "vehicleConfig.h" +#include "rc_board.h" + +#include "../../lib/SBUS/src/SBUS.h" + +// Hardware configuration: Set up nRF24L01 radio on hardware SPI bus & pins 8 (CE) & 7 (CSN) +#ifdef CONFIG_HAS_NRF24 + RF24 radio(8, 7); +#endif + +// SBUS object +#ifdef CONFIG_HAS_SBUS + #ifndef DEBUG + SBUS x8r(Serial); + + SBUS* getSBUSptr( void ){return &x8r;} + #endif +#endif + +RcData data; + +RcData* getDataptr(void) +{ + return &data; +} +// Radio channels (126 channels are supported) +byte chPointer = 0; // Channel 1 (the first entry of the array) is active by default +const byte NRFchannel[] { + 1, 2 +}; + +// the ID number of the used "radio pipe" must match with the selected ID on the transmitter! +// 10 ID's are available @ the moment +const uint64_t pipeIn[] = { + 0xE9E8F0F0B1LL, 0xE9E8F0F0B2LL, 0xE9E8F0F0B3LL, 0xE9E8F0F0B4LL, 0xE9E8F0F0B5LL, + 0xE9E8F0F0B6LL, 0xE9E8F0F0B7LL, 0xE9E8F0F0B8LL, 0xE9E8F0F0B9LL, 0xE9E8F0F0B0LL +}; +const int maxVehicleNumber = (sizeof(pipeIn) / (sizeof(uint64_t))); + +ackPayload payload; + +ackPayload* getPayloadptr( void ) +{ + return &payload; +} + +// +// ======================================================================================================= +// RADIO SETUP +// ======================================================================================================= +// + +void setupRadio() { +#ifdef CONFIG_HAS_NRF24 + + radio.begin(); + radio.setChannel(NRFchannel[chPointer]); + + // Set Power Amplifier (PA) level to one of four levels: RF24_PA_MIN, RF24_PA_LOW, RF24_PA_HIGH and RF24_PA_MAX + radio.setPALevel(RF24_PA_HIGH); // HIGH + + radio.setDataRate(RF24_250KBPS); + radio.setAutoAck(pipeIn[vehicleNumber - 1], true); // Ensure autoACK is enabled + radio.enableAckPayload(); + radio.enableDynamicPayloads(); + radio.setRetries(5, 5); // 5x250us delay (blocking!!), max. 5 retries + //radio.setCRCLength(RF24_CRC_8); // Use 8-bit CRC for performance + +#ifdef DEBUG + radio.printDetails(); + delay(3000); +#endif + + radio.openReadingPipe(1, pipeIn[vehicleNumber - 1]); + radio.startListening(); +#endif +} + +// +// ======================================================================================================= +// READ RADIO DATA +// ======================================================================================================= +// + +void readRadio() { +#ifdef CONFIG_HAS_NRF24 + + static unsigned long lastRecvTime = 0; + byte pipeNo; + + if (radio.available(&pipeNo)) { + radio.writeAckPayload(pipeNo, &payload, sizeof(struct ackPayload) ); // prepare the ACK payload + radio.read(&data, sizeof(struct RcData)); // read the radia data and send out the ACK payload + hazard = false; + lastRecvTime = millis(); +#ifdef DEBUG + Serial.print(data.axis1); + Serial.print("\t"); + Serial.print(data.axis2); + Serial.print("\t"); + Serial.print(data.axis3); + Serial.print("\t"); + Serial.print(data.axis4); + Serial.println("\t"); +#endif + } + + // Switch channel + if (millis() - lastRecvTime > 500) { + chPointer ++; + if (chPointer >= sizeof((*NRFchannel) / sizeof(byte))) chPointer = 0; + radio.setChannel(NRFchannel[chPointer]); + payload.channel = NRFchannel[chPointer]; + } + + if (millis() - lastRecvTime > 1000) { // set all analog values to their middle position, if no RC signal is received during 1s! + data.axis1 = 50; // Aileron (Steering for car) + data.axis2 = 50; // Elevator + data.axis3 = 50; // Throttle + data.axis4 = 50; // Rudder + hazard = true; // Enable hazard lights + payload.batteryOk = true; // Clear low battery alert (allows to re-enable the vehicle, if you switch off the transmitter) +#ifdef DEBUG + Serial.println("No Radio Available - Check Transmitter!"); +#endif + } + + if (millis() - lastRecvTime > 2000) { + setupRadio(); // re-initialize radio + lastRecvTime = millis(); + } +#endif +} + + +// +// ======================================================================================================= +// SBUS COMMANDS TO LIGHT- & SOUND CONTROLLER (if not DEBUG, not TXO_momentary1, not TXO_toggle1, not headLights) +// ======================================================================================================= +// + +void sendSbusCommands() +{ + vehicleConfig* cfg = getConfigptr(); + + static unsigned long lastSbusTime; + uint16_t channels[16]; + + // See: https://github.com/TheDIYGuy999/Rc_Engine_Sound_ESP32 + + if (cfg->serialCommands) { // only, if we are in serial command mode + if (millis() - lastSbusTime > 14) { // Send the data every 14ms + lastSbusTime = millis(); + + // Fill SBUS packet with our channels + + // Proportional channels + channels[0] = map(data.axis1, 0, 100, 172, 1811); + channels[1] = map(data.axis2, 0, 100, 172, 1811); + channels[2] = map(data.axis3, 0, 100, 172, 1811); + channels[3] = map(data.axis4, 0, 100, 172, 1811); + channels[4] = map(data.pot1, 0, 100, 172, 1811); + + // Switches etc. + if (data.mode1) channels[5] = 1811; else channels[5] = 172; + if (data.mode2) channels[6] = 1811; else channels[6] = 172; + if (data.momentary1) channels[7] = 1811; else channels[7] = 172; + if (getHazard()) channels[8] = 1811; else channels[8] = 172; + if (getLeft()) channels[9] = 1811; else channels[9] = 172; + if (getRight()) channels[10] = 1811; else channels[10] = 172; + + // write the SBUS packet +#ifdef CONFIG_HAS_SBUS + x8r.write(&channels[0]); +#endif + } + } +} + +// +// ======================================================================================================= +// SERIAL COMMANDS TO LIGHT- & SOUND CONTROLLER (if not DEBUG, not TXO_momentary1, not TXO_toggle1, not headLights) +// ======================================================================================================= +// + +void sendSerialCommands() +{ + vehicleConfig* cfg = getConfigptr(); + + static unsigned long lastSerialTime; + + // '\n' is used as delimiter (separator of variables) during parsing on the sound controller + // it is generated by the "println" (print line) command! + // See: https://github.com/TheDIYGuy999/Rc_Engine_Sound_ESP32 + + if (cfg->serialCommands) { // only, if we are in serial command mode + if (millis() - lastSerialTime > 20) { // Send the data every 20ms + lastSerialTime = millis(); + Serial.print('<'); // Start marker + Serial.println(data.axis1); + Serial.println(data.axis2); + Serial.println(data.axis3); + Serial.println(data.axis4); + Serial.println(data.pot1); + Serial.println(data.mode1); + Serial.println(data.mode2); + Serial.println(data.momentary1); + Serial.println(getHazard()); + Serial.println(getLeft()); + Serial.println(getRight()); + Serial.print('>'); // End marker + } + } +} diff --git a/src/radio.h b/src/radio.h new file mode 100644 index 0000000..53bfcc8 --- /dev/null +++ b/src/radio.h @@ -0,0 +1,44 @@ +#ifndef RADIOH +#define RADIOH + +#include +#include "../../lib/SBUS/src/SBUS.h" + + + + +// The size of this struct should not exceed 32 bytes +struct RcData { + byte axis1; // Aileron (Steering for car) + byte axis2; // Elevator + byte axis3; // Throttle + byte axis4; // Rudder + bool mode1 = false; // Mode1 (toggle speed limitation) + bool mode2 = false; // Mode2 (toggle acc. / dec. limitation) + bool momentary1 = false; // Momentary push button + byte pot1; // Potentiometer +}; + + +// This struct defines data, which are embedded inside the ACK payload +struct ackPayload { + float vcc; // vehicle vcc voltage + float batteryVoltage; // vehicle battery voltage + bool batteryOk = true; // the vehicle battery voltage is OK! + byte channel = 1; // the channel number +}; + + + +// Function prototypes +ackPayload* getPayloadptr( void ); +RcData* getDataptr(void); +void readRadio( void ); +void setupRadio( void ); + +void sendSerialCommands( void ); +SBUS* getSBUSptr( void ); +void sendSbusCommands( void ); + + +#endif // RADIOH \ No newline at end of file diff --git a/src/rc_board.h b/src/rc_board.h index 615cc0c..e6257f4 100644 --- a/src/rc_board.h +++ b/src/rc_board.h @@ -1,6 +1,13 @@ #ifndef RCBOARDH #define RCBOARDH +//#define CONFIG_HAS_TONE +//#define CONFIG_HAS_NRF24 +#define CONFIG_HAS_SBUS +//#define CONFIG_HAS_LIGHTS +//#define CONFIG_HAS_SERVO + + #define PIN_ADC_VBAT PA0 //NRF24 pins #define PIN_NRF_CE PA1 @@ -16,10 +23,10 @@ #define PIN_MPU_SCL PB6 //MC34933 motordrive pins -#define PIN_M2A PA15 -#define PIN_M2B PA12 -#define PIN_M1A PA10 -#define PIN_M1B PA11 +#define PIN_MOTOR_M2A PA15 +#define PIN_MOTOR_M2B PA12 +#define PIN_MOTOR_M1A PA10 +#define PIN_MOTOR_M1B PA11 //CPU pins #define PIN_MCU_BOOT1 PB2 diff --git a/src/readVCC.cpp b/src/readVCC.cpp new file mode 100644 index 0000000..eb36cdc --- /dev/null +++ b/src/readVCC.cpp @@ -0,0 +1,40 @@ +#include "Arduino.h" +#include "rc_board.h" + +// +// ======================================================================================================= +// READ VCC VOLTAGE +// ======================================================================================================= +// + +long readVcc() { + // Read 1.1V reference against AVcc + // set the reference to Vcc and the measurement to the internal 1.1V reference +#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1); +#elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) + ADMUX = _BV(MUX5) | _BV(MUX0); +#elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) + ADMUX = _BV(MUX3) | _BV(MUX2); +#elif defined (STM32F1xx) + //nothing to set +#else + ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1); +#endif + +#if defined (STM32F1xx) + long result = analogRead(PIN_ADC_VBAT); +#else + delayMicroseconds(500); // Wait for Vref to settle (was delay(2) analogRead() will be affected, if < 300micros!! + ADCSRA |= _BV(ADSC); // Start conversion + while (bit_is_set(ADCSRA, ADSC)); // measuring + + uint8_t low = ADCL; // must read ADCL first - it then locks ADCH + uint8_t high = ADCH; // unlocks both + + long result = (high << 8) | low; +#endif + result = 1125300L / result; // Calculate Vcc (in mV); 1125300 = 1.1*1023*1000 + return result; // Vcc in millivolts + +} diff --git a/src/readVCC.h b/src/readVCC.h new file mode 100644 index 0000000..7fa4701 --- /dev/null +++ b/src/readVCC.h @@ -0,0 +1,6 @@ +#ifndef readVCC_h +#define readVCC_h + +long readVcc(); + +#endif //readVCC_h \ No newline at end of file diff --git a/src/servos.cpp b/src/servos.cpp new file mode 100644 index 0000000..69b4679 --- /dev/null +++ b/src/servos.cpp @@ -0,0 +1,72 @@ +#include +#include "servos.h" + +// +// ======================================================================================================= +// WRITE SERVO POSITIONS +// ======================================================================================================= +// + +void writeServos() +{ +#ifdef CONFIG_HAS_SERVO + // Aileron or Steering + if (vehicleType != 5) { // If not car with MSRC stabilty control + servo1.write(map(data.axis1, 100, 0, lim1L, lim1R) ); // 45 - 135° + } + + // Elevator or shifting gearbox actuator +#ifdef TWO_SPEED_GEARBOX // Shifting gearbox mode, controlled by "Mode 1" button + if (!tailLights) { + if (data.mode1)servo2.write(lim2L); + else servo2.write(lim2R); + } + +#else +#ifdef THREE_SPEED_GEARBOX // Shifting gearbox mode, controlled by 3 position switch + if (!tailLights) { + if (data.axis2 < 10)servo2.write(lim2R); + else if (data.axis2 > 90)servo2.write(lim2L); + else servo2.write(lim2C); + } + +#else // Servo controlled by joystick CH2 + if (!tailLights) servo2.write(map(data.axis2, 100, 0, lim2L, lim2R) ); // 45 - 135° +#endif +#endif + + // Throttle (for ESC control, if you don't use the internal TB6612FNG motor driver) + if (data.mode1) { // limited speed! + servo3.write(map(data.axis3, 100, 0, lim3Llow, lim3Rlow ) ); // less than +/- 45° + } + else { // full speed! + servo3.write(map(data.axis3, 100, 0, lim3L, lim3R) ); // 45 - 135° + } + + // Rudder or trailer unlock actuator +#ifdef TRACTOR_TRAILER_UNLOCK // Tractor trailer unlocking, controlled by "Momentary 1" ("Back / Pulse") button + if (!beacons && !potentiometer1) { + if (data.momentary1)servo4.write(lim4L); + else servo4.write(lim4R); + } + +#else // Servo controlled by joystick CH4 + if (!potentiometer1) { // Servo 4 controlled by CH4 + if (!beacons) servo4.write(map(data.axis4, 100, 0, lim4L, lim4R) ); // 45 - 135° + } + else { // Servo 4 controlled by transmitter potentiometer knob + if (!beacons) servo4.write(map(data.pot1, 0, 100, 45, 135) ); // 45 - 135° + } +#endif + + // Axis 2 on the joystick switches engine sound on servo channel 3 on and off! + if (engineSound) { + if (data.axis2 > 80) { + servo3.attach(A2); // Enable servo 3 pulse + } + if (data. axis2 < 20) { + servo3.detach(); // Disable servo 3 pulse = engine off signal for "TheDIYGuy999" engine simulator! + } + } +#endif //CONFIG_HAS_SERVO +} \ No newline at end of file diff --git a/src/servos.h b/src/servos.h new file mode 100644 index 0000000..1e8a0ab --- /dev/null +++ b/src/servos.h @@ -0,0 +1,6 @@ +#ifndef SERVOSH +#define SERVOSH + +void writeServos(); + +#endif //SERVOSH \ No newline at end of file diff --git a/src/steeringCurves.cpp b/src/steeringCurves.cpp new file mode 100644 index 0000000..c205c11 --- /dev/null +++ b/src/steeringCurves.cpp @@ -0,0 +1,35 @@ +#include +#include "steeringCurves.h" + +// +// ======================================================================================================= +// NONLINEAR ARRAYS FOR STEERING OVERLAY +// ======================================================================================================= +// + +// In order to optimize the steering behaviour for your vehicle, just change the steering curves in the arrays below + + + +// +// ======================================================================================================= +// ARRAY INTERPOLATION +// ======================================================================================================= +// + +// Credit: http://interface.khm.de/index.php/lab/interfaces-advanced/nonlinear-mapping/ + +int reMap(float pts[][2], int input) { + int rr = 0; + float mm; + + for (int nn = 0; nn < 5; nn++) { + if (input >= pts[nn][0] && input <= pts[nn + 1][0]) { + mm = ( pts[nn][1] - pts[nn + 1][1] ) / ( pts[nn][0] - pts[nn + 1][0] ); + mm = mm * (input - pts[nn][0]); + mm = mm + pts[nn][1]; + rr = mm; + } + } + return (rr); +} \ No newline at end of file diff --git a/src/steeringCurves.h b/src/steeringCurves.h new file mode 100644 index 0000000..37fe6ab --- /dev/null +++ b/src/steeringCurves.h @@ -0,0 +1,7 @@ +#ifndef steeringCurves_h +#define steeringCurves_h + + +int reMap(float pts[][2], int input); + +#endif //steeringCurves_h diff --git a/src/tone.cpp b/src/tone.cpp new file mode 100644 index 0000000..635a544 --- /dev/null +++ b/src/tone.cpp @@ -0,0 +1,32 @@ +#include +#include "tone.h" +#include "vehicleConfig.h" + + +// +// ======================================================================================================= +// TONE FUNCTION FOR STAR WARS R2-D2 +// ======================================================================================================= +// + +// Engine sound +bool engineOn = false; + +int r2d2Tones[] = { + 3520, 3136, 2637, 2093, 2349, 3951, 2794, 4186 +}; + +void R2D2_tell() +{ + + vehicleConfig* cfg = getConfigptr(); + + if (cfg->gettoneOut()) { + for (int notePlay = 0; notePlay < 8; notePlay++) { + int noteRandom = random(7); + tone(A2, r2d2Tones[noteRandom], 80); // Pin, frequency, duration + delay(50); + noTone(A2); + } + } +} \ No newline at end of file diff --git a/src/tone.h b/src/tone.h new file mode 100644 index 0000000..a0d8a09 --- /dev/null +++ b/src/tone.h @@ -0,0 +1,8 @@ +#ifndef tone_h +#define tone_h + +#include "Arduino.h" + +void R2D2_tell(); + +#endif diff --git a/src/vehicleConfig.cpp b/src/vehicleConfig.cpp new file mode 100644 index 0000000..40f8598 --- /dev/null +++ b/src/vehicleConfig.cpp @@ -0,0 +1,2978 @@ +#include +#include "vehicleConfig.h" + + +#define CONFIG_ACTROS // <- Select the correct vehicle configuration here before uploading! + +#define CONFIG_HAS_SBUS // serial connection uses SBUS protocol instead of normal protocol, if not commented out + + +// +// ======================================================================================================= +// VEHICLE SPECIFIC CONFIGURATIONS +// ======================================================================================================= +// + +/* + // Battery type + bool liPo; // If "true", the vehicle can't be reactivated once the cutoff voltage is reached + float cutoffVoltage; // Min. battery discharge voltage, or min. VCC, if board rev. < 1.2 (3.6V for LiPo, 1.1 per NiMh cell) + + // Board type (see: https://www.youtube.com/watch?v=-vbmHhCvspg&t=18s) + float boardVersion; // Board revision (MUST MATCH WITH YOUR BOARD REVISION!!) + bool HP; // HP = "High Power" version (both TB6612FNG channels wired in parallel) -> No motor 1, motor 2 is the driving motor + + // Vehicle address + int vehicleNumber; // This number must be unique for each vehicle! + + // Vehicle type + byte vehicleType; + 0 = car (see: https://www.youtube.com/watch?v=A0SoK7KJxyc). Use this mode, if you want to use indicators. No MPU-6050 MRSC support. + 1 = vhctype_semi caterpillar, 2 = caterpillar (see: https://www.youtube.com/watch?v=Tjikm6hJ8hQ) + 3 = forklift (see: https://www.youtube.com/watch?v=3iXL9WvE4ro) + 4 = balancing (see: https://www.youtube.com/watch?v=zse9-l2Yo3Y) + 5 = car with MRSC (Micro RC Stability Control). Similar with ABS, ESP, Traxxas Stability Management TSM. Potentiometer on A6 input of + your transmitter required! (see: https://www.youtube.com/watch?v=IPve7QpdLBc&t=5s) Indicators can't be used in this mode (locked)! + 6 = simple dual motor plane with differential thrust steering. No rudders. + + // MRSC + #define MRSC_FIXED // Only use this definition, if you want to use an MRSC vehicle without a gain adjustment pot on your transmitter + byte mrscGain = 25; // 25% + + // Lights (see: https://www.youtube.com/watch?v=qbhPqHdBz3o , https://www.youtube.com/watch?v=wBTfsIk4vkU&t=84s) + bool tailLights; // Caution: the taillights are wired to the servo pin 2! -> Servo 2 not usable, if "true" + bool headLights; // Caution: the headlights are wired to the RXI pin! -> Serial not usable, if "true" + bool indicators; // Caution: the indicators are wired to the SDA / SCL pins! -> not usable, if vehicleType is 4 or 5 (locked) + bool beacons; // Caution: the beacons are wired to the servo pin 4! -> Servo 4 not usable, if "true" + + // Servo limits (45 - 135 means - 45° to 45° from the servo middle position) + byte lim1L, lim1R; // Servo 1 + byte lim2L, lim2C, lim2R; // Lim2C for THREE_SPEED_GEARBOX option (center position) + byte lim3L, lim3R; + byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles for external ESC + byte lim4L, lim4R; // Servo 4, also used for tractor trailer unlocking servo (TRACTOR_TRAILER_UNLOCK option) + #define TWO_SPEED_GEARBOX // Vehicle has a mechanical 2 speed shifting gearbox, switched by servo CH2. Not usable in combination with the "tailLights" option + #define THREE_SPEED_GEARBOX // Vehicle has a mechanical 3 speed shifting gearbox, switched by servo CH2. Not usable in combination with the "tailLights" option + + // Motor configuration + int maxPWMfull; // (100% PWM is 255) + int maxPWMlimited; + int minPWM; // backlash compensation for self balancing applications + byte maxAccelerationFull;// (ms per 1 step input signal change) + byte maxAccelerationLimited; + + // Variables for self balancing (vehicleType = 4) only! + float tiltCalibration = -0.2; // -0.2° (+ = leans more backwards!) Vary a bit, if you have slow oscillation with big amplitude + + // Steering configuration (100% torque is 255) + byte steeringTorque; + + // Motor 2 PWM frequency: 32 = 984Hz (default), 8 = 3936Hz, 1 = 31488Hz (only with board version. 1.3 or newer) + byte pwmPrescaler2; // Motor 2 = steering motor (or driving motor in "HP" High Power board version) + + // Additional Channels + bool TXO_momentary1; // The TXO output is linked to the momentary1 channel! -> Serial not usable, if "true" + bool TXO_toggle1; // The TXO output is linked to the toggle1 channel! -> Serial not usable, if "true" + bool potentiometer1; // The potentiometer knob on the transmitter is linked to the servo output CH4 + + // Engine sound (see: https://www.youtube.com/watch?v=pPlrx9yVI6E) + bool engineSound; // true = a "TheDIYGuy999" engine simulator is wired to servo channel 3 (allows to switch off RC signal, deprecated) + + // Tone sound (see: https://www.youtube.com/watch?v=fe5_1mMtcLQ&t=3s) + bool toneOut; // true = a BC337 amplifier for tone() is connected instead of servo 3 +*/ + +vehicleConfig cfg; + +vehicleConfig* getConfigptr( void ) +{ + return &cfg; +} + +void vehicleConfig::begin(vehicleconfig_t vehicle) +{ + switch (vehicle) + { + case COKECANCAR: + { + setBasicParams(true,3.6,1.3,false,6,carwithMRSC,false,false); + setIndicators(false,false,true,false,false); + setServoLimits(45,135,45,135,45,135,75,105,45,135); + setMotorConfig(255,170,0,3,12,0,160,1); + } + break; + + default: + case STANDARDCAR: + { + setBasicParams(false,3.1,1.0,false,1,car,false,false); + setIndicators(false,false,false,false,false); + setServoLimits(45,135,45,135,45,135,75,105,45,135); + setMotorConfig(255,170,0,3,12,0,255,32); + } + break; + } +} + +void vehicleConfig::setBasicParams( + bool liPo, + float cutoffVoltage, + float boardVersion, + bool HP, + int vehicleNumber, + vehicletype_t vehicleType, + bool engineSound, + bool toneOut + ) + { + m_liPo = liPo; + m_cutoffVoltage = cutoffVoltage; + m_boardVersion = boardVersion; + m_HP = HP; + m_vehicleNumber = vehicleNumber; + m_vehicleType = vehicleType; + m_engineSound = engineSound; + m_toneOut = toneOut; + } + + void vehicleConfig::setServoLimits( + byte lim1L, + byte lim1R, + byte lim2L, + byte lim2R, + byte lim3L, + byte lim3R, + byte lim3Llow, + byte lim3Rlow, + byte lim4L, + byte lim4R + ) + { + m_lim1L = lim1L; + m_lim1R = lim1R; + m_lim2L = lim2L; + m_lim2R = lim2R; + m_lim3L = lim3L; + m_lim3R = lim3R; + m_lim3Llow = lim3Llow; + m_lim3Rlow = lim3Rlow; + m_lim4L = lim4L; + m_lim4R = lim4R; + } + + void vehicleConfig::setIndicators( + bool escBrakeLights, + bool tailLights, + bool headLights, + bool indicators, + bool beacons + ) + { + m_escBrakeLights = escBrakeLights; + m_tailLights = tailLights; + m_headLights = headLights; + m_indicators = indicators; + m_beacons= beacons; + } + + void vehicleConfig::setMotorConfig( + int maxPWMfull, + int maxPWMlimited, + int minPWM, + byte maxAccelerationFull, + byte maxAccelerationLimited, + float tiltCalibration, + byte steeringTorque, + byte pwmPrescaler2 + ) + { + m_maxPWMfull = maxPWMfull; + m_maxPWMlimited = maxPWMlimited; + m_minPWM = minPWM; + m_maxAccelerationFull = maxAccelerationFull; + m_maxAccelerationLimited = maxAccelerationLimited; + m_tiltCalibration = tiltCalibration; + m_steeringTorque = steeringTorque; + m_pwmPrescaler2 = pwmPrescaler2; + } + + void vehicleConfig::setAdditionalChannels( + bool TXO_momentary1, + bool TXO_toggle1, + bool potentiometer1 + ) + { + m_TXO_momentary1 = TXO_momentary1; + m_TXO_toggle1 = TXO_toggle1; + m_potentiometer1 = potentiometer1; + } + + +// // Generic configuration, board v1.0------------------------------------------------------------------------- +// #ifdef CONFIG_GENERIC_V10 +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.1; // trigger, as soon as VCC drops! (no battery sensing) + +// // Board type +// float boardVersion = 1.0; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Generic configuration, board v1.3------------------------------------------------------------------- +// #ifdef CONFIG_GENERIC_V13 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.6; + +// // Board type +// float boardVersion = 1.3; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Generic configuration, board v1.3 HP---------------------------------------------------------------------------- +// #ifdef CONFIG_GENERIC_V13_HP +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.6; + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; // High Power Board! + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool escBrakeLights = true; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Tamiya NEO Fighter Buggy ------------------------------------------------------------------- +// #ifdef CONFIG_TAMIYA_FIGHTER +// // Battery type +// bool liPo = false; // ESC provides protection +// float cutoffVoltage = 4.9; // Regulated 6.0V supply from the ESC + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 41, lim1R = 131; // R 41, L131 +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 125, lim3Rlow = 60; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // WlToys K 1:28 Rally Fiesta---------------------------------------------------------------------------- +// #ifdef CONFIG_FIESTA +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.9; // Regulated 5.0V supply from the ESC + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; // High Power Board! + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 5; // MRSC vehicle! + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 55, lim1R = 150; // R55, L150 +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 140; +// byte lim3Llow = 75, lim3Rlow = 110; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Disney Lightning McQueen 95---------------------------------------------------------------------- +// #ifdef CONFIG_MC_QUEEN +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.1; // trigger, as soon as VCC drops! (no battery sensing) + +// // Board type +// float boardVersion = 1.0; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 61, lim1R = 104; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Tamiya VW GOLF Mk. 1 Racing Group 2 ------------------------------------------------------------------- +// #ifdef CONFIG_TAMIYA_GOLF +// // Battery type +// bool liPo = false; // ESC provides protection +// float cutoffVoltage = 4.9; // Regulated 6.0V supply from the ESC + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 2; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 110, lim1R = 60; // R 115, L62 +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 35, lim3R = 150; // ESC output signal not reversed +// byte lim3Llow = 60, lim3Rlow = 125; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // WLtoys 18429 Desert Buggy--------------------------------------------------------------------------- +// #ifdef CONFIG_18429 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.45; // Regulated 5.0V supply from the ESC + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 2; + +// // Vehicle type +// byte vehicleType = 5; + +// // Lights +// bool escBrakeLights = true; +// bool tailLights = true; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 130, lim1R = 75; // R125, L70 Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = true; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Disney 95 "DINOCO"-------------------------------------------------------------------------------- +// #ifdef CONFIG_DINOCO +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.1; // trigger, as soon as VCC drops! (no battery sensing) + +// // Board type +// float boardVersion = 1.0; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 2; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // HG P407 Tamiya Bruiser Clone------------------------------------------------------------------- +// #ifdef CONFIG_HG_P407 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.6; + +// // Board type +// float boardVersion = 1.5; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 3; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 155, lim1R = 65; // Steering R 155, L 65 +// byte lim2L = 71, lim2C = 106, lim2R = 146; // 3 speed gearbox shifting servo 71 = 3. gear, 106 2. gear, 146 = 1. gear. +// byte lim3L = 135, lim3R = 45; +// byte lim3Llow = 105, lim3Rlow = 75; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; +// #define THREE_SPEED_GEARBOX // Vehicle has a mechanical 3 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // JJRC Q46 Buggy ------------------------------------------------------------------- +// #ifdef CONFIG_JJRC_Q46 +// // Battery type +// bool liPo = false; // ESC provides protection +// float cutoffVoltage = 4.9; // Regulated 6.0V supply from the ESC + +// // Board type +// float boardVersion = 1.3; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 3; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 125, lim1R = 55; // R125, L70 Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 125, lim3Rlow = 60; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCANO 6952 "Tuning Radio Control"-------------------------------------------------------- +// #ifdef CONFIG_MECCANO_6953 +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.3; + +// // Board type +// float boardVersion = 1.3; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 3; + +// // Vehicle type +// byte vehicleType = 5; // MRSC stability control + +// // MRSC +// #define MRSC_FIXED +// byte mrscGain = 35; // 35% + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Maisto Mustang GT Fastback--------------------------------------------------------------------------- +// #ifdef CONFIG_MUSTANG +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.45; + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; + +// // Vehicle address +// int vehicleNumber = 3; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 60, lim1R = 129; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 1; // This a show car and we don't want PWM switching noise! So, 31.5KHz frequency. + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = true; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Maisto Dodge Challenger---------------------------------------------------------------------------- +// #ifdef CONFIG_CHALLENGER +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.6; + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; // High Power Board! + +// // Vehicle address +// int vehicleNumber = 4; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = true; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 134, lim1R = 69; // 120 55 +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; // was 245 +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // WPL C34KM Toyota FJ40 Land Cruiser------------------------------------------------------------------- +// #ifdef CONFIG_C34 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.5; // 5V receiver supply voltage surveillance from BEC only! + +// // Board type +// float boardVersion = 1.5; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 4; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 60, lim1R = 145; // R60, L145 +// byte lim2L = 120, lim2R = 65; // Gearbox shifter limits (1. and 2. gear) +// byte lim3L = 65, lim3R = 125; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// byte lim3Llow = 65, lim3Rlow = 125; // same setting (full throttle), because of shifting gearbox! +// byte lim4L = 45, lim4R = 135; +// #define TWO_SPEED_GEARBOX // Vehicle has a mechanical 2 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // GearGmax / KIDZTECH TOYS Porsche GT3 RS 4.0-------------------------------------------------------- +// #ifdef CONFIG_PORSCHE +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.3; + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 1; // one car number 1 and one number 5! + +// // Vehicle type +// byte vehicleType = 5; // MRSC vehicle! + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 62, lim1R = 101; // Car # 5: R 65, L 101 +// //byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Tamiya King Hauler Truck with ESP32 Sound Controller in SBUS mode (ESC controlled by ESP32) ----------------- +// #ifdef CONFIG_KING_HAULER +// // Battery type +// bool liPo = false; // LiPo is protected by ESC +// float cutoffVoltage = 4.0; // 5V supply + +// // Board type +// float boardVersion = 1.5; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 5; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 135, lim1R = 58; // Steering R 135, L 58 +// byte lim2L = 143, lim2C = 90, lim2R = 37; // 3 speed gearbox shifting servo (3., 2., 1. gear) +// byte lim3L = 135, lim3R = 45; // ESC +// byte lim3Llow = 135, lim3Rlow = 45; // limited top speed ESC angles! (full speed in this case) +// byte lim4L = 45, lim4R = 135; // Controlled by pot, for sound triggering! +// #define THREE_SPEED_GEARBOX // Vehicle has a mechanical 3 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Coke Can Car-------------------------------------------------------------------------------------- +// #ifdef CONFIG_CCC +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.6; + +// // Board type +// float boardVersion = 1.3; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 6; + +// // Vehicle type +// byte vehicleType = 5; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 160; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 1; // We don't want PWM switching noise from the steering! So, 31.5KHz frequency. + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // WPL B24 GAZ-66------------------------------------------------- +// #ifdef CONFIG_GAZ_66 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.5; // 5V receiver supply voltage surveillance from BEC only! + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; + +// // Vehicle address +// int vehicleNumber = 5; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = true; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits ---- +// // Steering +// byte lim1L = 57, lim1R = 123; // R57 L123 + +// // Throttle +// byte lim3L = 65, lim3R = 125; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// byte lim3Llow = 65, lim3Rlow = 125; // same setting (full throttle), because of shifting gearbox! + +// // Horn impulse (wired to Das Mikro TBS-Micro input "Prop 2") +// byte lim4L = 135, lim4R = 90; // Generating 90° (neutral) servo position, if switch released and 135°, if pressed +// #define TRACTOR_TRAILER_UNLOCK // TRACTOR_TRAILER_UNLOCK used for 3 position toggle switch. +// //Not usable in combination with the "beacons" and "potentiometer" option + +// // Gearbox shifting +// byte lim2L = 43, lim2R = 120; // Gearbox shifter limits (1. and 2. gear) +// #define TWO_SPEED_GEARBOX // Vehicle has a mechanical 2 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration ---- +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Hercules Hobby Actros Truck with ESP32 Sound Controller in SBUS mode (ESC controlled by ESP32) ----------------- +// #ifdef CONFIG_ACTROS +// // Battery type +// bool liPo = false; // LiPo is protected by ESC +// float cutoffVoltage = 4.0; // 5V supply + +// // Board type +// float boardVersion = 1.5; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 6; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 130, lim1R = 50; // Steering L 130, R 50 +// byte lim2L = 143, lim2C = 90, lim2R = 37; // 3 speed gearbox shifting servo (3., 2., 1. gear) +// byte lim3L = 135, lim3R = 45; // ESC +// byte lim3Llow = 135, lim3Rlow = 45; // limited top speed ESC angles! (full speed in this case) +// byte lim4L = 45, lim4R = 135; // Controlled by pot, for sound triggering! +// #define THREE_SPEED_GEARBOX // Vehicle has a mechanical 3 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Feiyue FY03 Eagle Buggy--------------------------------------------------------------------------- +// #ifdef CONFIG_FY03 +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 4.9; // Regulated 5.0V supply from the ESC + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 6; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 117, lim1R = 62; // R125, L70 Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 110, lim3Rlow = 75; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = true; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // HBX 12891 DUNE THUNDER Buggy--------------------------------------------------------------------------- +// #ifdef CONFIG_HBX_12891 +// // Battery type +// bool liPo = false; // protected by the ESC +// float cutoffVoltage = 3.5; // Regulated 5.0V supply from the ESC, but weak 18650 batteries! + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 7; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 125, lim1R = 70; // R125, L70 Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 110, lim3Rlow = 75; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = true; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // KD-Summit S600 RC Truggy------------------------------------------------------------------------- +// #ifdef CONFIG_KD_SUMMIT +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.9; // 5V receiver supply voltage surveillance from BEC only! + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; // High Power Board! + +// // Vehicle address +// int vehicleNumber = 7; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 130, lim1R = 50; // Direction inversed! +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 65, lim3R = 120; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// byte lim3Llow = 75, lim3Rlow = 110; // limited top speed angles! A slight offset towards reverse is required with this ESC +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // 1:12 MN Model Landrover Defender D90 Brushless with 2 speed transmission--------------------------------- +// #ifdef CONFIG_BRUSHLESS_D90 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.5; // 5V receiver supply voltage surveillance from BEC only! + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 7; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = true; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 75, lim1R = 130; // R65 L120 +// byte lim2L = 10, lim2R = 120; // Gearbox shifter limits (1. and 2. gear) +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 150, lim3Rlow = 35; // same setting (full throttle), because of shifting gearbox! +// byte lim4L = 45, lim4R = 135; +// #define TWO_SPEED_GEARBOX // Vehicle has a mechanical 2 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Receiver board scratch build tutorial---------------------------------------------------------------- +// #ifdef CONFIG_SCRATCH_TUTORIAL +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 4.4; + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; // High Power Board! + +// // Vehicle address +// int vehicleNumber = 8; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 35, lim1R = 132; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 5; +// byte maxAccelerationFull = 12; +// byte maxAccelerationLimited = 15; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + + +// // Maisto Chevy Camaro SS--------------------------------------------------------------------------- +// #ifdef CONFIG_CAMARO +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.45; + +// // Board type +// float boardVersion = 1.3; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 8; + +// // Vehicle type +// byte vehicleType = 5; // MPU6050 module required! + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 127, lim1R = 52; // R120, L45 Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 1; // We don't want PWM switching noise from the steering! So, 31.5KHz frequency. + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // JLB Racing Cheetah---------------------------------------------------------------------------- +// #ifdef CONFIG_CHEETAH +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.6; // Brushless ESC has its own battery protection + +// // Board type +// float boardVersion = 1.3; +// bool HP = true; // High Power Board! + +// // Vehicle address +// int vehicleNumber = 8; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 140, lim1R = 45; // Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 125, lim3Rlow = 60; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; // true = MRSC knob linked to servo CH4! + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // 1:12 MN Model Landrover Defender D90------------------------------------------------- +// #ifdef CONFIG_DEFENDER90 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.5; // 5V receiver supply voltage surveillance from BEC only! + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 8; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = true; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 55, lim1R = 125; // R50 L130 +// byte lim2L = 43, lim2R = 120; // Gearbox shifter limits (1. and 2. gear) +// byte lim3L = 65, lim3R = 125; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// byte lim3Llow = 65, lim3Rlow = 125; // same setting (full throttle), because of shifting gearbox! +// byte lim4L = 45, lim4R = 135; +// #define TWO_SPEED_GEARBOX // Vehicle has a mechanical 2 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Remo Hobby S Max--------------------------------------------------------------------------- +// #ifdef CONFIG_S_MAX +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.45; // Regulated 5.0V supply from the ESC + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 9; + +// // Vehicle type +// byte vehicleType = 5; + +// // Lights +// bool escBrakeLights = true; +// bool tailLights = true; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 125, lim1R = 80; // R125, L70 Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = true; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // 1:18 LaFerrari------------------------------------------------------------------------- +// #ifdef CONFIG_LAFERRARI +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.9; // 5V receiver supply voltage surveillance from BEC only! + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 9; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 50, lim1R = 140; // R50 L140 +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 65, lim3R = 120; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// byte lim3Llow = 75, lim3Rlow = 110; // limited top speed angles! A slight offset towards reverse is required with this ESC +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // 1:16 WPL B-36 Russian URAL-4320 Military Command Truck (old configuration)------------------------------------------------- +// #ifdef CONFIG_WPL_B_36_OLD +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 0.0; // 6V receiver supply voltage, but Sound module causes a lot of noise! + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 9; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 64, lim1R = 137; // R67 L137 +// byte lim2L = 43, lim2R = 120; // Gearbox shifter limits (1. and 2. gear) +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 150, lim3Rlow = 35; // 2 speed transmission, so same values! +// byte lim4L = 45, lim4R = 135; +// #define TWO_SPEED_GEARBOX // Vehicle has a mechanical 2 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = true; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // 1:16 WPL B-36 Russian URAL-4320 Military Command Truck------------------------------------------------- +// #ifdef CONFIG_WPL_B_36 +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 0.0; // 6V receiver supply voltage, but Sound module causes a lot of noise! + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 9; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 64, lim1R = 137; // R67 L137 +// //byte lim2L = 43, lim2R = 120; // Gearbox shifter limits (1. and 2. gear) +// byte lim2L = 120, lim2C = 120, lim2R = 43; // 3 speed gearbox shifting servo (3., 2., 1. gear) +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 150, lim3Rlow = 35; // 2 speed transmission, so same values! +// byte lim4L = 45, lim4R = 135; +// #define THREE_SPEED_GEARBOX // Vehicle has a mechanical 2 speed shifting gearbox, switched by servo CH2. Defined as 3 speed, because we want to use the 3 position switch +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Serial commands for ESP32 light and sound controller test (Tamiya Truck?) --------------------------------------- +// #ifdef CONFIG_SERIAL_TEST +// // Battery type +// bool liPo = false; // LiPo is protected by ESC +// float cutoffVoltage = 4.0; // 5V supply + +// // Board type +// float boardVersion = 1.5; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 9; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 135, lim1R = 45; // Steering R 155, L 65 +// byte lim2L = 45, lim2C = 90, lim2R = 135; // 3 speed gearbox shifting servo 71 = 3. gear, 106 2. gear, 146 = 1. gear. +// byte lim3L = 135, lim3R = 45; // ESC +// byte lim3Llow = 105, lim3Rlow = 75; // limited top speed ESC angles! +// byte lim4L = 45, lim4R = 135; // Controlled by pot, for sound triggering! +// #define THREE_SPEED_GEARBOX // Vehicle has a mechanical 3 speed shifting gearbox, switched by servo CH2. +// // Not usable in combination with the "tailLights" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = false; +// bool potentiometer1 = true; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // 1:16 JJRC Q60 (3D printed ZIL-131 body) Military Truck------------------------------------------------- +// #ifdef CONFIG_JJRC_Q60 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 4.5; // Original 6V NiCd battery pack or 2S LiPo, Receiver 5V supply from ESC + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 10; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = true; +// bool tailLights = true; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 74, lim1R = 117; // R74 L117 +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 65, lim3R = 125; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// byte lim3Llow = 80, lim3Rlow = 110; +// byte lim4L = 55, lim4R = 75; // Tractor trailer coupler unlocking limits (L = "Back / Pulse" button pressed) +// #define TRACTOR_TRAILER_UNLOCK // Vehicle has a trailer unlocking servo, switched by servo CH4. +// //Not usable in combination with the "beacons" and "potentiometer" option + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Wltoys A959--------------------------------------------------------------------------- +// #ifdef CONFIG_A959 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.45; // Regulated 5.0V supply from the ESC + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 10; + +// // Vehicle type +// byte vehicleType = 5; // MRSC Vehicle! + +// // Lights +// bool escBrakeLights = true; +// bool tailLights = true; +// bool headLights = true; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 62, lim1R = 133; // R80, L125 +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 8; // 3936Hz + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = true; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Rui Chuang Forklift------------------------------------------------------------------------- +// #ifdef CONFIG_FORKLIFT +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 4.4; // 4 Eneloop cells + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 10; + +// // Vehicle type +// byte vehicleType = 3; // Forklift mode + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = true; + +// // Servo limits +// byte lim1L = 145, lim1R = 35; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 7; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration (lift in this case) +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // R2-D2 STAR WARS robot----------------------------------------------------------------------- +// #ifdef CONFIG_R2D2 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.45; + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 10; + +// // Vehicle type +// byte vehicleType = 2; // 2 = caterpillar mode + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = true; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = true; +// #endif + +// // Caterpillar test vehicle----------------------------------------------------------------------- +// #ifdef CONFIG_CATERPILLAR_TEST +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.1; + +// // Board type +// float boardVersion = 1.0; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 10; + +// // Vehicle type +// byte vehicleType = 2; // 2 = caterpillar mode + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // Self balancing robot----------------------------------------------------------------------- +// #ifdef CONFIG_SELF_BALANCING +// // Battery type +// bool liPo = false; +// float cutoffVoltage = 3.1; // 4 Eneloop cells + +// // Board type +// float boardVersion = 1.0; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 10; + +// // Vehicle type +// byte vehicleType = 4; // 4 = balancing mode + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 75, lim3Rlow = 105; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 15; // 15 Backlash compensation, important for self balancing! +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = -0.2; // -0.2° (+ = leans more backwards!) Vary a bit, if you have slow oscillation with big amplitude + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = false; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // PIPER J3 CUB Plane----------------------------------------------------------------------- +// #ifdef PIPER_J3 +// // Battery type +// bool liPo = true; +// float cutoffVoltage = 3.1; + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 10; + +// // Vehicle type +// byte vehicleType = 6; // 6 = differential thrust controlled plane + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 45, lim3Rlow = 135; // no limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// byte maxAccelerationFull = 3; +// byte maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCEISO'S MECCANO VEHICLES *********************************************************************************** +// // https://www.youtube.com/channel/UCuzV4gX5T-5FzYAQXltWYnA ****************************************************** + +// // MECCANO V1.2 standard configuration----------------------------------------------------------------- +// #ifdef CONFIG_MECCANO_V12 +// // Battery type +// bool liPo = false; +// bool cutoffVoltage = 3.3; + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// int maxAccelerationFull = 3; +// int maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCAPILLAR----------------------------------------------------------------- +// #ifdef CONFIG_MECCAPILLAR +// // Battery type +// bool liPo = false; +// bool cutoffVoltage = 3.5; + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 1; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 45, lim3Rlow = 135; // no limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// int maxAccelerationFull = 3; +// int maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCANO CRANE CHASSIS----------------------------------------------------------------- +// #ifdef CONFIG_CRANE_CHASSIS +// // Battery type +// bool liPo = false; // ESC provides protection +// bool cutoffVoltage = 3.5; + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 3; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 135, lim1R = 45; // Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// int maxAccelerationFull = 3; +// int maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCANO CRANE TOWER----------------------------------------------------------------- +// #ifdef CONFIG_CRANE_TOWER +// // Battery type +// bool liPo = false; +// bool cutoffVoltage = 3.5; + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 4; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = false; +// bool indicators = false; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 45, lim3R = 135; +// byte lim3Llow = 45, lim3Rlow = 135; // no limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// int maxAccelerationFull = 3; +// int maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCANO CAR 5 Willys Jeep ----------------------------------------------------------- +// #ifdef CONFIG_MECCANO_CAR_5 +// // Battery type +// bool liPo = false; // ESC provides protection +// bool cutoffVoltage = 3.5; + +// // Board type +// float boardVersion = 1.2; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 5; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 135, lim1R = 45; // Steering reversed +// byte lim2L = 45, lim2R = 135; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// int maxAccelerationFull = 3; +// int maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCANO CAR 6 with Dumbo RC 10A ESC------------------------------------------------------- +// #ifdef CONFIG_MECCANO_CAR_6 +// // Battery type +// bool liPo = false; // Protected ESC! +// float cutoffVoltage = 3.5; // 2S LiPo, Receiver 5V supply from ESC + +// // Board type +// float boardVersion = 1.4; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 6; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; // Steering reversed +// byte lim2L = 45, lim2R = 135; +// //byte lim3L = 65, lim3R = 125; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// //byte lim3Llow = 80, lim3Rlow = 110; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// int maxAccelerationFull = 3; +// int maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif + +// // MECCANO CAR 7 with Dumbo RC 10A ESC------------------------------------------------------- +// #ifdef CONFIG_MECCANO_CAR_7 +// // Battery type +// bool liPo = false; // Protected ESC! +// float cutoffVoltage = 3.5; // 2S LiPo, Receiver 5V supply from ESC + +// // Board type +// float boardVersion = 1.5; +// bool HP = false; + +// // Vehicle address +// int vehicleNumber = 7; + +// // Vehicle type +// byte vehicleType = 0; + +// // Lights +// bool escBrakeLights = false; +// bool tailLights = false; +// bool headLights = true; +// bool indicators = true; +// bool beacons = false; + +// // Servo limits +// byte lim1L = 45, lim1R = 135; // Steering reversed +// byte lim2L = 45, lim2R = 135; +// //byte lim3L = 65, lim3R = 125; // +/-25° is still full throttle with the JMT-10A ESC! (Forward, Reverse) +// //byte lim3Llow = 80, lim3Rlow = 110; +// byte lim3L = 150, lim3R = 35; // ESC output signal reversed +// byte lim3Llow = 115, lim3Rlow = 70; // limited top speed angles! +// byte lim4L = 45, lim4R = 135; + +// // Motor configuration +// int maxPWMfull = 255; +// int maxPWMlimited = 170; +// int minPWM = 0; +// int maxAccelerationFull = 3; +// int maxAccelerationLimited = 12; + +// // Variables for self balancing (vehicleType = 4) only! +// float tiltCalibration = 0.0; + +// // Steering configuration +// byte steeringTorque = 255; + +// // Motor 2 PWM frequency +// byte pwmPrescaler2 = 32; + +// // Additional Channels +// bool TXO_momentary1 = true; +// bool TXO_toggle1 = false; +// bool potentiometer1 = false; + +// // Engine sound +// bool engineSound = false; + +// // Tone sound +// bool toneOut = false; +// #endif \ No newline at end of file diff --git a/src/vehicleConfig.h b/src/vehicleConfig.h new file mode 100644 index 0000000..8251ea1 --- /dev/null +++ b/src/vehicleConfig.h @@ -0,0 +1,183 @@ + +#ifndef vehicleConfig_h +#define vehicleConfig_h + +typedef enum vehicleconfig_e +{ + STANDARDCAR, + DRIFTCAR, + COKECANCAR +}vehicleconfig_t; + +typedef enum vehicletype_e +{ + car = 0, + vhctype_semi = 1, + vhctype_unknown = 2, + forklift = 3, + balancingthing = 4, + carwithMRSC = 5, + simpledualmotorplane = 6 +}vehicletype_t; + +class vehicleConfig +{ + bool m_liPo; + float m_cutoffVoltage; // trigger, as soon as VCC drops! (no battery sensing) + + // Board type + float m_boardVersion; + bool m_HP; + + // Vehicle address + int m_vehicleNumber; + + // Vehicle type + vehicletype_t m_vehicleType; + + // Lights + bool m_escBrakeLights; + bool m_tailLights; + bool m_headLights; + bool m_indicators; + bool m_beacons; + + // Servo limits + byte m_lim1L; + byte m_lim1R; + byte m_lim2L; + byte m_lim2R; + byte m_lim3L; + byte m_lim3R; + byte m_lim3Llow; + byte m_lim3Rlow; + byte m_lim4L; + byte m_lim4R; + + // Motor configuration + int m_maxPWMfull; + int m_maxPWMlimited; + int m_minPWM; + byte m_maxAccelerationFull; + byte m_maxAccelerationLimited; + + // Variables for self balancing (vehicleType = 4) only! + float m_tiltCalibration; + // Steering configuration + byte m_steeringTorque; + + // Motor 2 PWM frequency + byte m_pwmPrescaler2; + + // Additional Channels + bool m_TXO_momentary1; + bool m_TXO_toggle1; + bool m_potentiometer1; + + // Engine sound + bool m_engineSound; + + // Tone sound + bool m_toneOut; + + +public: + vehicleConfig() {serialCommands= false;}; + + bool serialCommands; + + + void begin(vehicleconfig_t vehicle); + + void setBasicParams( + bool liPo, + float cutoffVoltage, + float boardVersion, + bool HP, + int vehicleNumber, + vehicletype_t vehicleType, + bool engineSound, + bool toneOut + ); + + void setServoLimits( + byte lim1L, + byte lim1R, + byte lim2L, + byte lim2R, + byte lim3L, + byte lim3R, + byte lim3Llow, + byte lim3Rlow, + byte lim4L, + byte lim4R + ); + + void setIndicators( + bool escBrakeLights, + bool tailLights, + bool headLights, + bool indicators, + bool beacons + ); + + void setMotorConfig( + int maxPWMfull, + int maxPWMlimited, + int minPWM, + byte maxAccelerationFull, + byte maxAccelerationLimited, + float tiltCalibration, + byte steeringTorque, + byte pwmPrescaler2 + ); + + void setAdditionalChannels( + bool TXO_momentary1, + bool TXO_toggle1, + bool potentiometer1 + ); + + bool getliPo(){ return m_liPo; } + float getcutoffVoltage(){ return m_cutoffVoltage;} + float getboardVersion(){ return m_boardVersion;} + bool getHP(){ return m_HP;} + int getvehicleNumber(){ return m_vehicleNumber;} + vehicletype_t getvehicleType(){ return m_vehicleType;} + void setvehicleType(vehicletype_t vhc){m_vehicleType = vhc;} + bool getescBrakeLights(){ return m_escBrakeLights;} + bool gettailLights(){ return m_tailLights;} + bool getheadLights(){ return m_headLights;} + bool getindicators(){ return m_indicators;} + bool getbeacons(){ return m_beacons;} + byte getlim1L(){ return m_lim1L;} + byte getlim1R(){ return m_lim2R;} + byte getlim2L(){ return m_lim2L;} + byte getlim2R(){ return m_lim2R;} + byte getlim3L(){ return m_lim3L;} + byte getlim3R(){ return m_lim3R;} + byte getlim3Llow(){ return m_lim3Llow;} + byte getlim3Rlow(){ return m_lim3Rlow;} + byte getlim4L(){ return m_lim4L;} + byte getlim4R(){ return m_lim4R;} + int getmaxPWMfull(){ return m_maxPWMfull;} + int getmaxPWMlimited(){ return m_maxPWMlimited;} + int getminPWM(){ return m_minPWM;} + byte getmaxAccelerationFull(){ return m_maxAccelerationFull;} + byte getmaxAccelerationLimited(){ return m_maxAccelerationLimited;} + float gettiltCalibration(){ return m_tiltCalibration;} + byte getsteeringTorque(){ return m_steeringTorque;} + byte getpwmPrescaler2(){ return m_pwmPrescaler2; } + void setpwmPrescaler2(byte psc) { m_pwmPrescaler2 = psc;} + bool getTXO_momentary1(){ return m_TXO_momentary1;} + bool getTXO_toggle1(){ return m_TXO_toggle1;} + bool getpotentiometer1(){ return m_potentiometer1; } + bool getengineSound(){ return m_engineSound;} + bool gettoneOut(){ return m_toneOut;} + +}; //class vehicleConfig + +vehicleConfig* getConfigptr( void ); + + +#endif