From 3b1d46266288302287db6372e54739cdcb4c735b Mon Sep 17 00:00:00 2001 From: Michael Fraser Date: Mon, 12 Jun 2023 20:47:18 +1000 Subject: [PATCH 1/2] Merge ESP32 and ESP32_S2 code into new MultiTarget version This prefers the newer ESP32 version of the file where changes won't break the ESP32_S2 build. --- .../MultiTarget/Main/DiyActivePedal_types.h | 45 + Arduino/MultiTarget/Main/Main.ino | 767 ++++++++++++++++++ 2 files changed, 812 insertions(+) create mode 100644 Arduino/MultiTarget/Main/DiyActivePedal_types.h create mode 100644 Arduino/MultiTarget/Main/Main.ino diff --git a/Arduino/MultiTarget/Main/DiyActivePedal_types.h b/Arduino/MultiTarget/Main/DiyActivePedal_types.h new file mode 100644 index 00000000..0bd7a1bf --- /dev/null +++ b/Arduino/MultiTarget/Main/DiyActivePedal_types.h @@ -0,0 +1,45 @@ +struct DAP_config_st { + // structure identification via payload + uint8_t payloadType; + + // variable to check if structure at receiver matched version from transmitter + uint8_t version; + + // To check if structure is valid + uint8_t checkSum; + + // configure pedal start and endpoint + // In percent + uint8_t pedalStartPosition; + uint8_t pedalEndPosition; + + // configure pedal forces + uint8_t maxForce; + uint8_t preloadForce; + + // design force vs travel curve + // In percent + uint8_t relativeForce_p000; + uint8_t relativeForce_p020; + uint8_t relativeForce_p040; + uint8_t relativeForce_p060; + uint8_t relativeForce_p080; + uint8_t relativeForce_p100; + + // parameter to configure damping + uint8_t dampingPress; + uint8_t dampingPull; + + // configure ABS effect + uint8_t absFrequency; // In Hz + uint8_t absAmplitude; // In steps + + // geometric properties of the pedal + // in mm + uint8_t lengthPedal_AC; + uint8_t horPos_AB; + uint8_t verPos_AB; + uint8_t lengthPedal_CB; + + +} ; \ No newline at end of file diff --git a/Arduino/MultiTarget/Main/Main.ino b/Arduino/MultiTarget/Main/Main.ino new file mode 100644 index 00000000..ce91dd87 --- /dev/null +++ b/Arduino/MultiTarget/Main/Main.ino @@ -0,0 +1,767 @@ +long currentTime = 0; +long elapsedTime = 0; +long previousTime = 0; +double Force_Current_KF = 0.; +double Force_Current_KF_dt = 0.; +float averageCycleTime = 0.0f; +uint64_t maxCycles = 100; +uint64_t cycleIdx = 0; +int32_t joystickNormalizedToInt32 = 0; +float delta_t = 0.; +float delta_t_pow2 = 0.; +float delta_t_pow3 = 0.; +float delta_t_pow4 = 0.; +long Position_Next = 0; +long set = 0; +bool checkPosition = 1; + +bool absActive = 0; +float absFrequency = 2 * PI * 5; +float absAmplitude = 100; +float absTime = 0; +float stepperAbsOffset = 0; +float absDeltaTimeSinceLastTrigger = 0; + + +#include "DiyActivePedal_types.h" +DAP_config_st dap_config_st; + + + + + + +//USBCDC USBSerial; + +#define MIN_STEPS 5 + +//#define SUPPORT_ESP32_PULSE_COUNTER + + +/**********************************************************************************************/ +/* */ +/* target-specific definitions */ +/* */ +/**********************************************************************************************/ + +// Wiring connections +#if CONFIG_IDF_TARGET_ESP32S2 + #define minPin 11 + #define maxPin 10 + #define dirPinStepper 8//17//8//12//3 + #define stepPinStepper 9//16//9//13//2 // step pin must be pin 9 + #define USB_JOYSTICK +#elif CONFIG_IDF_TARGET_ESP32 + #define minPin 34 + #define maxPin 35 + #define dirPinStepper 0//8 + #define stepPinStepper 4//9 + #define BLUETOOTH_GAMEPAD +#endif + + +/**********************************************************************************************/ +/* */ +/* controller definitions */ +/* */ +/**********************************************************************************************/ + +#define JOYSTICK_MIN_VALUE 0 +#define JOYSTICK_MAX_VALUE 10000 + +#if defined USB_JOYSTICK + #include + + Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID, JOYSTICK_TYPE_GAMEPAD, + 0, 0, // Button Count, Hat Switch Count + false, false, false, // X and Y, but no Z Axis + false, false, false, // No Rx, Ry, or Rz + false, false, // No rudder or throttle + false, true, false); // No accelerator, brake, or steering + + void SetupController() { + Joystick.setBrakeRange(JOYSTICK_MIN_VALUE, JOYSTICK_MAX_VALUE); + delay(100); + Joystick.begin(); + } + void SetControllerOutputValue(int32_t value) { + Joystick.setBrake(value); + } + +#elif defined BLUETOOTH_GAMEPAD + #include + + BleGamepad bleGamepad; + BleGamepadConfiguration bleGamepadConfig; + + void SetupController() { + bleGamepadConfig.setControllerType(CONTROLLER_TYPE_MULTI_AXIS); // CONTROLLER_TYPE_JOYSTICK, CONTROLLER_TYPE_GAMEPAD (DEFAULT), CONTROLLER_TYPE_MULTI_AXIS + bleGamepadConfig.setAxesMin(JOYSTICK_MIN_VALUE); // 0 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal + bleGamepadConfig.setAxesMax(JOYSTICK_MAX_VALUE); // 32767 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal + bleGamepad.begin(&bleGamepadConfig); + } + void SetControllerOutputValue(int32_t value) { + if (bleGamepad.isConnected()) { + bleGamepad.setAxes(value, 0, 0, 0, 0, 0, 0, 0); + } + } + +#endif + + + + + +/**********************************************************************************************/ +/* */ +/* endstop definitions */ +/* */ +/**********************************************************************************************/ +#define ENDSTOP_MOVEMENT 5 // movement per cycle to find endstop positions + +long stepperPosMin = 0; +long stepperPosMax = 0; +long stepperPosMin_global = 0; +long stepperPosMax_global = 0; +bool minEndstopNotTriggered = false; +bool maxEndstopNotTriggered = false; +long stepperPosPrevious = 0; +long stepperPosCurrent = 0; + + +/**********************************************************************************************/ +/* */ +/* pedal mechanics definitions */ +/* */ +/**********************************************************************************************/ +float startPosRel = 0.35; +float endPosRel = 0.8; + +float springStiffnesss = 1; +float springStiffnesssInv = 1; +float Force_Min = 0.1; //Min Force in lb to activate Movement +float Force_Max = 9.; //Max Force in lb = Max Travel Position +double conversion = 4000.; + + +/**********************************************************************************************/ +/* */ +/* Kalman filter definitions */ +/* */ +/**********************************************************************************************/ +#include +using namespace BLA; +// Configuration of Kalman filter +// assume constant rate of change +// observed states: +// x = [force, d force / dt] +// state transition matrix +// x_k+1 = [1, delta_t; 0, 1] * x_k + + +// Dimensions of the matrices +#define KF_CONST_VEL +#define Nstate 2 // length of the state vector +#define Nobs 1 // length of the measurement vector +#define KF_MODEL_NOISE_FORCE_ACCELERATION (float)500.0f // adjust model noise here + +KALMAN K; // your Kalman filter +BLA::Matrix obs; // observation vector + + + +/**********************************************************************************************/ +/* */ +/* loadcell definitions */ +/* */ +/**********************************************************************************************/ +#define LOADCELL_STD_MIN 0.001f +#define LOADCELL_VARIANCE_MIN LOADCELL_STD_MIN*LOADCELL_STD_MIN + +float loadcellOffset = 0.0f; //offset value +float varEstimate = 0.0f; // estimated loadcell variance +float stdEstimate = 0.0f; + + + +/**********************************************************************************************/ +/* */ +/* stepper motor definitions */ +/* */ +/**********************************************************************************************/ + +#include "FastAccelStepper.h" + +//no clue what this does +FastAccelStepperEngine engine = FastAccelStepperEngine(); +FastAccelStepper *stepper = NULL; + + +#define TRAVEL_PER_ROTATION_IN_MM (float)5.0f +#define STEPS_PER_MOTOR_REVOLUTION (float)800.0f +#define MAXIMUM_STEPPER_RPM (float)4000.0f +#define MAXIMUM_STEPPER_SPEED (MAXIMUM_STEPPER_RPM/60*STEPS_PER_MOTOR_REVOLUTION) //needs to be in us per step || 1 sec = 1000000 us +#define SLOW_STEPPER_SPEED (float)(MAXIMUM_STEPPER_SPEED * 0.05f) +#define MAXIMUM_STEPPER_ACCELERATION (float)1e6 + + + + +/**********************************************************************************************/ +/* */ +/* ADC definitions */ +/* */ +/**********************************************************************************************/ +#include +#include +#define NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION 1000 + +float clockMHZ = 7.68; // crystal frequency used on ADS1256 +float vRef = 2.5; // voltage reference + +// Construct and init ADS1256 object +ADS1256 adc(clockMHZ,vRef,false); // RESETPIN is permanently tied to 3.3v +double loadcellReading; + + + + + + + + + + +/**********************************************************************************************/ +/* */ +/* helper function */ +/* */ +/**********************************************************************************************/ + + + +// initialize configuration struct at startup +void initConfig() +{ + + dap_config_st.payloadType = 100; + dap_config_st.version = 0; + dap_config_st.pedalStartPosition = 35; + dap_config_st.pedalEndPosition = 80; + + dap_config_st.maxForce = 90; + dap_config_st.preloadForce = 1; + + dap_config_st.relativeForce_p000 = 0; + dap_config_st.relativeForce_p020 = 20; + dap_config_st.relativeForce_p040 = 40; + dap_config_st.relativeForce_p060 = 60; + dap_config_st.relativeForce_p080 = 80; + dap_config_st.relativeForce_p100 = 100; + + dap_config_st.dampingPress = 0; + dap_config_st.dampingPull = 0; + + dap_config_st.absFrequency = 5; + dap_config_st.absAmplitude = 100; + + dap_config_st.lengthPedal_AC = 100; + dap_config_st.horPos_AB = 150; + dap_config_st.verPos_AB = 50; + dap_config_st.lengthPedal_CB = 150; +} + +// update the local variables used for computation from the config struct +void updateComputationalVariablesFromConfig() +{ + + startPosRel = ((float)dap_config_st.pedalStartPosition) / 100.0f; + endPosRel = ((float)dap_config_st.pedalEndPosition) / 100.0f; + + Force_Min = ((float)dap_config_st.preloadForce) / 10.0f; + Force_Max = ((float)dap_config_st.maxForce) / 10.0f; + + absFrequency = ((float)dap_config_st.absFrequency); + absAmplitude = ((float)dap_config_st.absAmplitude); + +} + +// compute pedal incline angle +float computePedalInclineAngle(float sledPosInCm) +{ + + // see https://de.wikipedia.org/wiki/Kosinussatz + // A: is lower pedal pivot + // C: is upper pedal pivot + // B: is rear pedal pivot + float a = ((float)dap_config_st.lengthPedal_CB) / 10.0f; + float b = ((float)dap_config_st.lengthPedal_AC) / 10.0f; + float c_ver = ((float)dap_config_st.verPos_AB) / 10.0f; + float c_hor = ((float)dap_config_st.horPos_AB) / 10.0f; + c_hor += sledPosInCm; + float c = sqrtf(c_ver * c_ver + c_hor * c_hor); + + + float nom = a*a + b*b - c*c; + float den = 2 * a * b; + + float gamma = 0; + + if (abs(den) > 0.01) + { + gamma = acos( nom / den ); + } + + + // add incline due to AB incline --> result is incline realtive to horizontal + if (abs(c_hor)>0.01) + { + gamma += atan(c_ver / c_hor); + } + + return gamma; + +} + + +/**********************************************************************************************/ +/* */ +/* setup function */ +/* */ +/**********************************************************************************************/ +void setup() +{ + //Serial.begin(115200); + Serial.begin(921600); + Serial.setTimeout(5); + + // initialize configuration and update local variables + initConfig(); + updateComputationalVariablesFromConfig(); + + + //USBSerial.begin(921600); + + delay(1000); + + SetupController(); + + + // define endstop switch + pinMode(minPin, INPUT); + pinMode(maxPin, INPUT); + + + engine.init(); + stepper = engine.stepperConnectToPin(stepPinStepper); + + + Serial.println("Starting ADC"); + adc.initSpi(clockMHZ); + delay(1000); + Serial.println("ADS: send SDATAC command"); + //adc.sendCommand(ADS1256_CMD_SDATAC); + + + // start the ADS1256 with data rate of 15kSPS SPS and gain x64 + adc.begin(ADS1256_DRATE_15000SPS,ADS1256_GAIN_64,false); + Serial.println("ADC Started"); + + adc.waitDRDY(); // wait for DRDY to go low before changing multiplexer register + adc.setChannel(0,1); // Set the MUX for differential between ch2 and 3 + adc.setConversionFactor(conversion); + + + + Serial.println("ADC: Identify loadcell offset"); + // Due to construction and gravity, the loadcell measures an initial voltage difference. + // To compensate this difference, the difference is estimated by moving average filter. + float ival = 0; + loadcellOffset = 0.0f; + for (long i = 0; i < NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION; i++){ + loadcellReading = adc.readCurrentChannel(); // DOUT arriving here are from MUX AIN0 and + ival = loadcellReading / (float)NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION; + Serial.println(loadcellReading,10); + loadcellOffset += ival; + } + + Serial.print("Offset "); + Serial.println(loadcellOffset,10); + + + + // automatically identify sensor noise for KF parameterization + #ifdef ESTIMATE_LOADCELL_VARIANCE + Serial.println("ADC: Identify loadcell variance"); + float varNormalizer = 1. / (float)(NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION - 1); + varEstimate = 0.0f; + for (long i = 0; i < NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION; i++){ + adc.waitDRDY(); // wait for DRDY to go low before next register read + loadcellReading = adc.readCurrentChannel(); // DOUT arriving here are from MUX AIN0 and + ival = (loadcellReading - loadcellOffset); + ival *= ival; + varEstimate += ival * varNormalizer; + Serial.println(loadcellReading,10); + } + + // make sure estimate is nonzero + if (varEstimate < LOADCELL_VARIANCE_MIN){varEstimate = LOADCELL_VARIANCE_MIN; } + varEstimate *= 9; + #else + varEstimate = 0.13f * 0.13f; + #endif + stdEstimate = sqrt(varEstimate); + + Serial.print("stdEstimate:"); + Serial.println(stdEstimate, 6); + + + + //FastAccelStepper setup + if (stepper) { + + Serial.println("Setup stepper!"); + stepper->setDirectionPin(dirPinStepper, false); + stepper->setAutoEnable(true); + + //Stepper Parameters + stepper->setSpeedInHz(MAXIMUM_STEPPER_SPEED); // steps/s + stepper->setAcceleration(MAXIMUM_STEPPER_ACCELERATION); // 100 steps/s² + + delay(5000); + } + + + + + + + + // Find min stepper position + minEndstopNotTriggered = digitalRead(minPin); + Serial.print(minEndstopNotTriggered); + while(minEndstopNotTriggered == true){ + stepper->moveTo(set, true); + minEndstopNotTriggered = digitalRead(minPin); + set = set - ENDSTOP_MOVEMENT; + } + stepper->forceStopAndNewPosition(0); + stepper->moveTo(0); + stepperPosMin_global = (long)stepper->getCurrentPosition(); + stepperPosMin = (long)stepper->getCurrentPosition(); + + Serial.println("The limit switch: Min On"); + Serial.print("Min Position is "); + Serial.println( stepperPosMin ); + + + // Find max stepper position + set = 0; + maxEndstopNotTriggered = digitalRead(maxPin); + Serial.print(maxEndstopNotTriggered); + while(maxEndstopNotTriggered == true){ + stepper->moveTo(set, true); + maxEndstopNotTriggered = digitalRead(maxPin); + set = set + ENDSTOP_MOVEMENT; + } + stepperPosMax_global = (long)stepper->getCurrentPosition(); + stepperPosMax = (long)stepper->getCurrentPosition(); + + Serial.println("The limit switch: Max On"); + Serial.print("Max Position is "); + Serial.println( stepperPosMax ); + + + + // correct start and end position as requested from the user + float stepperRange = (stepperPosMax - stepperPosMin); + stepperPosMin = 0*stepperPosMin + stepperRange * startPosRel; + stepperPosMax = 0*stepperPosMin + stepperRange * endPosRel; + + // move to initial position + stepper->moveTo(stepperPosMin, true); + + + // compute pedal stiffness parameters + springStiffnesss = (Force_Max-Force_Min) / (float)(stepperPosMax-stepperPosMin); + springStiffnesssInv = 1.0 / springStiffnesss; + + // obtain current stepper position + stepperPosPrevious = stepper->getCurrentPosition(); + + + + // Kalman filter setup + // example of evolution matrix. Size is + K.F = {1.0, 0.0, + 0.0, 1.0}; + + // example of measurement matrix. Size is + K.H = {1.0, 0.0}; + + // example of model covariance matrix. Size is + K.Q = {1.0f, 0.0, + 0.0, 1.0}; + + // example of measurement covariance matrix. Size is + K.R = {varEstimate}; + + + + + + + + + +/*#if defined(SUPPORT_ESP32_PULSE_COUNTER) + stepper->attachToPulseCounter(1, 0, 0); +#endif*/ + + + Serial.println("Setup end!"); + + previousTime = micros(); + + + + +} + + + + + + + +/**********************************************************************************************/ +/* */ +/* Main function */ +/* */ +/**********************************************************************************************/ +void loop() +{ + + + // obtain time + currentTime = micros(); + elapsedTime = currentTime - previousTime; + if (elapsedTime<1){elapsedTime=1;} + previousTime = currentTime; + + + + #define RECALIBRATE_POSITION_FROM_Serial + #ifdef RECALIBRATE_POSITION_FROM_Serial + byte n = Serial.available(); + if(n !=0 ) + { + int menuChoice = Serial.parseInt(); + + switch (menuChoice) { + // resset minimum position + case 1: + + //Serial.println("Reset position!"); + set = stepperPosMin_global; + while(minEndstopNotTriggered == true){ + stepper->moveTo(set, true); + minEndstopNotTriggered = digitalRead(minPin); + set = set - ENDSTOP_MOVEMENT; + } + stepper->forceStopAndNewPosition(stepperPosMin_global); + //stepper->moveTo(0); + + break; + + // toggle ABS + case 2: + //Serial.print("Second case:"); + absActive = true; + absDeltaTimeSinceLastTrigger = 0; + break; + + default: + Serial.print("Default case:"); + } + } + #endif + + #define ABS_OSCILLATION + #ifdef ABS_OSCILLATION + + // compute pedal oscillation, when ABS is active + if (absActive) + { + //Serial.print(2); + absTime += elapsedTime * 1e-6; + absDeltaTimeSinceLastTrigger += elapsedTime * 1e-6; + stepperAbsOffset = absAmplitude * sin(absFrequency * absTime); + } + + // reset ABS when trigger is not active anymore + if (absDeltaTimeSinceLastTrigger > 0.1) + { + absTime = 0; + absActive = false; + } + #endif + + + #define COMPUTE_PEDAL_INCLINE_ANGLE + #ifdef COMPUTE_PEDAL_INCLINE_ANGLE + float sledPosition = ((float)stepperPosCurrent) / STEPS_PER_MOTOR_REVOLUTION * TRAVEL_PER_ROTATION_IN_MM; + float pedalInclineAngle = computePedalInclineAngle(sledPosition); + #endif + + + // average execution time averaged over multiple cycles + #define PRINT_CYCLETIME + #ifdef PRINT_CYCLETIME + averageCycleTime += elapsedTime; + cycleIdx++; + if (maxCycles< cycleIdx) + { + cycleIdx = 0; + + averageCycleTime /= (float)maxCycles; + + Serial.println(averageCycleTime); + /*Serial.print("A:"); + Serial.print(loadcellReading,6); + Serial.print(",B:"); + Serial.print(Force_Current_KF,6); + Serial.print(",C:"); + Serial.println(Force_Current_KF_dt,6);*/ + + + + + + averageCycleTime = 0; + } + #endif + + + + + #define RECALIBRATE_POSITION + #ifdef RECALIBRATE_POSITION + // in case the stepper loses its position and therefore an endstop is triggered reset position + minEndstopNotTriggered = digitalRead(minPin); + maxEndstopNotTriggered = digitalRead(maxPin); + + if (minEndstopNotTriggered == false) + { + stepper->forceStopAndNewPosition(stepperPosMin_global); + stepper->moveTo(stepperPosMin, true); + } + if (maxEndstopNotTriggered == false) + { + stepper->forceStopAndNewPosition(stepperPosMax_global); + stepper->moveTo(stepperPosMax, true); + } + + #endif + + // read ADC value + adc.waitDRDY(); // wait for DRDY to go low before next register read + loadcellReading = adc.readCurrentChannel(); // read as voltage according to gain and vref + loadcellReading -= loadcellOffset; + + + // Kalman filter + // update state transition and system covariance matrices + delta_t = (float)elapsedTime / 1000000.0f; // convert to seconds + delta_t_pow2 = delta_t * delta_t; + delta_t_pow3 = delta_t_pow2 * delta_t; + delta_t_pow4 = delta_t_pow2 * delta_t_pow2; + + K.F = {1.0, delta_t, + 0.0, 1.0}; + + double K_Q_11 = KF_MODEL_NOISE_FORCE_ACCELERATION * 0.5f * delta_t_pow3; + K.Q = {KF_MODEL_NOISE_FORCE_ACCELERATION * 0.25f * delta_t_pow4, K_Q_11, + K_Q_11, KF_MODEL_NOISE_FORCE_ACCELERATION * delta_t_pow2}; + + + // APPLY KALMAN FILTER + obs(0) = loadcellReading; + K.update(obs); + Force_Current_KF = K.x(0,0); + Force_Current_KF_dt = K.x(0,1); + + + // compute target position + Position_Next = springStiffnesssInv * (Force_Current_KF-Force_Min) + stepperPosMin ; //Calculates new position using linear function + //Position_Next -= Force_Current_KF_dt * 0.045f * springStiffnesssInv; // D-gain for stability + //Position_Next += 1000; + #ifdef ABS_OSCILLATION + Position_Next += stepperAbsOffset; + #endif + Position_Next = (int32_t)constrain(Position_Next, stepperPosMin, stepperPosMax); + + + + + #define SET_STEPPER + #ifdef SET_STEPPER + // get current stepper position + stepperPosCurrent = stepper->getCurrentPosition(); + + /*#if defined(SUPPORT_ESP32_PULSE_COUNTER) + //if (stepperPosCurrent > (stepperPosMin + 30)) + { + if (checkPosition == 1) + { + checkPosition = 0; + } + int16_t pcnt = stepper->readPulseCounter(); + //if (stepperPosMin != pcnt) + { + Serial.print('A:'); + Serial.print(stepperPosMin); + Serial.print('B:'); + Serial.print(pcnt); + Serial.println(" "); + } + } + else + { + checkPosition = 1; + } + #endif*/ + + + + long movement = abs( stepperPosCurrent - Position_Next); + if (movement>MIN_STEPS ) + { + stepper->moveTo(Position_Next, false); + } + #endif + + + joystickNormalizedToInt32 = ( Force_Current_KF - Force_Min) / (Force_Max-Force_Min) * JOYSTICK_MAX_VALUE; + joystickNormalizedToInt32 = (int32_t)constrain(joystickNormalizedToInt32, JOYSTICK_MIN_VALUE, JOYSTICK_MAX_VALUE); + SetControllerOutputValue(joystickNormalizedToInt32); + + + //#define PRINT_DEBUG + #ifdef PRINT_DEBUG + Serial.print("elapsedTime:"); + Serial.print(elapsedTime); + Serial.print(",instantaneousForceMeasured:"); + Serial.print(loadcellReading,6); + Serial.print(",Kalman_x:"); + Serial.print(Force_Current_KF, 6); + Serial.print(",Position_Next:"); + Serial.print(Position_Next, 6); + Serial.println(" "); + + delay(100); + #endif + + //Serial.println("debug message!"); + +} From 23d974484388ff21d2832513095ec8cd4b2c5f38 Mon Sep 17 00:00:00 2001 From: Michael Fraser Date: Mon, 12 Jun 2023 21:58:23 +1000 Subject: [PATCH 2/2] Merge multithread code into MultiTarget source This makes the multithread code compile with the ESP32_S2 target. --- Arduino/MultiTarget/Main/Main.ino | 438 +++++++++++++++++++++--------- 1 file changed, 306 insertions(+), 132 deletions(-) diff --git a/Arduino/MultiTarget/Main/Main.ino b/Arduino/MultiTarget/Main/Main.ino index ce91dd87..c2ec0c5f 100644 --- a/Arduino/MultiTarget/Main/Main.ino +++ b/Arduino/MultiTarget/Main/Main.ino @@ -4,7 +4,7 @@ long previousTime = 0; double Force_Current_KF = 0.; double Force_Current_KF_dt = 0.; float averageCycleTime = 0.0f; -uint64_t maxCycles = 100; +uint64_t maxCycles = 1000; uint64_t cycleIdx = 0; int32_t joystickNormalizedToInt32 = 0; float delta_t = 0.; @@ -27,6 +27,8 @@ float absDeltaTimeSinceLastTrigger = 0; DAP_config_st dap_config_st; +#define RAD_2_DEG 180.0f / PI + @@ -38,6 +40,32 @@ DAP_config_st dap_config_st; //#define SUPPORT_ESP32_PULSE_COUNTER +//#define PRINT_CYCLETIME + + + +/**********************************************************************************************/ +/* */ +/* multitasking definitions */ +/* */ +/**********************************************************************************************/ +#include "soc/rtc_wdt.h" + +//rtc_wdt_protect_off(); // Turns off the automatic wdt service +//rtc_wdt_enable(); // Turn it on manually +//rtc_wdt_set_time(RTC_WDT_STAGE0, 20000); // Define how long you desire to let dog wait. + + + +TaskHandle_t Task1; +TaskHandle_t Task2; +//SemaphoreHandle_t batton; +//SemaphoreHandle_t semaphore_updateJoystick; + +//static SemaphoreHandle_t batton=NULL; +static SemaphoreHandle_t semaphore_updateJoystick=NULL; + + /**********************************************************************************************/ /* */ /* target-specific definitions */ @@ -84,6 +112,7 @@ DAP_config_st dap_config_st; delay(100); Joystick.begin(); } + bool IsControllerReady() { return true; } void SetControllerOutputValue(int32_t value) { Joystick.setBrake(value); } @@ -91,19 +120,27 @@ DAP_config_st dap_config_st; #elif defined BLUETOOTH_GAMEPAD #include - BleGamepad bleGamepad; - BleGamepadConfiguration bleGamepadConfig; + BleGamepad bleGamepad("DiyActiveBrake", "DiyActiveBrake", 100); void SetupController() { + BleGamepadConfiguration bleGamepadConfig; bleGamepadConfig.setControllerType(CONTROLLER_TYPE_MULTI_AXIS); // CONTROLLER_TYPE_JOYSTICK, CONTROLLER_TYPE_GAMEPAD (DEFAULT), CONTROLLER_TYPE_MULTI_AXIS bleGamepadConfig.setAxesMin(JOYSTICK_MIN_VALUE); // 0 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal bleGamepadConfig.setAxesMax(JOYSTICK_MAX_VALUE); // 32767 --> int16_t - 16 bit signed integer - Can be in decimal or hexadecimal + //bleGamepadConfig.setWhichSpecialButtons(false, false, false, false, false, false, false, false); + //bleGamepadConfig.setWhichAxes(false, false, false, false, false, false, false, false); + bleGamepadConfig.setWhichSimulationControls(false, false, false, true, false); // only brake active + bleGamepadConfig.setButtonCount(0); + bleGamepadConfig.setHatSwitchCount(0); + bleGamepadConfig.setAutoReport(false); bleGamepad.begin(&bleGamepadConfig); } + bool IsControllerReady() { return bleGamepad.isConnected(); } void SetControllerOutputValue(int32_t value) { - if (bleGamepad.isConnected()) { - bleGamepad.setAxes(value, 0, 0, 0, 0, 0, 0, 0); - } + //bleGamepad.setBrake(value); + bleGamepad.setAxes(value, 0, 0, 0, 0, 0, 0, 0); + bleGamepad.sendReport(); + //Serial.println(value); } #endif @@ -265,10 +302,10 @@ void initConfig() dap_config_st.absFrequency = 5; dap_config_st.absAmplitude = 100; - dap_config_st.lengthPedal_AC = 100; - dap_config_st.horPos_AB = 150; - dap_config_st.verPos_AB = 50; - dap_config_st.lengthPedal_CB = 150; + dap_config_st.lengthPedal_AC = 150; + dap_config_st.horPos_AB = 215; + dap_config_st.verPos_AB = 80; + dap_config_st.lengthPedal_CB = 200; } // update the local variables used for computation from the config struct @@ -298,28 +335,47 @@ float computePedalInclineAngle(float sledPosInCm) float b = ((float)dap_config_st.lengthPedal_AC) / 10.0f; float c_ver = ((float)dap_config_st.verPos_AB) / 10.0f; float c_hor = ((float)dap_config_st.horPos_AB) / 10.0f; - c_hor += sledPosInCm; + c_hor += sledPosInCm / 10.0f; float c = sqrtf(c_ver * c_ver + c_hor * c_hor); + /*Serial.print("a: "); + Serial.print(a); - float nom = a*a + b*b - c*c; - float den = 2 * a * b; + Serial.print(", b: "); + Serial.print(b); - float gamma = 0; + Serial.print(", c: "); + Serial.print(c); + + Serial.print(", sledPosInCm: "); + Serial.print(sledPosInCm);*/ + + float nom = b*b + c*c - a*a; + float den = 2 * b * c; + + float alpha = 0; if (abs(den) > 0.01) { - gamma = acos( nom / den ); + alpha = acos( nom / den ); } + + /*Serial.print(", alpha1: "); + Serial.print(alpha * RAD_2_DEG);*/ + // add incline due to AB incline --> result is incline realtive to horizontal if (abs(c_hor)>0.01) { - gamma += atan(c_ver / c_hor); + alpha += atan(c_ver / c_hor); } + + /*Serial.print(", alpha2: "); + Serial.print(alpha * RAD_2_DEG); + Serial.println(" ");*/ - return gamma; + return alpha * RAD_2_DEG; } @@ -335,6 +391,23 @@ void setup() Serial.begin(921600); Serial.setTimeout(5); + + + + //batton = xSemaphoreCreateBinary(); + //semaphore_updateJoystick = xSemaphoreCreateBinary(); + semaphore_updateJoystick = xSemaphoreCreateMutex(); + + + if(semaphore_updateJoystick==NULL) + { + Serial.println("Could not create semaphore"); + ESP.restart(); + } + + disableCore0WDT(); + + // initialize configuration and update local variables initConfig(); updateComputationalVariablesFromConfig(); @@ -354,6 +427,7 @@ void setup() engine.init(); stepper = engine.stepperConnectToPin(stepPinStepper); + //stepper = engine.stepperConnectToPin(stepPinStepper, DRIVER_RMT); Serial.println("Starting ADC"); @@ -381,7 +455,7 @@ void setup() for (long i = 0; i < NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION; i++){ loadcellReading = adc.readCurrentChannel(); // DOUT arriving here are from MUX AIN0 and ival = loadcellReading / (float)NUMBER_OF_SAMPLES_FOR_LOADCELL_OFFFSET_ESTIMATION; - Serial.println(loadcellReading,10); + //Serial.println(loadcellReading,10); loadcellOffset += ival; } @@ -401,7 +475,7 @@ void setup() ival = (loadcellReading - loadcellOffset); ival *= ival; varEstimate += ival * varNormalizer; - Serial.println(loadcellReading,10); + //Serial.println(loadcellReading,10); } // make sure estimate is nonzero @@ -444,7 +518,7 @@ void setup() stepper->moveTo(set, true); minEndstopNotTriggered = digitalRead(minPin); set = set - ENDSTOP_MOVEMENT; - } + } stepper->forceStopAndNewPosition(0); stepper->moveTo(0); stepperPosMin_global = (long)stepper->getCurrentPosition(); @@ -463,7 +537,7 @@ void setup() stepper->moveTo(set, true); maxEndstopNotTriggered = digitalRead(maxPin); set = set + ENDSTOP_MOVEMENT; - } + } stepperPosMax_global = (long)stepper->getCurrentPosition(); stepperPosMax = (long)stepper->getCurrentPosition(); @@ -519,6 +593,28 @@ void setup() #endif*/ + //create a task that will be executed in the Task2code() function, with priority 1 and executed on core 1 + xTaskCreatePinnedToCore( + pedalUpdateTask, /* Task function. */ + "pedalUpdateTask", /* name of task. */ + 10000, /* Stack size of task */ + NULL, /* parameter of the task */ + 1, /* priority of the task */ + &Task2, /* Task handle to keep track of created task */ + 0); /* pin task to core 1 */ + delay(500); + + xTaskCreatePinnedToCore( + serialCommunicationTask, + "serialCommunicationTask", + 10000, + NULL, + 1, + &Task2, + 1); + delay(500); + + Serial.println("Setup end!"); previousTime = micros(); @@ -539,8 +635,32 @@ void setup() /* Main function */ /* */ /**********************************************************************************************/ -void loop() -{ +void loop() { +} + + +/**********************************************************************************************/ +/* */ +/* pedal update task */ +/* */ +/**********************************************************************************************/ + +long cycleIdx2 = 0; + + + //void loop() + void pedalUpdateTask( void * pvParameters ) + { + + for(;;){ + + + // update local variables for this thread + // 1. take semaphore + //xSemaphoreTake(batton, portMAX_DELAY); + // 2. read global variables + // 3. release semaphore + //xSemaphoreGive(batton); // obtain time @@ -550,43 +670,6 @@ void loop() previousTime = currentTime; - - #define RECALIBRATE_POSITION_FROM_Serial - #ifdef RECALIBRATE_POSITION_FROM_Serial - byte n = Serial.available(); - if(n !=0 ) - { - int menuChoice = Serial.parseInt(); - - switch (menuChoice) { - // resset minimum position - case 1: - - //Serial.println("Reset position!"); - set = stepperPosMin_global; - while(minEndstopNotTriggered == true){ - stepper->moveTo(set, true); - minEndstopNotTriggered = digitalRead(minPin); - set = set - ENDSTOP_MOVEMENT; - } - stepper->forceStopAndNewPosition(stepperPosMin_global); - //stepper->moveTo(0); - - break; - - // toggle ABS - case 2: - //Serial.print("Second case:"); - absActive = true; - absDeltaTimeSinceLastTrigger = 0; - break; - - default: - Serial.print("Default case:"); - } - } - #endif - #define ABS_OSCILLATION #ifdef ABS_OSCILLATION @@ -608,62 +691,31 @@ void loop() #endif - #define COMPUTE_PEDAL_INCLINE_ANGLE + //#define COMPUTE_PEDAL_INCLINE_ANGLE #ifdef COMPUTE_PEDAL_INCLINE_ANGLE float sledPosition = ((float)stepperPosCurrent) / STEPS_PER_MOTOR_REVOLUTION * TRAVEL_PER_ROTATION_IN_MM; float pedalInclineAngle = computePedalInclineAngle(sledPosition); + + //Serial.println(pedalInclineAngle); #endif // average execution time averaged over multiple cycles - #define PRINT_CYCLETIME #ifdef PRINT_CYCLETIME averageCycleTime += elapsedTime; cycleIdx++; if (maxCycles< cycleIdx) { cycleIdx = 0; - averageCycleTime /= (float)maxCycles; - + Serial.print("PU cycle time: "); Serial.println(averageCycleTime); - /*Serial.print("A:"); - Serial.print(loadcellReading,6); - Serial.print(",B:"); - Serial.print(Force_Current_KF,6); - Serial.print(",C:"); - Serial.println(Force_Current_KF_dt,6);*/ - - - - - averageCycleTime = 0; } #endif - - #define RECALIBRATE_POSITION - #ifdef RECALIBRATE_POSITION - // in case the stepper loses its position and therefore an endstop is triggered reset position - minEndstopNotTriggered = digitalRead(minPin); - maxEndstopNotTriggered = digitalRead(maxPin); - - if (minEndstopNotTriggered == false) - { - stepper->forceStopAndNewPosition(stepperPosMin_global); - stepper->moveTo(stepperPosMin, true); - } - if (maxEndstopNotTriggered == false) - { - stepper->forceStopAndNewPosition(stepperPosMax_global); - stepper->moveTo(stepperPosMax, true); - } - - #endif - // read ADC value adc.waitDRDY(); // wait for DRDY to go low before next register read loadcellReading = adc.readCurrentChannel(); // read as voltage according to gain and vref @@ -695,7 +747,12 @@ void loop() // compute target position Position_Next = springStiffnesssInv * (Force_Current_KF-Force_Min) + stepperPosMin ; //Calculates new position using linear function //Position_Next -= Force_Current_KF_dt * 0.045f * springStiffnesssInv; // D-gain for stability - //Position_Next += 1000; + + /*cycleIdx2 += 1; + cycleIdx2 %= JOYSTICK_MAX_VALUE; + Force_Current_KF = (float)cycleIdx2 / (float)JOYSTICK_MAX_VALUE * Force_Max;*/ + + #ifdef ABS_OSCILLATION Position_Next += stepperAbsOffset; #endif @@ -704,47 +761,30 @@ void loop() - #define SET_STEPPER - #ifdef SET_STEPPER // get current stepper position - stepperPosCurrent = stepper->getCurrentPosition(); - - /*#if defined(SUPPORT_ESP32_PULSE_COUNTER) - //if (stepperPosCurrent > (stepperPosMin + 30)) - { - if (checkPosition == 1) - { - checkPosition = 0; - } - int16_t pcnt = stepper->readPulseCounter(); - //if (stepperPosMin != pcnt) - { - Serial.print('A:'); - Serial.print(stepperPosMin); - Serial.print('B:'); - Serial.print(pcnt); - Serial.println(" "); - } - } - else - { - checkPosition = 1; - } - #endif*/ - - - + //stepperPosCurrent = stepper->getCurrentPosition(); + stepperPosCurrent = stepper->getPositionAfterCommandsCompleted(); long movement = abs( stepperPosCurrent - Position_Next); if (movement>MIN_STEPS ) { stepper->moveTo(Position_Next, false); } - #endif - joystickNormalizedToInt32 = ( Force_Current_KF - Force_Min) / (Force_Max-Force_Min) * JOYSTICK_MAX_VALUE; - joystickNormalizedToInt32 = (int32_t)constrain(joystickNormalizedToInt32, JOYSTICK_MIN_VALUE, JOYSTICK_MAX_VALUE); - SetControllerOutputValue(joystickNormalizedToInt32); + + + float den = (Force_Max-Force_Min); + if(abs(den)>0.01) + { + int32_t joystickNormalizedToInt32_local = ( Force_Current_KF - Force_Min) / den * JOYSTICK_MAX_VALUE; + if(xSemaphoreTake(semaphore_updateJoystick, 1)==pdTRUE) + { + joystickNormalizedToInt32 = (int32_t)constrain(joystickNormalizedToInt32_local, JOYSTICK_MIN_VALUE, JOYSTICK_MAX_VALUE); + xSemaphoreGive(semaphore_updateJoystick); + } + + } + //#define PRINT_DEBUG @@ -758,10 +798,144 @@ void loop() Serial.print(",Position_Next:"); Serial.print(Position_Next, 6); Serial.println(" "); - delay(100); #endif - //Serial.println("debug message!"); + } + } + + + + + + + + + + +/**********************************************************************************************/ +/* */ +/* pedal update task */ +/* */ +/**********************************************************************************************/ + + unsigned long sc_currentTime = 0; + unsigned long sc_previousTime = 0; + unsigned long sc_elapsedTime = 0; + unsigned long sc_cycleIdx = 0; + float sc_averageCycleTime = 0; + int32_t joystickNormalizedToInt32_local = 0; + + void serialCommunicationTask( void * pvParameters ) + { + + for(;;){ + + + // average cycle time averaged over multiple cycles + #ifdef PRINT_CYCLETIME + + // obtain time + sc_currentTime = micros(); + sc_elapsedTime = sc_currentTime - sc_previousTime; + if (sc_elapsedTime<1){sc_elapsedTime=1;} + sc_previousTime = sc_currentTime; + + sc_averageCycleTime += sc_elapsedTime; + sc_cycleIdx++; + if (maxCycles < sc_cycleIdx) + { + sc_cycleIdx = 0; + sc_averageCycleTime /= (float)maxCycles; + Serial.print("SC cycle time: "); + Serial.println(sc_averageCycleTime); + sc_averageCycleTime = 0; + } + #endif + + + + + + // read serial input + byte n = Serial.available(); + + /*// likely config structure + if ( n == sizeof(DAP_config_st) ) + { + DAP_config_st * dap_config_st_local; + Serial.readBytes((char*)dap_config_st_local, sizeof(DAP_config_st)); + + // check if data is plausible + bool structChecker = true; + if ( dap_config_st_local->payloadType != dap_config_st.payloadType ){ structChecker = false;} + if ( dap_config_st_local->version != dap_config_st.version ){ structChecker = false;} + + // if checks are successfull, overwrite global configuration struct + if (structChecker == true) + { + // 1. take semaphore + xSemaphoreTake(batton, portMAX_DELAY); + // 2. update global variables + dap_config_st = *dap_config_st_local; + // 3. release semaphore + xSemaphoreGive(batton); + } + } + else*/ + { + if (n!=0) + { + int menuChoice = Serial.parseInt(); + switch (menuChoice) { + // resset minimum position + case 1: + //Serial.println("Reset position!"); + set = stepperPosMin_global; + while(minEndstopNotTriggered == true){ + stepper->moveTo(set, true); + minEndstopNotTriggered = digitalRead(minPin); + set = set - ENDSTOP_MOVEMENT; + } + stepper->forceStopAndNewPosition(stepperPosMin_global); + //stepper->moveTo(0); + break; + + // toggle ABS + case 2: + //Serial.print("Second case:"); + absActive = true; + absDeltaTimeSinceLastTrigger = 0; + break; + + default: + Serial.print("Default case:"); + } + + } + } + + + + + + + // transmit joystick output + if (IsControllerReady()) + { + delay(1); + if(xSemaphoreTake(semaphore_updateJoystick, 1)==pdTRUE) + { + joystickNormalizedToInt32_local = joystickNormalizedToInt32; + xSemaphoreGive(semaphore_updateJoystick); + } + + + SetControllerOutputValue(joystickNormalizedToInt32_local); + } + + } + } + + -}