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;

    x = x_;
    delta_x = x - x0;
    v = (x - prev_x)/dt; // delta_x in rads and dt in sec
    a = (v - prev_v)/dt;
    torque = delta_x*k_sel + dampener*v + mass*a;
    
    /* 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;
}