Ben Katz
/
Hobbyking_Cheetah_V1
FOC Implementation for putting multirotor motors in robots
ImpedanceController/ImpedanceController.cpp
- Committer:
- benkatz
- Date:
- 2016-05-22
- Revision:
- 11:c83b18d41e54
- Parent:
- 9:d7eb815cb057
File content as of revision 11:c83b18d41e54:
#include "TorqueController.h" #include "ImpedanceController.h" #include "CurrentRegulator.h" #include "PositionSensor.h" ImpedanceController::ImpedanceController(TorqueController *torqueController, PositionSensor *sensor_pos, PositionSensor *sensor_vel){ _torqueController = torqueController; _sensor_pos = sensor_pos; _sensor_vel = sensor_vel; } void ImpedanceController::SetImpedance(float K, float B, float ref){ float position = _sensor_pos->GetMechPosition(); float velocity = _sensor_vel->GetMechVelocity(); float error = ref-position; float output = K*error + B*velocity; _torqueController->SetTorque(output); }