Ben Katz
FOC Implementation for putting multirotor motors in robots
ImpedanceController/ImpedanceController.cpp@14:80ce59119d93, 2016-10-31 (annotated)
- Committer:
- benkatz
- Date:
- Mon Oct 31 16:48:16 2016 +0000
- Revision:
- 14:80ce59119d93
- Parent:
- 11:c83b18d41e54
Misc. changes. Finally fixed transforms (turns out B and C current measurements were accidentally swapped)
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| benkatz | 9:d7eb815cb057 | 1 | #include "TorqueController.h" |
| benkatz | 3:6a0015d88d06 | 2 | #include "ImpedanceController.h" |
| benkatz | 9:d7eb815cb057 | 3 | #include "CurrentRegulator.h" |
| benkatz | 9:d7eb815cb057 | 4 | #include "PositionSensor.h" |
| benkatz | 3:6a0015d88d06 | 5 | |
| benkatz | 9:d7eb815cb057 | 6 | |
| benkatz | 9:d7eb815cb057 | 7 | ImpedanceController::ImpedanceController(TorqueController *torqueController, PositionSensor *sensor_pos, PositionSensor *sensor_vel){ |
| benkatz | 9:d7eb815cb057 | 8 | _torqueController = torqueController; |
| benkatz | 9:d7eb815cb057 | 9 | _sensor_pos = sensor_pos; |
| benkatz | 9:d7eb815cb057 | 10 | _sensor_vel = sensor_vel; |
| benkatz | 9:d7eb815cb057 | 11 | } |
| benkatz | 9:d7eb815cb057 | 12 | |
| benkatz | 9:d7eb815cb057 | 13 | void ImpedanceController::SetImpedance(float K, float B, float ref){ |
| benkatz | 9:d7eb815cb057 | 14 | float position = _sensor_pos->GetMechPosition(); |
| benkatz | 9:d7eb815cb057 | 15 | float velocity = _sensor_vel->GetMechVelocity(); |
| benkatz | 9:d7eb815cb057 | 16 | float error = ref-position; |
| benkatz | 9:d7eb815cb057 | 17 | float output = K*error + B*velocity; |
| benkatz | 9:d7eb815cb057 | 18 | |
| benkatz | 9:d7eb815cb057 | 19 | _torqueController->SetTorque(output); |
| benkatz | 9:d7eb815cb057 | 20 | |
| benkatz | 9:d7eb815cb057 | 21 | } |
| benkatz | 9:d7eb815cb057 | 22 |