Beispiel #1
0
void Controller::sendSettings(){
#ifdef SUPPORT_MQTT
  if ( isSwitch() ){
    MQTT::publishSwitch( getName(), false );
  } else {
    MQTT::publishTempSetup( getName(), getTargetT(), getMinimumLevel(), getProfileID() );
  }
#endif
}
bool KinematicsSolver::solve(ColumnVector targetPose, ColumnVector currentQ) {
	this->currentQ4 = currentQ(4);

	selector = new SolutionSelector(currentQ);
	solutions.clear();

	//TODO: combine following next 3 lines into a single function
	//getTMatrix(ColumnVector jointAngles);

	targetPose(3) = targetPose(3) - DH_d1;
	R = getR(targetPose(4), targetPose(5), targetPose(6));

	target = getTargetT(targetPose(1), targetPose(2), targetPose(3), R);

	pos = getP_wr(targetPose(1), targetPose(2), targetPose(3), R);

	solveThetas();
	return solutions.size() > 0;
}