position and vibration fixes

Author: mnh86

.gitignore

@@ -4,4 +4,5 @@
 .vscode/launch.json
 .vscode/settings.json
 .vscode/ipch
+*.log
 NOTES.md

CHANGELOG.md

@@ -1,7 +1,11 @@
 # Change log
 
-## v0.3
-- Reduced the maximum setting for the vibration amplitude from 25 to 20 (position units).
+## v0.3 - 02/06/2023
+- Fix: Divide by zero crash during vibration calculations.
+- `L0` Up Position: mapped range was changed from (-1000 -> 1000) to (-750 -> 750). Based on analysis of Pendant behavior which does not send values higher than 750. Testing values at 1000 causes slamming of the actuator piston to occur against its enclosure.
+- `V0` Vibration Amplitude: internal range was changed from (0 -> 20) to (0 -> 25) position units. Its default is now 25, which matches more closely with the NimbleStroker Pendant behavior.
+- `V0` Vibration Amplitude: changes will now ease in and out rather than a linear change.
+- `A2` Vibration Speed: default was changed from 10hz to 20hz.
 
 ## v0.2 - 02/05/2023
 - Added new aux axis for controlling vibration speed via tcode

README.md

@@ -8,23 +8,23 @@
 - `D1` - Identify TCode version: `TCode v0.4`
 - `D2` - List available axes and user range preferences:
   - `L0 0 9999 Up`: **Up/down position** linear motion (default: `5000`)
-    - Maps to NimbleStroker positions: -1000 to 1000
+    - Maps to NimbleStroker positions: -750 to 750
   - `V0 0 9999 Vibe`: **Vibration intensity** (default: `0`)
     - An oscillation is applied to the position when sent to the NimbleStroker.
-    - Maps to the amplitude of oscillation: 0 to 20 (position units)
+    - Maps to the amplitude of oscillation: 0 to 25 (position units)
   - `A0 0 9999 Air`: **Auxilliary air in/out valve** (default `5000`)
     - Value `0000` = air-out valve (looser)
     - Value `5000` = stop valve (default)
     - Value `9999` = air-in valve (tighter)
   - `A1 0 9999 Force`: **Force command** (default `9999`)
     - Maps to NimbleStroker force command values: 0 to 1023
     - Controls the air pressure force of the actuator(?)
-  - `A2 0 9999 VibSpeed`: **Vibration speed** (default: `5000`)
-    - Maps to an oscillation speed for vibration: 0 to 20.0hz (default 10.0hz)
+  - `A2 0 9999 VibSpeed`: **Vibration speed** (default: `9999`)
+    - Maps to an oscillation speed for vibration: 0 to 20hz (default 20hz)
 
 Other info:
 
-- Sending live control values to an axis will ease to target over multiple frames rather than jump immediately, if the difference in change is large (> 100 t-code units, or >50 position units). This is intended to protect the user and device. ([Source1](https://github.com/mnh86/NimbleTCodeSerial/blob/6ab66638b2670115e770fdee9d2ec5c7b04f9390/include/TCodeAxis.h#L217-L228), [Source2](https://github.com/mnh86/NimbleTCodeSerial/blob/6ab66638b2670115e770fdee9d2ec5c7b04f9390/src/main.cpp#L104-L111))
+- Sending live control values to an axis will ease to target over multiple frames rather than jump immediately when the difference in change is large (> 100 t-code units, or >50 position units). This is intended to protect the user and device. ([Source1](https://github.com/mnh86/NimbleTCodeSerial/blob/6ab66638b2670115e770fdee9d2ec5c7b04f9390/include/TCodeAxis.h#L217-L228), [Source2](https://github.com/mnh86/NimbleTCodeSerial/blob/6ab66638b2670115e770fdee9d2ec5c7b04f9390/src/main.cpp#L104-L111))
 
 ## Usage
 

include/nimbleConModule.h

@@ -104,7 +104,7 @@ struct Pendant
 struct Pendant pendant; // Declare pendant
 
 // Actuator Variables
-#define ACTUATOR_MAX_POS 1000
+#define ACTUATOR_MAX_POS 750
 
 struct Actuator
 {

platformio.ini

@@ -19,6 +19,7 @@ monitor_eol = LF
 monitor_filters =
 	default
 	colorize
+	time
 lib_deps =
 	madhephaestus/ESP32Encoder@^0.10.1
 	mickey9801/ButtonFever@^1.0

src/main.cpp

@@ -18,11 +18,11 @@ int16_t targetPos = 0; // position controlled via TCode
 int16_t lastFramePos = 0;  // positon that was sent to the actuator last loop frame
 int16_t frameForce = IDLE_FORCE; // next force to send to the actuator (0 to 1023)
 
-#define VIBRATION_MAX_AMP 20
+#define VIBRATION_MAX_AMP 25
 #define VIBRATION_MAX_SPEED 20.0 // hz
 
-float vibrationSpeed = 10.0; // hz
-uint16_t vibrationAmplitude = 0; // in position units (0 to 10)
+float vibrationSpeed = VIBRATION_MAX_SPEED; // hz
+uint16_t vibrationAmplitude = 0; // in position units (0 to 25)
 int16_t vibrationPos = 0;
 
 #define RUN_MODE_OFF 0
@@ -76,17 +76,22 @@ void setup()
 
     // TCode setup
     tcode.init();
-    tcode.axisRegister("L0", F("Up"));
-    tcode.axisRegister("V0", F("Vibe"));
-    tcode.axisRegister("A0", F("Air"));
+    tcode.axisRegister("L0", F("Up")); // Up stroke position
+    tcode.axisWrite("L0", 5000); // 5000: midpoint
+    tcode.axisEasingType("L0", EasingType::EASEINOUT);
+
+    tcode.axisRegister("V0", F("Vibe")); // Vibration Amplitude
+    tcode.axisWrite("V0", 0);    // 0: vibration off
+    tcode.axisEasingType("V0", EasingType::EASEINOUT);
+
+    tcode.axisRegister("A0", F("Air")); // Air in/out valve
+    tcode.axisWrite("A0", 5000); // 0: air out, 5000: stop, 9999: air in
+
     tcode.axisRegister("A1", F("Force"));
+    tcode.axisWrite("A1", 9999); // 9999: max force
+
     tcode.axisRegister("A2", F("VibeSpeed"));
-    tcode.axisEasingType("L0", EasingType::EASEINOUT);
-    tcode.axisWrite("L0", 5000);
-    tcode.axisWrite("V0", 0);
-    tcode.axisWrite("A0", 5000);
-    tcode.axisWrite("A1", 9999);
-    tcode.axisWrite("A2", 5000);
+    tcode.axisWrite("A2", 9999); // 9999: max vibration speed
 
     // Button interface
     btn.onDoublePress(pressHandler)
@@ -120,10 +125,10 @@ void logTimer()
 
 void ledPulse(short int pos, short int vibPos, bool isOn = true)
 {
-    byte ledScale = map(abs(pos), 1, ACTUATOR_MAX_POS, 1, LED_MAX_DUTY);
+    byte ledScale = map(abs(pos), 0, ACTUATOR_MAX_POS, 1, LED_MAX_DUTY);
     byte ledState1 = 0;
     byte ledState2 = 0;
-    byte vibScale = map(abs(vibPos), 1, VIBRATION_MAX_AMP, 1, LED_MAX_DUTY);
+    byte vibScale = map(abs(vibPos), 0, VIBRATION_MAX_AMP, 1, LED_MAX_DUTY);
     byte vibState1 = 0;
     byte vibState2 = 0;
 
@@ -165,7 +170,7 @@ void updateLEDs()
 }
 
 /**
- * Up position is mixed with a vibration oscillator wave
+ * L0 Up position is mixed with a V0/A2 vibration oscillator
  */
 void positionHandler()
 {
@@ -179,11 +184,20 @@ void positionHandler()
         vibrationAmplitude = map(val, 0, 9999, 0, VIBRATION_MAX_AMP);
     }
 
-    int vibSpeedMillis = 1000 / vibrationSpeed;
-    int vibModMillis = millis() % vibSpeedMillis;
-    float tempPos = float(vibModMillis) / vibSpeedMillis;
-    int vibWaveDeg = tempPos * 360;
-    vibrationPos = sin(radians(vibWaveDeg)) * vibrationAmplitude;
+    if (vibrationAmplitude > 0 && vibrationSpeed > 0) {
+        int vibSpeedMillis = 1000 / vibrationSpeed;
+        int vibModMillis = millis() % vibSpeedMillis;
+        float tempPos = float(vibModMillis) / vibSpeedMillis;
+        int vibWaveDeg = tempPos * 360;
+        vibrationPos = round(sin(radians(vibWaveDeg)) * vibrationAmplitude);
+    } else {
+        vibrationPos = 0;
+    }
+    // Serial.printf("A:%5d S:%0.2f P:%5d\n",
+    //     vibrationAmplitude,
+    //     vibrationSpeed,
+    //     vibrationPos
+    // );
 
     int targetPosTmp = targetPos;
     if (targetPos - vibrationAmplitude < -ACTUATOR_MAX_POS) {
@@ -233,6 +247,18 @@ void loop()
     airHandler();
     forceHandler();
 
+    // if (readFromPend())
+    // { // Read values from pendant. If the function returns true, the values were updated so update the pass-through values.
+    //     // Pass through data from pendant to actuator
+    //     // actuator.positionCommand = pendant.positionCommand; // We control this
+    //     // actuator.forceCommand = pendant.forceCommand;       // We control this
+
+    //     Serial.printf("P P:%4d F:%4d\n",
+    //         pendant.positionCommand,
+    //         pendant.forceCommand
+    //     );
+    // }
+
     // Send packet of values to the actuator when time is ready
     if (checkTimer())
     {
@@ -252,6 +278,12 @@ void loop()
         // if (actuator.tempLimiting) {
         //     setRunMode(RUN_MODE_OFF);
         // }
+
+        // Serial.printf("A P:%4d F:%4d T:%s\n",
+        //     actuator.positionFeedback,
+        //     actuator.forceFeedback,
+        //     actuator.tempLimiting ? "true" : "false"
+        // );
     }
 
     updateLEDs();