projects/rc-micro-car
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

re-factoring/re-structure of modules

Browse Source
This commit is contained in:
willem oldemans
2020-09-16 14:54:35 +02:00
parent 2b028d543e
commit c246965111
31 changed files with 4982 additions and 362 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

BIN
include/.DS_Store vendored Normal file
View File

Binary file not shown.

6
platformio.ini
View File

@@ -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

26
rc-micro-car.code-workspace Normal file
View File

@@ -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"
}
}
}

146
src/MC34933.cpp
View File

@@ -1,146 +0,0 @@
#include <Arduino.h>
#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;
}
}

52
src/MC34933.h
View File

@@ -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

241
src/MPU6050.cpp Normal file
View File

@@ -0,0 +1,241 @@
#include "Arduino.h"
#include "MPU6050.h"
#include <Wire.h>
#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;
}

15
src/MPU6050.h Normal file
View File

@@ -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

173
src/balancing.cpp Normal file
View File

@@ -0,0 +1,173 @@
#include <Arduino.h>
#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
}
}

10
src/balancing.h Normal file
View File

@@ -0,0 +1,10 @@
#ifndef BALANCINGH
#define BALANCINGH
void balancing( void );
void driveMotorsBalancing( void );
void setupPid( void );
void mrsc();
#endif //BALANCINGH

92
src/battery.cpp Normal file
View File

@@ -0,0 +1,92 @@
#include <arduino.h>
#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;
}

10
src/battery.h Normal file
View File

@@ -0,0 +1,10 @@
#ifndef BATTERYH
#define BATTERYH
void checkBattery();
float vccAverage();
float batteryAverage();
long readVcc();
#endif //BATTERYH

23
src/helper.h Normal file
View File

@@ -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

180
src/lights.cpp Normal file
View File

@@ -0,0 +1,180 @@
#include <Arduino.h>
#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();
}
}
}

14
src/lights.h Normal file
View File

@@ -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

362
src/main.cpp
View File

@@ -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 <Arduino.h>
#include <ticker.h>
// 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 <Wire.h> // I2C library (for the MPU-6050 gyro /accelerometer)
//#include <RF24.h> // Installed via Tools > Board > Boards Manager > Type RF24
#include <Servo.h>
#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 <math.h>
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps20.h>
//#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
}

326
src/motors.cpp Normal file
View File

@@ -0,0 +1,326 @@
#include <Arduino.h>
#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!
}
}

25
src/motors.h Normal file
View File

@@ -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

217
src/radio.cpp Normal file
View File

@@ -0,0 +1,217 @@
#include <arduino.h>
#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
}
}
}

44
src/radio.h Normal file
View File

@@ -0,0 +1,44 @@
#ifndef RADIOH
#define RADIOH
#include <Arduino.h>
#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

15
src/rc_board.h
View File

@@ -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

40
src/readVCC.cpp Normal file
View File

@@ -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
}

6
src/readVCC.h Normal file
View File

@@ -0,0 +1,6 @@
#ifndef readVCC_h
#define readVCC_h
long readVcc();
#endif //readVCC_h

72
src/servos.cpp Normal file
View File

@@ -0,0 +1,72 @@
#include <Arduino.h>
#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
}

6
src/servos.h Normal file
View File

@@ -0,0 +1,6 @@
#ifndef SERVOSH
#define SERVOSH
void writeServos();
#endif //SERVOSH

35
src/steeringCurves.cpp Normal file
View File

@@ -0,0 +1,35 @@
#include <Arduino.h>
#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);
}

7
src/steeringCurves.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef steeringCurves_h
#define steeringCurves_h
int reMap(float pts[][2], int input);
#endif //steeringCurves_h

32
src/tone.cpp Normal file
View File

@@ -0,0 +1,32 @@
#include <Arduino.h>
#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);
}
}
}

8
src/tone.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef tone_h
#define tone_h
#include "Arduino.h"
void R2D2_tell();
#endif

2978
src/vehicleConfig.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

183
src/vehicleConfig.h Normal file
View File

@@ -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