add haptics test

config.py

@@ -9,5 +9,5 @@
 # COM_PORT = "/dev/ttyUSB0"
 
 ### Flag
-uploading_firmware = False
+uploading_firmware = True
 developer = False

firmware/09 god object/firmware/.gitignore

@@ -0,0 +1,4 @@
+.vscode
+.pio
+.pioenvs
+.piolibdeps

firmware/09 god object/firmware/include/config/config.hpp

@@ -0,0 +1,103 @@
+/*
+ * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist.
+ * 
+ * config.hpp contains hardware specific data like the pantograph size and the I/O pins.
+ * It is generated by GenerateHardwareConfig.js using the hardware specifications found in the Hardware dir.
+ * 
+ * In order to avoid additional checks, unused data is rerouted to invalid pins instead of filtering all calls.
+ * For the current configuration, this dummy pin is 40. Any assignments to this pin will be ignored, all reads from this pin will return 0.
+ */
+
+#pragma once
+
+#include <Arduino.h>
+
+const uint8_t configHash[] = {0x8F, 0x37, 0xA2, 0x7A, 0x29, 0xDA, 0x7D, 0xB4, 0xBB, 0x54, 0x0F, 0x13, 0x43, 0x71, 0xBC, 0xE9};
+const float opMinDist = 65,
+            opMaxDist = 210,
+            opAngle = 2.2;
+extern float forceFactor;
+const uint8_t pantoCount = 2;
+const uint8_t dummyPin = 40;
+#define LINKAGE_ENCODER_USE_SPI
+const uint32_t numberOfSpiEncoders = 4;
+const float linkageBaseX[] = {
+    -23.7, 61.1, 0, -61.1, 23.7, 0
+};
+const float linkageBaseY[] = {
+    0, 0, 0, 0, 0, 0
+};
+const float linkageInnerLength[] = {
+    95.07, 57.71, 0, 57.71, 95.07, 0
+};
+const float linkageOuterLength[] = {
+    119.79, 119.79, 0, 119.79, 119.79, 0
+};
+const uint8_t linkageHandleMount[] = {
+    0, 0, 1, 0, 0, 0
+};
+const float motorPowerLimit[] = {
+    0.2, 0.2, 0.2, 0.2, 0.2, 0.2
+};
+const float motor_powerLimitForce[] = {
+  0.6, 0.6, 0.2, 0.6, 0.6, 0.2
+};
+extern float pidFactor[6][3];
+const float forceP = 0.375;
+const float forceI = 0;
+const float forceD = 0;
+const float forcePidFactor[2][3] = {
+  {forceP, forceI, forceD}, {forceP, forceI, forceD}
+};
+const uint8_t motorPwmPin[] = {
+    40, 40, 25, 40, 40, 23
+};
+const uint8_t motorPwmPinForwards[] = {
+    16, 2, 40, 22, 19, 40
+};
+const uint8_t motorPwmPinBackwards[] = {
+    4, 17, 40, 21, 18, 40
+};
+const uint8_t motorDirAPin[] = {
+    40, 40, 32, 40, 40, 26
+};
+const uint8_t motorDirBPin[] = {
+    40, 40, 40, 40, 40, 40
+};
+const bool motorFlipped[] = {
+    false, false, true, false, false, true
+};
+const uint8_t encoderAPin[] = {
+    40, 40, 35, 40, 40, 36
+};
+const uint8_t encoderBPin[] = {
+    40, 40, 34, 40, 40, 39
+};
+const uint8_t encoderIndexPin[] = {
+    40, 40, 40, 40, 40, 40
+};
+const uint32_t encoderSteps[] = {
+    16384, 16384, 260, 16384, 16384, 260
+};
+const uint32_t encoderSpiIndex[] = {
+    3, 2, 4294967295, 0, 1, 4294967295
+};
+const float encoderFlipped[] = {
+    1, 1, 1, -1, -1, -1
+};
+const float setupAngle[] = {
+    -0.5, 0.001, 0, -0.5, 0.001, 0
+};
+constexpr float rangeMinX = -180;
+constexpr float rangeMinY = -205;
+constexpr float rangeMaxX = 180;
+constexpr float rangeMaxY = 5;
+constexpr uint32_t hashtableMaxMemory = 100000;
+constexpr uint32_t hashtableUsedMemory = 98532;
+constexpr uint32_t hashtableMaxCells = 8333;
+constexpr uint32_t hashtableNumCells = 8211;
+constexpr uint32_t hashtableStepsX = 119;
+constexpr uint32_t hashtableStepsY = 69;
+constexpr double hashtableStepSizeX = 3.0252100840336134;
+constexpr double hashtableStepSizeY = 3.0434782608695654;
+const uint32_t obstacleChangesPerFrame = 1;

firmware/09 god object/firmware/include/hardware/angleAccessor.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include <functional>
+
+typedef std::function<uint32_t()> AngleAccessor;

firmware/09 god object/firmware/include/hardware/panto.hpp

@@ -0,0 +1,126 @@
+#pragma once
+
+#include <Encoder.h>
+
+#include "config/config.hpp"
+#include "hardware/angleAccessor.hpp"
+#include "utils/vector.hpp"
+#include <EEPROM.h>
+
+// make sure results are in range -270° ~ 0° ~ +90°
+#define ensureAngleRange(angle) \
+    angle > HALF_PI ? \
+        angle - TWO_PI : \
+    angle < -(PI + HALF_PI) ? \
+        angle + TWO_PI : \
+        angle
+
+class Panto
+{
+private:
+    static const uint16_t c_ledcFrequency = 20000;
+    static const uint16_t c_ledcResolution = 12;
+    static const uint16_t PWM_MAX = 4095; // (2^12)-1
+    static const uint8_t c_dofCount = 3;
+    const uint8_t c_localLeftIndex = 0;
+    const uint8_t c_localRightIndex = 1;
+    const uint8_t c_localHandleIndex = 2;
+
+    const uint8_t c_pantoIndex;
+    const uint8_t c_globalIndexOffset;
+    const uint8_t c_globalLeftIndex;
+    const uint8_t c_globalRightIndex;
+    const uint8_t c_globalHandleIndex;
+    const float c_leftInnerLength;
+    const float c_rightInnerLength;
+    const float c_leftOuterLength;
+    const float c_rightOuterLength;
+    const float c_leftInnerLengthDoubled;
+    const float c_rightInnerLengthDoubled;
+    const float c_leftInnerLengthSquared;
+    const float c_rightInnerLengthSquared;
+    const float c_leftOuterLengthSquared;
+    const float c_rightOuterLengthSquared;
+    const float c_leftInnerLengthSquaredMinusLeftOuterLengthSquared;
+    const float c_rightInnerLengthSquaredMinusRightOuterLengthSquared;
+    const float c_leftOuterLengthSquaredMinusRightOuterLengthSquared;
+    const bool c_handleMountedOnRightArm;
+    const float c_leftBaseX;
+    const float c_leftBaseY;
+    const float c_rightBaseX;
+    const float c_rightBaseY;
+
+    #ifdef LINKAGE_ENCODER_USE_SPI
+    AngleAccessor m_angleAccessors[2];
+    #endif
+    Encoder* m_encoder[c_dofCount];
+    float m_actuationAngle[c_dofCount];
+    float m_previousAngle[c_dofCount];
+    float m_previousAngles[c_dofCount][5];
+    float m_targetAngle[c_dofCount];
+    float m_previousDiff[c_dofCount];
+    float m_integral[c_dofCount];
+    uint32_t m_prevTime = 0;
+
+    int m_previousAnglesCount = 0;
+    int m_encoderErrorCount = 0;
+    int m_encoderErrorCounts[4] = {0,0,0,0};
+    int m_encoderRequestCount = 0;
+    int m_encoderRequestCounts[4] = {0,0,0,0};
+    float m_leftInnerAngle = 0;
+    float m_rightInnerAngle = 0;
+    float m_pointingAngle = 0;
+    float m_handleX = 0;
+    float m_handleY = 0;
+    float m_targetX = 0;
+    float m_targetY = 0;
+    float m_startX = 0;
+    float m_startY = 0;
+    float m_filteredX = 0;
+    float m_filteredY = 0;
+    float m_tweeningValue = 0.0f;
+    float m_tweeningStep = 0.00001f;
+    float m_tweeningSpeed = 1.0f;
+    uint32_t m_tweeningPrevtime = 0;
+    float m_dt = 0.0001f;
+    float delta = 0.01;
+    float velocity = 1.0f;
+    bool m_isforceRendering = false;
+    bool m_inTransition = false;
+    float m_jacobian[2][2] = {{0.0, 0.0}, {0.0, 0.0}};
+    bool m_isFrozen = false;
+    bool m_isCalibrating = false;
+
+    void inverseKinematics();
+    void setMotor(
+        const uint8_t& localIndex, const bool& dir, const float& power);
+    void disengageMotors();
+public:
+    Panto(uint8_t pantoIndex);
+    float getActuationAngle(const uint8_t index) const;
+    Vector2D getPosition() const;
+    float getRotation() const;
+    void setAngleAccessor(const uint8_t index, const AngleAccessor accessor);
+    void setTarget(const Vector2D target, const bool isForceRendering);
+    void setSpeed(const float speed);
+    void setRotation(const float rotation);
+    bool getCalibrationState();
+    void calibrateEncoders();
+    void calibratePanto();
+    void calibrationEnd();
+    void resetActuationAngle();
+    void readEncoders();
+    void forwardKinematics();
+    void actuateMotors();
+    int getEncoderErrorCount();
+    int getEncoderRequests();
+    int getEncoderErrorCounts(int i);
+    int getEncoderRequestsCounts(int i);
+    uint8_t getPantoIndex();
+    bool getInTransition();
+    void setInTransition(bool inTransition);
+    bool getIsFrozen();
+    void setIsFrozen(bool isFrozen);
+};
+
+extern std::vector<Panto> pantos;

firmware/09 god object/firmware/include/hardware/spiCommands.hpp

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <Arduino.h>
+
+namespace SPICommands
+{
+    extern const uint16_t c_readAngle;
+    extern const uint16_t c_clearError;
+    extern const uint16_t c_nop;
+    extern const uint16_t c_highZeroRead;
+    extern const uint16_t c_lowZeroRead;
+    extern const uint16_t c_highZeroWrite;
+    extern const uint16_t c_lowZeroWrite;
+};

firmware/09 god object/firmware/include/hardware/spiEncoder.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <SPI.h>
+
+#include "hardware/spiPacket.hpp"
+
+class SPIEncoder
+{
+    friend class SPIEncoderChain;
+private:
+    SPIPacket m_lastPacket;
+    uint16_t m_lastValidAngle;
+    SPIClass* m_spi;
+    SPIEncoder() = default;
+    SPIEncoder(SPIClass* spi);
+    void transfer(uint16_t transmisson);
+public:
+    uint32_t getAngle();
+};

firmware/09 god object/firmware/include/hardware/spiEncoderChain.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <SPI.h>
+#include <vector>
+
+#include "hardware/angleAccessor.hpp"
+#include "hardware/spiEncoder.hpp"
+
+class SPIEncoderChain
+{
+private:
+    SPISettings m_settings;
+    SPIClass m_spi;
+    uint32_t m_numberOfEncoders;
+    std::vector<SPIEncoder> m_encoders;
+    std::vector<uint16_t> m_values;
+    std::vector<uint16_t> m_zeros;
+    uint32_t errors = 0;
+    uint32_t requests = 0;
+    uint8_t m_maxTries = 4;
+    uint8_t m_currentTry = 0;
+    // static const uint32_t c_hspiSsPin = 15;
+
+    static const uint32_t c_hspiSsPin1 = 15;
+    static const uint32_t c_hspiSsPin2 = 5;
+    static const uint16_t c_nop = 0x0;
+    static const uint16_t c_clearError = 0x4001;
+    static const uint16_t c_readAngle = 0xFFFF;
+    static const uint16_t c_dataMask = 0x3FFF;
+
+    void begin();
+    void end();
+    std::vector<uint16_t> getZero();
+    void transfer(uint16_t transmission);
+    void setZero(std::vector<uint16_t> newZero);
+public:
+    SPIEncoderChain(uint32_t numberOfEncoders);
+    void update(int channel);
+    void update();
+    void clearError();
+    void setZero();
+    bool needsZero();
+    void wakeUp();
+    void __setZeros();
+    void setPosition(std::vector<uint16_t> positions);
+    uint32_t getErrors();
+    uint32_t getRequests();
+    void resetErrors();
+    AngleAccessor getAngleAccessor(uint32_t index);
+};

firmware/09 god object/firmware/include/hardware/spiPacket.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <Arduino.h>
+
+struct SPIPacket
+{
+    bool m_parity;
+    bool m_flag;
+    uint16_t m_data;
+    bool m_valid;
+    uint16_t m_transmission;
+    static const uint16_t c_parityMask = 0x8000;
+    static const uint16_t c_flagMask = 0x4000;
+    static const uint16_t c_dataMask = 0x3FFF;
+    SPIPacket(bool flag, uint16_t data);
+    SPIPacket(uint16_t transmisson);
+    SPIPacket() = default;
+    bool calcParity();
+    bool parityError();
+    bool commandInvalidError();
+    bool framingError();
+};