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;
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
13035516
committed
torque = dampener*v + delta_x*k_sel;
} else {
v = 0;
torque = 0;
}
prev_x = x;