Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

ImpedanceController/ImpedanceController.cpp

Committer:
benkatz
Date:
2016-10-31
Revision:
14:80ce59119d93
Parent:
11:c83b18d41e54

File content as of revision 14:80ce59119d93:

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