Newer
Older
13035516
committed
#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) {
13035516
committed
k_sel = ks2;
} else {
k_sel = ks0;
13035516
committed
}
}
void ImpedanceControl::setSpringZeroPos(int kc) {
// Default value is zero0
if (kc == 1) {
13035516
committed
x0 = zero1;
} else {
x0 = zero0;
13035516
committed
}
}
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;