ImpedanceControl.cpp

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

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

ImpedanceControl::~ImpedanceControl() { }

void ImpedanceControl::setSpringConst(int kc) {
    // Default value is ks0
    if (kc == 1) {
        k_sel = ks1;
    } else if (kc == 2) {
        k_sel = ks2;
    } else {
        k_sel = ks0;
    }
}

void ImpedanceControl::setSpringZeroPos(int kc) {
    // Default value is zero0
    if (kc == 1) {
        x0 = zero1;
    } else {
        x0 = zero0;
    }
}

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

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

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

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

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