dasad

Author: 3dPaulHarvey

.vscode/settings.json

@@ -0,0 +1,56 @@
+{
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "chrono": "cpp",
+        "compare": "cpp",
+        "concepts": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "cinttypes": "cpp",
+        "typeinfo": "cpp"
+    }
+}

PositionManager.h

@@ -16,8 +16,9 @@ class PositionManager {
       stepsToAccelerate(stepsToAccelerate), decelerationSteps(decelerationSteps), req(req) {}
 
     // Motor control functions
+    inline void holdPosition(float position);
     inline void holdPositionDuration(float position, float duration);
-    inline void homing(float& commandedPosition, float& currentPosition);
+    inline bool homing(float& commandedPosition, float& currentPosition);
 
     inline void performAcceleration(float& commandedPosition, float& currentPosition);
     inline void performCruising(float& commandedPosition, float& currentPosition);
@@ -53,13 +54,13 @@ void PositionManager::performAcceleration(float& commandedPosition, float& curre
 
     float currentVelocity = 0.0f;
     float accelerationPerStep = -maxSpeed / stepsToAccelerate; // Negative for reverse direction
-    std::cout << "Reverse Acceleration Per Step: " << accelerationPerStep << std::endl;
+    std::cout << "Acceleration Per Step: " << accelerationPerStep << std::endl;
 
     for (int i = 1; i <= stepsToAccelerate; i++) {
         currentVelocity += accelerationPerStep;
         float newPosition = accelPositions.back() + currentVelocity;
         accelPositions.push_back(newPosition);
-        std::cout << "Current Reverse Velocity: " << currentVelocity << "\tNew Reverse Position: " << newPosition << std::endl;
+        std::cout << "CurrentVelocity: " << currentVelocity << "\tNewPosition: " << newPosition << std::endl;
     }
 
     for (size_t i = 0; i < accelPositions.size(); i++) {
@@ -71,7 +72,7 @@ void PositionManager::performAcceleration(float& commandedPosition, float& curre
             currentPosition = controller_state[0];
             torques.push_back(controller_state[2]);
         }
-        std::cout << "Reverse Step: " << i << ", Target: " << accelPositions[i]
+        std::cout << "Step: " << i << ", Target: " << accelPositions[i]
                   << ", Actual: " << currentPosition << "\tVelocity" << controller_state[1] << std::endl;
     }
 }
@@ -88,7 +89,7 @@ void PositionManager::performCruising(float& commandedPosition, float& currentPo
             currentPosition = controller_state[0];
             torques.push_back(controller_state[2]);
         }
-        std::cout << "Reverse Cruising at position: " << currentPosition << "\tVelocity" << controller_state[1] << "\tTorque" << controller_state[2] << std::endl;
+        std::cout << "Cruising at position: " << currentPosition << "\tVelocity" << controller_state[1] << "\tTorque" << controller_state[2] << std::endl;
     }
 }
 
@@ -108,10 +109,10 @@ void PositionManager::performDeceleration(float& commandedPosition, float& curre
             currentPosition = controller_state[0];
             torques.push_back(controller_state[2]);
         }
-        std::cout << "Reverse Decelerating, step " << step + 1 << ": Position = " << controller_state[0]
+        std::cout << "Decelerating, step " << step + 1 << ": Position = " << controller_state[0]
                   << ": Velocity = " << controller_state[1] << std::endl;
         if (currentVelocity >= 0) { // Check for stopping condition
-            std::cout << "Reverse Deceleration complete at step " << step + 1 << std::endl;
+            std::cout << "Deceleration complete at step " << step + 1 << std::endl;
             break;
         }
     }
@@ -207,8 +208,21 @@ void PositionManager::holdPositionDuration(float position, float duration) {
     }
 }
 
+// HOLD POSITION 
+void PositionManager::holdPosition(float position) {
+
+    struct timespec req = {0, 1200 * 1000};
+    controller.sendWriteCommand(position, 0.0);
+    nanosleep(&req, NULL);
+    auto controller_state = controller.sendReadCommand();
+    if (controller_state[0] >= 450 && controller_state[0] <= 550) {
+        torques.push_back(controller_state[2]);
+    }
+    //std::cout << "Position: " << controller_state[0] << " Velocity: " << controller_state[1] << " Torque: " << controller_state[2] << std::endl;
+}
+
 // HOMING
-void PositionManager::homing(float& commandedPosition, float& currentPosition) {    
+bool PositionManager::homing(float& commandedPosition, float& currentPosition) {    
     std::cout << "Starting Homing..." << std::endl;
     controller.sendRezeroCommand(500.0); // sets the current position to 500.0
     int index = 0;
@@ -233,7 +247,7 @@ void PositionManager::homing(float& commandedPosition, float& currentPosition) {
                         auto controller_state = controller.sendReadCommand();
                         torques.push_back(controller_state[2]);
                     }
-                    break;
+                    return false;
                 }
             }
             // Check if the button is pressed
@@ -248,7 +262,7 @@ void PositionManager::homing(float& commandedPosition, float& currentPosition) {
     commandedPosition = 500.0f;
     currentPosition = 500.0f;
     std::cout << "HOMED" << std::endl; 
-    return; 
+    return true; 
 } // End HOMING
 
 #endif // POSITION_MANAGER_H

VelocityCurveGenerator.h

@@ -0,0 +1,90 @@
+#include "MyController.h"
+#include "GraphPlotter.h"
+#include "MyGpio.h"
+#include "PositionManager.h"
+
+
+
+int main() {
+    // GPIO SETUP
+    MyGpio homeLimitSwitch("gpiochip0", 24);  
+    if (!homeLimitSwitch.init()) {
+        std::cerr << "Failed to initialize hom button" << std::endl;
+        return 1;
+    }
+    MyGpio extendLimitSwitch("gpiochip0", 27); 
+    if (!extendLimitSwitch.init()) {
+        std::cerr << "Failed to initialize extend button" << std::endl;
+        return 1;
+    }
+    MyGpio activateSwitch("gpiochip0", 17); 
+    if (!activateSwitch.init()) {
+        std::cerr << "Failed to initialize extend button" << std::endl;
+        return 1;
+    }
+    MyGpio safetySwitch("gpiochip0", 26); 
+    if (!activateSwitch.init()) {
+        std::cerr << "Failed to initialize extend button" << std::endl;
+        return 1;
+    }
+
+    // CONTROLLER SETUP
+    MyController controller("/dev/fdcanusb");
+    if (!controller.setupSerialPort()) {
+        std::cerr << "Failed to setup serial port" << std::endl;
+        return 1;
+    }
+    controller.sendStopCommand();  //gets controller to a known state
+    controller.sendRezeroCommand(500.0f); // sets the current position to 500.0
+
+
+    // Initialization
+    std::vector<float> torques;
+    const float startPosition = 500.0f;
+    const float cruisingEndPosition = 480.0f; // + 2.2f;
+    const float cruisingReverseEndPosition = 520.0f; // - 1.10f;
+    const float stepsToAccelerate = 50.0f;  
+    const float decelerationSteps = 20.0f; 
+    const float maxSpeed = 0.055f;       // Max speed in units per control loop iteration
+    struct timespec req = {0, 1200 * 1000}; // Control loop frequency
+    float commandedPosition = startPosition;
+    float currentPosition = startPosition;
+    PositionManager positionManager (controller, homeLimitSwitch, extendLimitSwitch, maxSpeed, cruisingEndPosition, cruisingReverseEndPosition, stepsToAccelerate, decelerationSteps, req);
+
+    // Homing
+    positionManager.holdPositionDuration(commandedPosition, 0.5);
+    positionManager.homing(commandedPosition, currentPosition);
+    positionManager.holdPositionDuration(commandedPosition, 0.5);
+
+    // Perform Forward motion
+    controller.sendRezeroCommand(500.0f); // sets the current position to 500.0
+    commandedPosition = 500.0f;
+    currentPosition = 500.0f;
+    positionManager.performAcceleration(commandedPosition, currentPosition);
+    positionManager.performCruising(commandedPosition, currentPosition);
+    positionManager.performDeceleration(commandedPosition, currentPosition);
+    positionManager.holdPositionDuration(commandedPosition, 0.5);
+
+    // Perform Reverse motion
+    controller.sendRezeroCommand(500.0f); // sets the current position to 500.0
+    commandedPosition = 500.0f;
+    currentPosition = 500.0f;
+    positionManager.performAccelerationReverse(commandedPosition, currentPosition);
+    positionManager.performCruisingReverse(commandedPosition, currentPosition);
+    positionManager.performDecelerationReverse(commandedPosition, currentPosition);
+    positionManager.holdPositionDuration(commandedPosition, 0.5);
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+
+    // Stop the motor
+    //controller.sendStopCommand();
+    controller.closeSerialPort();
+
+    // Graph the torque values collected
+    //GraphPlotter plotter;
+    //plotter.plot(positionManager.getTorques(), "Torque Readings Through Various Phases");
+
+    return 0;
+}
+ 

bk4_position_control_test.cpp

@@ -23,6 +23,13 @@ int main() {
         return 1;
     }
 
+    MyGpio button4("gpiochip0", 21);  //SAFETY BUTTON
+
+    if (!button4.init()) {
+        std::cerr << "Failed to initialize GPIO button" << std::endl;
+        return 1;
+    }
+
 
     while (true) {
         //button1
@@ -67,6 +74,20 @@ int main() {
             std::cout << "Button not pressed." << std::endl;
         }
 
+        //button4
+        int value4 = button4.readValue();
+
+        if (value4 == -1) {
+            std::cerr << "Error reading GPIO value" << std::endl;
+            break;
+        }
+
+        if (value4 == 0) {
+            std::cout << "Button pressed44444444444444444444444444444444444444444444444!" << std::endl;
+        } else {
+            std::cout << "Button not pressed." << std::endl;
+        }
+
        // sleep(1); // Check the button state every second
     }