Ikhee Jo / T-Motor_AK80_BaseCode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-dev-f303 FastPWM3

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);
    
    }