float Motor::update() {
float pos = getPosition();
// Velocity calculation should happen independent of mode
float error = fmodf_mpi_pi(pos - setpoint);
float posCtrlVal = pd.update(error);
// In position mode, update the motor command
if (mode == POSITION_MODE)
val = posCtrlVal;
// In open-loop mode, val has been set and nothing else needs to be done
// Send command, but don't modify "val" (set by user)
correctedVal = mapVal(val + barrier.calculate(pos));
// send the command
sendOpenLoop(correctedVal);
// Return the command so that the slave can do the same thing
return correctedVal;
}
void PidDlg::onSend()
{
Pid newPid;
MotorTorqueParameters newMotorTorqueParams;
switch (ui->tabMain->currentIndex()) {
case TAB_POSITION:
newPid.kp = ui->tablePosition->item(POSITION_KP,1)->text().toDouble();
newPid.kd = ui->tablePosition->item(POSITION_KD,1)->text().toDouble();
newPid.ki = ui->tablePosition->item(POSITION_KI,1)->text().toDouble();
newPid.scale = ui->tablePosition->item(POSITION_SCALE,1)->text().toDouble();
newPid.offset = ui->tablePosition->item(POSITION_OFFSET,1)->text().toDouble();
newPid.stiction_up_val = ui->tablePosition->item(POSITION_STICTIONUP,1)->text().toDouble();
newPid.max_output = ui->tablePosition->item(POSITION_MAXOUTPUT,1)->text().toDouble();
newPid.stiction_down_val = ui->tablePosition->item(POSITION_STICTIONDW,1)->text().toDouble();
newPid.max_int = ui->tablePosition->item(POSITION_MAXINT,1)->text().toDouble();
sendPositionPid(jointIndex,newPid);
break;
case TAB_VELOCITY:
newPid.kp = ui->tableVelocity->item(VELOCITY_KP, 1)->text().toDouble();
newPid.kd = ui->tableVelocity->item(VELOCITY_KD, 1)->text().toDouble();
newPid.ki = ui->tableVelocity->item(VELOCITY_KI, 1)->text().toDouble();
newPid.scale = ui->tableVelocity->item(VELOCITY_SCALE, 1)->text().toDouble();
newPid.offset = ui->tableVelocity->item(VELOCITY_OFFSET, 1)->text().toDouble();
newPid.stiction_up_val = ui->tableVelocity->item(VELOCITY_STICTIONUP, 1)->text().toDouble();
newPid.max_output = ui->tableVelocity->item(VELOCITY_MAXOUTPUT, 1)->text().toDouble();
newPid.stiction_down_val = ui->tableVelocity->item(VELOCITY_STICTIONDW, 1)->text().toDouble();
newPid.max_int = ui->tableVelocity->item(VELOCITY_MAXINT, 1)->text().toDouble();
sendVelocityPid(jointIndex, newPid);
break;
case TAB_TORQUE:
newPid.kp = ui->tableTorque->item(TORQUE_KP,1)->text().toDouble();
newPid.kff = ui->tableTorque->item(TORQUE_KFF,1)->text().toDouble();
newPid.kd = ui->tableTorque->item(TORQUE_KD,1)->text().toDouble();
newMotorTorqueParams.bemf = ui->tableTorque->item(TORQUE_BEMFGAIN,1)->text().toDouble();
newMotorTorqueParams.bemf_scale = ui->tableTorque->item(TORQUE_BEMFSCALE,1)->text().toDouble();
newMotorTorqueParams.ktau = ui->tableTorque->item(TORQUE_KTAUGAIN,1)->text().toDouble();
newMotorTorqueParams.ktau_scale = ui->tableTorque->item(TORQUE_KTAUSCALE,1)->text().toDouble();
newPid.ki = ui->tableTorque->item(TORQUE_KI,1)->text().toDouble();
newPid.scale = ui->tableTorque->item(TORQUE_SCALE,1)->text().toDouble();
newPid.offset = ui->tableTorque->item(TORQUE_OFFSET,1)->text().toDouble();
newPid.stiction_up_val = ui->tableTorque->item(TORQUE_STITCTIONUP,1)->text().toDouble();
newPid.max_output = ui->tableTorque->item(TORQUE_MAXOUTPUT,1)->text().toDouble();
newPid.stiction_down_val = ui->tableTorque->item(TORQUE_STICTIONDW,1)->text().toDouble();
newPid.max_int = ui->tableTorque->item(TORQUE_MAXINT,1)->text().toDouble();
sendTorquePid(jointIndex,newPid,newMotorTorqueParams);
break;
case TAB_STIFF:{
double desiredStiff = ui->tableStiffness->item(0,3)->text().toDouble();
double desiredDamp = ui->tableStiffness->item(1,3)->text().toDouble();
double desiredForce = ui->tableStiffness->item(2,3)->text().toDouble();
sendStiffness(jointIndex,desiredStiff,desiredDamp,desiredForce);
break;
}
case TAB_OPENLOOP:{
int desiredOpenLoop = ui->tableOpenloop->item(0,1)->text().toDouble();
sendOpenLoop(jointIndex,desiredOpenLoop);
break;
}
case TAB_CURRENT:{
newPid.kp = ui->tableCurrent->item(CURRENT_KP, 1)->text().toDouble();
newPid.kd = ui->tableCurrent->item(CURRENT_KD, 1)->text().toDouble();
newPid.ki = ui->tableCurrent->item(CURRENT_KI, 1)->text().toDouble();
newPid.scale = ui->tableCurrent->item(CURRENT_SCALE, 1)->text().toDouble();
newPid.offset = ui->tableCurrent->item(CURRENT_OFFSET, 1)->text().toDouble();
newPid.max_output = ui->tableCurrent->item(CURRENT_MAXOUTPUT, 1)->text().toDouble();
newPid.max_int = ui->tableCurrent->item(CURRENT_MAXINT, 1)->text().toDouble();
sendCurrentPid(jointIndex, newPid);
break;
}
case TAB_VARIABLES:{
// Remote Variables
int rows = ui->tableVariables->rowCount();
for (int i = 0; i < rows; i++)
{
std::string key = ui->tableVariables->item(i, 0)->text().toStdString();
std::string val = ui->tableVariables->item(i, 1)->text().toStdString();
yarp::os::Bottle valb(val);
sendSingleRemoteVariable(key, valb);
}
updateAllRemoteVariables();
break;
}
default:
break;
}
}