From 08d35d7b1fd82dba18b320e845aeffb65628613c Mon Sep 17 00:00:00 2001 From: Lukas Date: Sat, 4 Dec 2021 21:26:44 -0800 Subject: [PATCH] Add support for PPM receivers. * Based on Nikkilae's PPM-reader. * This commit is not tested on hardware. --- HoverBot.ino | 53 +++++++++++++++++++++++++++++------------- config.h | 27 ++++++++++++++++------ pwmDecoder.ino | 62 ++++++++++++++++++++++++++------------------------ 3 files changed, 89 insertions(+), 53 deletions(-) diff --git a/HoverBot.ino b/HoverBot.ino index 87964d9..6304923 100644 --- a/HoverBot.ino +++ b/HoverBot.ino @@ -5,6 +5,9 @@ #include #include #include +#if USE_PPM +#include // https://github.com/Nikkilae/PPM-reader +#endif // USE_PPM int motorsActive = 0; // motor state bool tilt_limit_exceeded = false; // motor desired state @@ -17,29 +20,36 @@ Metro controllerMetro = Metro(CONTROLLER_INTERVAL); Metro activationMetro = Metro(ACTIVATION_INTERVAL); // PWM decoder variables -int pwmDutyCycle_throttle = 0; -int pwmDutyCycle_steering = 0; -int pwmDutyCycle_mode = 0; +int steeringCommand_us = 0; +int throttleCommand_us = 0; +int modeCommand_us = 0; + +#if USE_PPM +// Initialize a PPMReader +PPMReader ppmDecoder(PPM_PIN, NUM_PPM_CHANNELS); +#endif // USE_PPM void setup() { - pinMode(LEDPIN, OUTPUT); + pinMode(LEDPIN, OUTPUT); Serial2.begin(BAUDRATE_ODRIVE); // ODrive uses 115200 baud - + Serial.begin(BAUDRATE_PC); // Serial to PC // IMU if (!bno.begin()) { - Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); + Serial.print("No BNO found. Check wiring or I2C address!"); while (1); // halt for safety } delay(1000); bno.setExtCrystalUse(true); +#if !USE_PPM // pwm decoder interrupts - attachInterrupt(digitalPinToInterrupt(PWM_CHANNEL_1), decodePwm_1, CHANGE); - attachInterrupt(digitalPinToInterrupt(PWM_CHANNEL_2), decodePwm_2, CHANGE); - attachInterrupt(digitalPinToInterrupt(PWM_CHANNEL_3), decodePwm_3, CHANGE); + attachInterrupt(digitalPinToInterrupt(PWM_PIN_STEERING), decodeSteeringPwm, CHANGE); + attachInterrupt(digitalPinToInterrupt(PWM_PIN_THROTTLE), decodeThrottlePwm, CHANGE); + attachInterrupt(digitalPinToInterrupt(PWM_PIN_MODE), decodeModePwm, CHANGE); +#endif // !USE_PPM } void loop() { @@ -56,7 +66,12 @@ void controlTask() { void activationTask() { if (activationMetro.check()) { - modeSwitch(pwmDutyCycle_mode > ENGAGE_THRESHOLD && !tilt_limit_exceeded); +#if USE_PPM + // Get command from PPM signal + modeCommand_us = ppmDecoder.latestValidChannelValue(PPM_MODE_CHANNEL, ENGAGE_THRESHOLD); +#endif // USE_PPM + // If using PWM, command will be updated in the ISR + modeSwitch(modeCommand_us > ENGAGE_THRESHOLD && !tilt_limit_exceeded); } } @@ -70,23 +85,29 @@ void motionController() { imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); - if (abs(euler.z()) > TILT_LIMIT){ + if (abs(euler.z()) > TILT_LIMIT) { tilt_limit_exceeded = true; } - else{ + else { tilt_limit_exceeded = false; } // balance controller float balanceControllerOutput = euler.z() * KP_BALANCE + gyro.x() * KD_BALANCE; - // planar controllera (lateral position and steering angle) - float positionControllerOutput = KP_POSITION * (pwmDutyCycle_throttle - PWM_CENTER); - float steeringControllerOutput = KP_STEERING * (pwmDutyCycle_steering - PWM_CENTER) + gyro.z() * KD_ORIENTATION; + // planar controllers (lateral position and steering angle) +#if USE_PPM + // Get commands from PPM signal + throttleCommand_us = ppmDecoder.latestValidChannelValue(PPM_THROTTLE_CHANNEL, PWM_CENTER); + steeringCommand_us = ppmDecoder.latestValidChannelValue(PPM_STEERING_CHANNEL, PWM_CENTER); +#endif // USE_PPM + // If using PWM, commands will be updated in the ISRs + float positionControllerOutput = KP_POSITION * (throttleCommand_us - PWM_CENTER); + float steeringControllerOutput = KP_STEERING * (steeringCommand_us - PWM_CENTER) + gyro.z() * KD_ORIENTATION; float controllerOutput_right = balanceControllerOutput + positionControllerOutput + steeringControllerOutput; float controllerOutput_left = balanceControllerOutput + positionControllerOutput - steeringControllerOutput; - + odrive.SetCurrent(0, MOTORDIR_0 * controllerOutput_right); odrive.SetCurrent(1, MOTORDIR_1 * controllerOutput_left); } diff --git a/config.h b/config.h index 060d1cb..f4d8052 100644 --- a/config.h +++ b/config.h @@ -1,14 +1,12 @@ /* - * General configuration for HoverBot.ino - * Tested on an Arduino Mega - */ + General configuration for HoverBot.ino + Tested on an Arduino Mega +*/ // HARDWARE -#define PWM_CHANNEL_1 2 -#define PWM_CHANNEL_2 3 -#define PWM_CHANNEL_3 18 #define LEDPIN 13 +// MOTOR DIRECTIONS #define MOTORDIR_0 -1 #define MOTORDIR_1 1 @@ -20,7 +18,22 @@ #define KD_ORIENTATION 0.01 #define TILT_LIMIT 40 -// RADIO CONTROL +// RADIO CONTROL HARDWARE +// true for PPM receivers, false for PWM reicevers +#define USE_PPM false +#if USE_PPM +#define PPM_PIN 2 // Arduino interrupt pin connected to receiver PPM output +#define NUM_PPM_CHANNELS 8 // Number of channels transmitted in the PPM signal +#define PPM_STEERING_CHANNEL 1 // RC channel for steering +#define PPM_THROTTLE_CHANNEL 2 // RC channel for throttle +#define PPM_MODE_CHANNEL 3 // RC channel for mode selection +#else // Using PWM +#define PWM_PIN_STEERING 2 +#define PWM_PIN_THROTTLE 3 +#define PWM_PIN_MODE 18 +#endif // USE_PPM + +// RADIO CONTROL PARAMETERS #define ENGAGE_THRESHOLD 1500 #define PWM_CENTER 1500 diff --git a/pwmDecoder.ino b/pwmDecoder.ino index a80048f..f6cafab 100644 --- a/pwmDecoder.ino +++ b/pwmDecoder.ino @@ -1,31 +1,33 @@ -// Interrupt callbacks to measure the pwm duty cycle - -void decodePwm_1() { - static unsigned long riseTime = micros(); // initialize riseTime - if (digitalRead(PWM_CHANNEL_1) == 1) { // the signal has risen - riseTime = micros(); // save the rise time - } - else { // the signal has fallen - pwmDutyCycle_steering = micros() - riseTime; // compute the duration of the high pulse - } -} - -void decodePwm_2() { - static unsigned long riseTime = micros(); // initialize riseTime - if (digitalRead(PWM_CHANNEL_2) == 1) { // the signal has risen - riseTime = micros(); // save the rise time - } - else { // the signal has fallen - pwmDutyCycle_throttle = micros() - riseTime; // compute the duration of the high pulse - } -} - -void decodePwm_3() { - static unsigned long riseTime = micros(); // initialize riseTime - if (digitalRead(PWM_CHANNEL_3) == 1) { // the signal has risen - riseTime = micros(); // save the rise time - } - else { // the signal has fallen - pwmDutyCycle_mode = micros() - riseTime; // compute the duration of the high pulse - } +#if !USE_PPM +// Interrupt callbacks to measure the pwm duty cycle + +void decodeSteeringPwm() { + static unsigned long riseTime = micros(); // initialize riseTime + if (digitalRead(PWM_PIN_STEERING) == 1) { // the signal has risen + riseTime = micros(); // save the rise time + } + else { // the signal has fallen + steeringCommand_us = micros() - riseTime; // compute the duration of the high pulse + } } + +void decodeThrottlePwm() { + static unsigned long riseTime = micros(); // initialize riseTime + if (digitalRead(PWM_PIN_THROTTLE) == 1) { // the signal has risen + riseTime = micros(); // save the rise time + } + else { // the signal has fallen + throttleCommand_us = micros() - riseTime; // compute the duration of the high pulse + } +} + +void decodeModePwm() { + static unsigned long riseTime = micros(); // initialize riseTime + if (digitalRead(PWM_PIN_MODE) == 1) { // the signal has risen + riseTime = micros(); // save the rise time + } + else { // the signal has fallen + modeCommand_us = micros() - riseTime; // compute the duration of the high pulse + } +} +#endif // !USE_PPM