ImpedanceControl.cpp

#include "ImpedanceControl.h"
#include <Arduino.h>

ImpedanceControl::ImpedanceControl() {
    k_sel = ks1;
    x = 0;
    x0 = 0;
    prev_x = 0;
    current = 0;
    torque = 0;
}

ImpedanceControl::~ImpedanceControl() { }

void ImpedanceControl::setSpringConst(bool kc) {
    if (kc) {
        k_sel = ks2;
    } else {
        k_sel = ks1;
    }
}

void ImpedanceControl::setSpringZeroPos(bool kc) {
    if (kc) {
        x0 = zero2;
    } else {
        x0 = zero1;
    }
}

float ImpedanceControl::getSpringConst() {
    return k_sel;
}

float ImpedanceControl::getSpringZeroPos() {
    return x0;
}

void ImpedanceControl::impedanceControllerUpdate(float x_, float dt) {
    float torque = 0;
    float delta_x = 0;
    float v = 0;

    x = x_;
    delta_x = (x - prev_x) - x0;
    if (delta_x >= 0) {
        v = delta_x/dt; // delta_x in rads and dt in sec
        torque = dampener*v + delta_x*k_sel;
        current = torque/(kt*belt_ratio);
    } else {
        v = 0;
        torque = 0;
        current = 0;
    }
}

float ImpedanceControl::getDesiredCurrent() {
    return current*1000;
}

float ImpedanceControl::getDesiredTorque() {
    return torque;
}