float pi_controller(float in, pi_controller_t *pi)
{
float erro;
float dynamic_lim;
float temp;
erro = rate_limiter(pi->ref, &i_ref) - in;
//erro = pi->ref - in;
temp = erro * pi->kp;
saturation(temp, pi->max);
pi->up = temp;
if(temp < 0.0)
temp = -temp;
dynamic_lim = (pi->max - temp);
temp = pi->ui + erro * pi->ki;
saturation(temp, dynamic_lim);
pi->ui = temp;
temp = pi->ui + pi->up;
//pi->u = rate_limiter(temp, &out);
pi->u = temp;
return pi->u;
}
// Set the base velocity command
void GuardianControllerClass::setCommand(const geometry_msgs::Twist &cmd_vel)
{
// left_right
// 0: Moving forward/backward
// -1: Turning left
// +1: Turning right
if(cmd_vel.linear.x!=0) {
v_ref_ = saturation(cmd_vel.linear.x, -10.0, 10.0);
left_right=0;
} else if(cmd_vel.linear.y<0) {
v_ref_ = -1 * saturation(cmd_vel.linear.y, -10.0, 10.0);
left_right=1;
} else if(cmd_vel.linear.y>0) {
v_ref_ = saturation(cmd_vel.linear.y, -10.0, 10.0);
left_right=-1;
} else {
v_ref_= 0;
left_right=-2; // Any other key will be interpretated as a stop order
}
w_ref_ = 2.0 * saturation(cmd_vel.angular.z, -1.0, 1.0); // -10.0 10.0
}
void CHeli::setAngles(float ipitch, float iroll,float iyaw,float iheight)
{
int32_t _yaw,_pitch,_roll,_height;
_yaw = saturation(iyaw, 33000);
_roll = saturation(iroll, 33000);
_pitch = saturation(ipitch, 33000);
_height = saturation(iheight,33000);
// fprintf(stdout,"Angle request: %d %d %d %d ",pitch,roll,height,yaw);
at_set_radiogp_input(_roll, _pitch, _height, _yaw);
}
bool ArdroneInterface::velCom (geometry_msgs::Twist & vel_msg)
{
if (hovering_)
{
saturation (vel_msg.linear.x , max_vel_ , min_vel_);
saturation (vel_msg.linear.y , max_vel_ , min_vel_);
saturation (vel_msg.linear.z , max_vel_ , min_vel_);
saturation (vel_msg.angular.z , max_vel_ , min_vel_);
vel_pub_.publish(vel_msg);
return 1;
}
else
return 0;
}
void Haptuator::update(double t) {
vector<double> v = {0.0,0.0};
// Update state for reference model
_ref_model.setInput(_acc_set);
_solver.do_step(_ref_model,_acc_ref,t,_deltaT);
// Update control parameters base on actual error & adaptation law
_err = -_acc_ref[0] + _acc_act[0];
_input_filter.setInput(_ctrl_signal);
_solver.do_step(_input_filter,v,t,_deltaT);
_omega[1] = v[0];
_output_filter.setInput(_acc_act);
_solver.do_step(_input_filter,v,t,_deltaT);
_omega[2] = v[0];
_omega[0] = _acc_set[0];
_omega[3] = _acc_act[0];
// Adaptation law
adaptationLaw();
// Adaptation controller
_ctrl_signal[0] = _theta[0] * _omega[0] + _theta[1] * _omega[1] + _theta[2] * _omega[2] +_theta[3] * _omega[3];
_ctrl = true;
saturation();
}
colour hsv_colour::to_rgb() const
{
double c = value() * saturation();
double h2 = hue() / 60.0;
double x = c * (1.0 - std::abs(std::fmod(h2, 2.0) - 1.0));
double r, g, b;
if (h2 >= 0.0 && h2 < 1.0)
r = c, g = x, b = 0.0;
else if (h2 >= 1.0 && h2 < 2.0)
r = x, g = c, b = 0.0;
else if (h2 >= 2.0 && h2 < 3.0)
r = 0.0, g = c, b = x;
else if (h2 >= 3.0 && h2 < 4.0)
r = 0.0, g = x, b = c;
else if (h2 >= 4.0 && h2 < 5.0)
r = x, g = 0.0, b = c;
else if (h2 >= 5.0 && h2 < 6.0)
r = c, g = 0.0, b = x;
else
r = g = b = 0.0;
double m = value() - c;
colour result(
static_cast<colour::component>(std::floor((r + m) * 255.0)),
static_cast<colour::component>(std::floor((g + m) * 255.0)),
static_cast<colour::component>(std::floor((b + m) * 255.0)),
static_cast<colour::component>(std::floor(alpha() * 255.0)));
return result;
}
colour hsl_colour::to_rgb(double aAlpha) const
{
double c = (1.0 - std::abs(2.0 * lightness() - 1.0)) * saturation();
double h2 = hue() / 60.0;
double x = c * (1.0 - std::abs(std::fmod(h2, 2.0) - 1.0));
double r, g, b;
if (hue() == undefined_hue())
r = g = b = 0.0;
else if (h2 >= 0.0 && h2 < 1.0)
r = c, g = x, b = 0.0;
else if (h2 >= 1.0 && h2 < 2.0)
r = x, g = c, b = 0.0;
else if (h2 >= 2.0 && h2 < 3.0)
r = 0.0, g = c, b = x;
else if (h2 >= 3.0 && h2 < 4.0)
r = 0.0, g = x, b = c;
else if (h2 >= 4.0 && h2 < 5.0)
r = x, g = 0.0, b = c;
else if (h2 >= 5.0 && h2 < 6.0)
r = c, g = 0.0, b = x;
else
r = g = b = 0.0;
double m = lightness() - 0.5f * c;
colour result(
static_cast<colour::component>(std::floor((r + m) * 255.0)),
static_cast<colour::component>(std::floor((g + m) * 255.0)),
static_cast<colour::component>(std::floor((b + m) * 255.0)),
static_cast<colour::component>(std::floor(aAlpha * 255.0)));
return result;
}
/*!
\ingroup group_imgproc_histogram
Adjust the contrast of a color image by performing an histogram equalization.
The intensity distribution is redistributed over the full [0 - 255] range such as the cumulative histogram
distribution becomes linear. The alpha channel is ignored / copied from the source alpha channel.
\param I : The color image to apply histogram equalization.
\param useHSV : If true, the histogram equalization is performed on the value channel (in HSV space), otherwise
the histogram equalization is performed independently on the RGB channels.
*/
void vp::equalizeHistogram(vpImage<vpRGBa> &I, const bool useHSV) {
if(I.getWidth()*I.getHeight() == 0) {
return;
}
if(!useHSV) {
//Split the RGBa image into 4 images
vpImage<unsigned char> pR(I.getHeight(), I.getWidth());
vpImage<unsigned char> pG(I.getHeight(), I.getWidth());
vpImage<unsigned char> pB(I.getHeight(), I.getWidth());
vpImage<unsigned char> pa(I.getHeight(), I.getWidth());
vpImageConvert::split(I, &pR, &pG, &pB, &pa);
//Apply histogram equalization for each channel
vp::equalizeHistogram(pR);
vp::equalizeHistogram(pG);
vp::equalizeHistogram(pB);
//Merge the result in I
unsigned int size = I.getWidth()*I.getHeight();
unsigned char *ptrStart = (unsigned char*) I.bitmap;
unsigned char *ptrEnd = ptrStart + size*4;
unsigned char *ptrCurrent = ptrStart;
unsigned int cpt = 0;
while(ptrCurrent != ptrEnd) {
*ptrCurrent = pR.bitmap[cpt];
++ptrCurrent;
*ptrCurrent = pG.bitmap[cpt];
++ptrCurrent;
*ptrCurrent = pB.bitmap[cpt];
++ptrCurrent;
*ptrCurrent = pa.bitmap[cpt];
++ptrCurrent;
cpt++;
}
} else {
vpImage<unsigned char> hue(I.getHeight(), I.getWidth());
vpImage<unsigned char> saturation(I.getHeight(), I.getWidth());
vpImage<unsigned char> value(I.getHeight(), I.getWidth());
unsigned int size = I.getWidth()*I.getHeight();
//Convert from RGBa to HSV
vpImageConvert::RGBaToHSV((unsigned char *) I.bitmap, (unsigned char *) hue.bitmap,
(unsigned char *) saturation.bitmap, (unsigned char *) value.bitmap, size);
//Histogram equalization on the value plane
vp::equalizeHistogram(value);
//Convert from HSV to RGBa
vpImageConvert::HSVToRGBa((unsigned char*) hue.bitmap, (unsigned char*) saturation.bitmap,
(unsigned char*) value.bitmap, (unsigned char*) I.bitmap, size);
}
}
bool hsv_colour::operator==(const hsv_colour& aOther) const
{
return hue() == aOther.hue() &&
saturation() == aOther.saturation() &&
value() == aOther.value() &&
alpha() == aOther.alpha() &&
hue_undefined() == aOther.hue_undefined();
}
void KHueSaturationSelector::drawPalette( QPixmap *pixmap )
{
int xSize = contentsRect().width(), ySize = contentsRect().height();
QImage image( QSize( xSize, ySize ), QImage::Format_RGB32 );
QColor col;
int h, s;
uint *p;
col.setHsv( hue(), saturation(), colorValue() );
int _h, _s, _v, _r, _g, _b;
col.getHsv( &_h, &_s, &_v );
col.getRgb( &_r, &_g, &_b );
for ( s = ySize-1; s >= 0; s-- )
{
p = ( uint * ) image.scanLine( ySize - s - 1 );
for( h = 0; h < xSize; h++ )
{
switch ( chooserMode() ) {
case ChooserClassic:
default:
col.setHsv( 359 * h / ( xSize - 1 ), 255 * s / ( ( ySize == 1 ) ? 1 : ySize - 1 ), 192 );
break;
case ChooserHue:
col.setHsv( _h, 255 * h / ( xSize - 1 ), 255 * s / ( ( ySize == 1 ) ? 1 : ySize - 1 ) );
break;
case ChooserSaturation:
col.setHsv( 359 * h / ( xSize - 1 ), _s, 255 * s / ( ( ySize == 1 ) ? 1 : ySize - 1 ) );
break;
case ChooserValue:
col.setHsv( 359 * h / ( xSize - 1 ), 255 * s / ( ( ySize == 1 ) ? 1 : ySize - 1 ), _v );
break;
case ChooserRed:
col.setRgb( _r, 255 * h / ( xSize - 1 ), 255 * s / ( ( ySize == 1 ) ? 1 : ySize - 1 ) );
break;
case ChooserGreen:
col.setRgb( 255 * h / ( xSize - 1 ), _g, 255 * s / ( ( ySize == 1 ) ? 1 : ySize - 1 ) );
break;
case ChooserBlue:
col.setRgb( 255 * s / ( ( ySize == 1 ) ? 1 : ySize - 1 ), 255 * h / ( xSize - 1 ), _b );
break;
}
*p = col.rgb();
p++;
}
}
/*
if ( pixmap->depth() <= 8 )
{
const QVector<QColor> standardPalette = kdeui_standardPalette();
KImageEffect::dither( image, standardPalette.data(), standardPalette.size() );
}
*/
*pixmap = QPixmap::fromImage( image );
}
// from http://www.cs.rit.edu/~ncs/color/t_convert.html
FColor HSB::hsbToRgb(const FColor& hsb) {
auto h = hue(hsb), s = saturation(hsb), b = brightness(hsb);
float red, green, blue;
if (s == 0) {
// achromatic (grey)
red = green = blue = b;
} else {
h *= 6; // sector 0 to 5
int i = floor(h);
auto f = h - i; // factorial part of h
auto p = b * (1 - s);
auto q = b * (1 - s * f);
auto t = b * (1 - s * (1 - f));
switch(i) {
case 0:
red = b;
green = t;
blue = p;
break;
case 1:
red = q;
green = b;
blue = p;
break;
case 2:
red = p;
green = b;
blue = t;
break;
case 3:
red = p;
green = q;
blue = b;
break;
case 4:
red = t;
green = p;
blue = b;
break;
case 5:
default:
red = b;
green = p;
blue = q;
break;
}
}
return FColor(red, green, blue, hsb.alpha());
}
double discretePI::increment(double sp, double meas, double t, double dt)
{
double out, delta;
delta = vpi.increment(sp, meas); // Calc velocity PI
if (t == 0.) prevDelta = ic; //
delta += prevDelta; // Sum with last
delta = saturation(hi, lo, delta); // Test the limits
out = prevDelta; // Get the previous value (unit delay)
prevDelta = delta; // Store current to use on the next iteration
return out;
}
void AgentCore::guidance() {
double los_distance = std::sqrt(std::pow(pose_virtual_.position.x - pose_.position.x, 2)
+ std::pow(pose_virtual_.position.y - pose_.position.y, 2));
// std::atan2 automatically handle the los_distance == 0 case >> los_angle = 0
double los_angle = std::atan2(pose_virtual_.position.y - pose_.position.y,
pose_virtual_.position.x - pose_.position.x);
double speed_command = k_p_speed_*los_distance;
speed_command_sat_ = saturation(speed_command, speed_min_, speed_max_);
double steer_command = k_p_steer_*angles::shortest_angular_distance(getTheta(pose_.orientation), los_angle);
steer_command_sat_ = saturation(steer_command, steer_min_, steer_max_);
// there is no need to get sub-centimeter accuracy
floor(speed_command_sat_, 1);
std::stringstream s;
s << "Guidance commands (" << speed_command_sat_ << ", " << steer_command_sat_ << ").";
console(__func__, s, DEBUG_VV);
s << "LOS distance and angle (" << los_distance << ", " << los_angle << ").";
console(__func__, s, DEBUG_VVVV);
}
/// @brief
/// @todo Doc me!
void run()
{
// Initial saturation.
std::vector<double> saturation(this->init_saturation_);
std::vector<double> saturation_old(saturation);
// Gravity.
// Dune::FieldVector<double, 3> gravity(0.0);
// gravity[2] = -Dune::unit::gravity;
// Compute flow field.
if (this->gravity_.two_norm() > 0.0) {
MESSAGE("Warning: Gravity not handled by flow solver.");
}
// Solve some steps.
for (int i = 0; i < this->simulation_steps_; ++i) {
std::cout << "\n\n================ Simulation step number " << i
<< " ===============" << std::endl;
// Flow.
this->flow_solver_.solve(this->res_prop_, saturation, this->bcond_, this->injection_rates_psolver_,
this->residual_tolerance_, this->linsolver_verbosity_, this->linsolver_type_);
// if (i == 0) {
// flow_solver_.printSystem("linsys_dump_mimetic");
// }
// Transport.
this->transport_solver_.transportSolve(saturation, this->stepsize_, this->gravity_,
this->flow_solver_.getSolution(),
this->injection_rates_);
// Output.
writeVtkOutput(this->ginterf_,
this->res_prop_,
this->flow_solver_.getSolution(),
saturation,
"testsolution-" + boost::lexical_cast<std::string>(i));
writeField(saturation, "saturation-" + boost::lexical_cast<std::string>(i));
// Comparing old to new.
int num_cells = saturation.size();
double maxdiff = 0.0;
for (int i = 0; i < num_cells; ++i) {
maxdiff = std::max(maxdiff, std::fabs(saturation[i] - saturation_old[i]));
}
std::cout << "Maximum saturation change: " << maxdiff << std::endl;
// Copy to old.
saturation_old = saturation;
}
}
unsigned int filter_color(unsigned int color, t_opt *opt)
{
if (opt->filter == 1)
color = filter_sepia(color);
if (opt->filter == 2)
color = filter_grey(color);
if (opt->gamma != -1)
color = gamma_filter(color, opt->gamma);
if (opt->filter == 3)
color = revers_filter(color, opt);
if (opt->contrast != 1.0)
color = apply_contrast(color, opt);
if (opt->saturation != 1.0)
color = saturation(color, opt->saturation);
return (color);
}
void ColorBox::setColor(const QColor &color)
{
if (m_color == color)
return;
int oldsaturation = m_color.hsvSaturation();
int oldvalue = m_color.value();
int oldhue = m_color.hsvHue();
int oldAlpha = m_color.alpha();
m_color=color;
update();
if (oldhue != m_color.hsvHue()) emit hueChanged();
if (oldsaturation != saturation()) emit saturationChanged();
if (oldvalue != value()) emit valueChanged();
if (oldAlpha != alpha()) emit alphaChanged();
}
bool equals(const BlackoilState& other, double epsilon = 1e-8) const {
bool equal = (numPhases() == other.numPhases());
for (int phaseIdx = 0; phaseIdx < BlackoilPhases::MaxNumPhases; ++ phaseIdx) {
equal = equal && (usedPhases_.phase_used[phaseIdx] == other.usedPhases_.phase_used[phaseIdx]);
if (usedPhases_.phase_used[phaseIdx])
equal = equal && (usedPhases_.phase_pos[phaseIdx] == other.usedPhases_.phase_pos[phaseIdx]);
}
equal = equal && (vectorApproxEqual( pressure() , other.pressure() , epsilon));
equal = equal && (vectorApproxEqual( facepressure() , other.facepressure() , epsilon));
equal = equal && (vectorApproxEqual( faceflux() , other.faceflux() , epsilon));
equal = equal && (vectorApproxEqual( surfacevol() , other.surfacevol() , epsilon));
equal = equal && (vectorApproxEqual( saturation() , other.saturation() , epsilon));
equal = equal && (vectorApproxEqual( gasoilratio() , other.gasoilratio() , epsilon));
return equal;
}
void ItemPinned::paintEvent(QPaintEvent *paintEvent)
{
const auto *parent = parentWidget();
auto color = parent->palette().color(QPalette::Background);
const int lightThreshold = 100;
const bool menuBackgrounIsLight = color.lightness() > lightThreshold;
color.setHsl(
color.hue(),
color.saturation(),
qMax(0, qMin(255, color.lightness() + (menuBackgrounIsLight ? -200 : 50)))
);
QPainter painter(this);
const int border = pointsToPixels(6);
const QRect rect(width() - border, 0, width(), height());
painter.setOpacity(0.15);
painter.fillRect(rect, color);
QWidget::paintEvent(paintEvent);
}
QVariant S60CameraImageProcessingControl::processingParameter(
QCameraImageProcessingControl::ProcessingParameter parameter) const
{
switch (parameter) {
case QCameraImageProcessingControl::Contrast:
return QVariant(contrast());
case QCameraImageProcessingControl::Saturation:
return QVariant(saturation());
case QCameraImageProcessingControl::Brightness:
return QVariant(brightness());
case QCameraImageProcessingControl::Sharpening:
return QVariant(sharpeningLevel());
case QCameraImageProcessingControl::Denoising:
return QVariant(denoisingLevel());
case QCameraImageProcessingControl::ColorTemperature:
return QVariant(manualWhiteBalance());
default:
return QVariant();
}
}
void BandwidthGui::handleClickedLegend(QCPLegend *legend, QCPAbstractLegendItem *item, QMouseEvent *event)
{
QCPPlottableLegendItem* graphItem = dynamic_cast<QCPPlottableLegendItem*>(item);
if(!graphItem)
return;
auto color = graphItem->plottable()->brush().color();
auto hue = color.hue() + 180 % 360;
auto sat = color.saturation();
auto val = color.value();
if(sat == BRUSH_SATURATION)
{
sat = 255;
val = 255;
}
else
{
sat = BRUSH_SATURATION;
val = BRUSH_VALUE;
}
color.setHsv(hue, sat, val);
graphItem->plottable()->setBrush(QBrush(color));
}
//------------------------------------------------------------------------------
// serialize() -- print the value of this object to the output stream sout.
//------------------------------------------------------------------------------
std::ostream& Hsv::serialize(std::ostream& sout, const int i, const bool slotsOnly) const
{
int j = 0;
if ( !slotsOnly ) {
sout << "( " << getFactoryName() << std::endl;
j = 4;
}
indent(sout,i+j);
sout << "hue: " << hue() << std::endl;
indent(sout,i+j);
sout << "saturation: " << saturation() << std::endl;
indent(sout,i+j);
sout << "value: " << value() << std::endl;
if ( !slotsOnly ) {
indent(sout,i);
sout << ")" << std::endl;
}
return sout;
}
static unsigned char R(const YCbCr& src) throw()
{
return saturation
((src.Y * 256) + 359 * (src.Cr - 128));
}
/*!
* \brief Returns the color used to draw instructions.
*/
QColor instructionTextColor()
{
const auto baseColor = QGuiApplication::palette().base().color();
return (baseColor.value() > 204 && baseColor.saturation() < 63) ? QColor(0x00, 0x33, 0x99) : QGuiApplication::palette().text().color();
}
static unsigned char G(const YCbCr& src) throw()
{
return saturation
((src.Y * 256) - 88 * (src.Cb - 128) - 183 * (src.Cr - 128));
}
/* ==================================== */
int32_t lkern(struct xvimage *image, int32_t connex)
/* ==================================== */
{
int32_t x; /* index muet de pixel */
int32_t y; /* index muet (generalement un voisin de x) */
int32_t k; /* index muet */
int32_t rs = rowsize(image); /* taille ligne */
int32_t cs = colsize(image); /* taille colonne */
int32_t N = rs * cs; /* taille image */
uint8_t *F = UCHARDATA(image); /* l'image de depart */
Fahp * FAHP;
int32_t t4mm, t4m, t8p, t8pp;
#ifdef DEBUG
uint8_t oldFx;
#endif
if (depth(image) != 1)
{
fprintf(stderr, "lkern: cette version ne traite pas les images volumiques\n");
exit(0);
}
IndicsInit(N);
FAHP = CreeFahpVide(N);
if (FAHP == NULL)
{ fprintf(stderr, "lkern() : CreeFahpVide failed\n");
return(0);
}
/* ================================================ */
/* DEBUT ALGO */
/* ================================================ */
/* empile tous les voisins des points du bord */
for (x = rs + 1, y = (cs-2) * rs + 1; x < 2*rs-1; x++, y++)
{
FahpPush(FAHP, x, F[x]); Set(x, EN_FAHP);
FahpPush(FAHP, y, F[y]); Set(y, EN_FAHP);
}
for (x = 2*rs+1 , y = 3*rs-2; x < (cs-2)*rs+1; y += rs, x += rs)
{
FahpPush(FAHP, x, F[x]); Set(x, EN_FAHP);
FahpPush(FAHP, y, F[y]); Set(y, EN_FAHP);
}
saturation(rs, cs, N, F, FAHP);
/* met a 0 les puits et les interieurs de plateaux (T-- = 0) */
/* et empile les voisins */
for (x = 0; x < N; x++)
if ((x%rs != rs-1) && (x >= rs) && (x%rs != 0) && (x < N-rs) && /* non point de bord */
(F[x] != 0))
{
nbtopo(F, x, rs, N, &t4mm, &t4m, &t8p, &t8pp);
if (t4mm == 0)
{
F[x] = 0;
for (k = 0; k < 8; k += 1) /* parcourt les voisins en 8-connexite */
{ /* pour empiler les voisins */
y = voisin(x, k, rs, N); /* non deja empiles */
if ((y != -1) && (F[y] > 0) && (! IsSet(y, EN_FAHP)))
{
FahpPush(FAHP, y, F[y]);
Set(y, EN_FAHP);
} /* if y */
} /* for k */
} /* if (t4mm == 0) */
saturation(rs, cs, N, F, FAHP);
} /* for (x = 0; x < N; x++) */
/* ================================================ */
/* UN PEU DE MENAGE */
/* ================================================ */
IndicsTermine();
FahpTermine(FAHP);
return(1);
}
static unsigned char B(const YCbCr& src) throw()
{
return saturation
(src.Y * 256 + 454 * src.Cb - 128);
}
void Gosu::Color::setValue(double v)
{
*this = fromAHSV(alpha(), hue(), saturation(), v);
}
void KColorValueSelector::drawPalette( QPixmap *pixmap )
{
int xSize = contentsRect().width(), ySize = contentsRect().height();
QImage image( QSize( xSize, ySize ), QImage::Format_RGB32 );
QColor col;
uint *p;
QRgb rgb;
int _r, _g, _b;
col.setHsv( hue(), saturation(), colorValue() );
col.getRgb( &_r, &_g, &_b );
if ( orientation() == Qt::Horizontal )
{
for ( int v = 0; v < ySize; v++ )
{
p = ( uint * ) image.scanLine( ySize - v - 1 );
for( int x = 0; x < xSize; x++ )
{
switch ( chooserMode() ) {
case ChooserClassic:
default:
col.setHsv( hue(), saturation(), 255 * x / ( ( xSize == 1 ) ? 1 : xSize - 1 ) );
break;
case ChooserRed:
col.setRgb( 255 * x / ( ( xSize == 1 ) ? 1 : xSize - 1 ), _g, _b );
break;
case ChooserGreen:
col.setRgb( _r, 255 * x / ( ( xSize == 1 ) ? 1 : xSize - 1 ), _b );
break;
case ChooserBlue:
col.setRgb( _r, _g, 255 * x / ( ( xSize == 1 ) ? 1 : xSize - 1 ) );
break;
case ChooserHue:
col.setHsv( 360 * x / ( ( xSize == 1 ) ? 1 : xSize - 1 ), 255, 255 );
break;
case ChooserSaturation:
col.setHsv( hue(), 255 * x / ( ( xSize == 1 ) ? 1 : xSize - 1 ), colorValue() );
break;
case ChooserValue:
col.setHsv( hue(), saturation(), 255 * x / ( ( xSize == 1 ) ? 1 : xSize - 1 ) );
break;
}
rgb = col.rgb();
*p++ = rgb;
}
}
}
if( orientation() == Qt::Vertical )
{
for ( int v = 0; v < ySize; v++ )
{
p = ( uint * ) image.scanLine( ySize - v - 1 );
switch ( chooserMode() ) {
case ChooserClassic:
default:
col.setHsv( hue(), saturation(), 255 * v / ( ( ySize == 1 ) ? 1 : ySize - 1 ) );
break;
case ChooserRed:
col.setRgb( 255 * v / ( ( ySize == 1 ) ? 1 : ySize - 1 ), _g, _b );
break;
case ChooserGreen:
col.setRgb( _r, 255 * v / ( ( ySize == 1 ) ? 1 : ySize - 1 ), _b );
break;
case ChooserBlue:
col.setRgb( _r, _g, 255 * v / ( ( ySize == 1 ) ? 1 : ySize - 1 ) );
break;
case ChooserHue:
col.setHsv( 360 * v / ( ( ySize == 1 ) ? 1 : ySize - 1 ), 255, 255 );
break;
case ChooserSaturation:
col.setHsv( hue(), 255 * v / ( ( ySize == 1 ) ? 1 : ySize - 1 ), colorValue() );
break;
case ChooserValue:
col.setHsv( hue(), saturation(), 255 * v / ( ( ySize == 1 ) ? 1 : ySize - 1 ) );
break;
}
rgb = col.rgb();
for ( int i = 0; i < xSize; i++ )
*p++ = rgb;
}
}
/*
if ( pixmap->depth() <= 8 )
{
extern QVector<QColor> kdeui_standardPalette();
const QVector<QColor> standardPalette = kdeui_standardPalette();
KImageEffect::dither( image, standardPalette.data(), standardPalette.size() );
}
*/
*pixmap = QPixmap::fromImage( image );
}
void Gosu::Color::setHue(double h)
{
*this = fromAHSV(alpha(), h, saturation(), value());
}
void JointTorqueControl::computeOutputMotorTorques()
{
//Compute joint level torque PID
double dt = this->getRate() * 0.001;
for(int j=0; j < this->axes; j++ )
{
JointTorqueLoopGains &gains = jointTorqueLoopGains[j];
jointTorquesError[j] = measuredJointTorques[j] - desiredJointTorques[j];
integralState[j] = saturation(integralState[j] + gains.ki*dt*jointTorquesError(j),gains.max_int,-gains.max_int );
jointControlOutputBuffer[j] = desiredJointTorques[j] - gains.kp*jointTorquesError[j] - integralState[j];
}
toEigenVector(jointControlOutput) = couplingMatrices.fromJointTorquesToMotorTorques * toEigenVector(jointControlOutputBuffer);
toEigenVector(measuredMotorVelocities) = couplingMatrices.fromJointVelocitiesToMotorVelocities * toEigenVector(measuredJointVelocities);
// Evaluation of coulomb friction with smoothing close to zero velocity
double coulombFriction;
double coulombVelThre;
for (int j = 0; j < this->axes; j++)
{
MotorParameters motorParam = motorParameters[j];
coulombVelThre = motorParam.coulombVelThr;
//viscous friction compensation
if (fabs(measuredMotorVelocities[j]) >= coulombVelThre)
{
coulombFriction = sign(measuredMotorVelocities[j]);
}
else
{
coulombFriction = pow(measuredMotorVelocities[j] / coulombVelThre, 3);
}
if (measuredMotorVelocities[j] > 0 )
{
coulombFriction = motorParam.kcp*coulombFriction;
}
else
{
coulombFriction = motorParam.kcn*coulombFriction;
}
jointControlOutput[j] = motorParam.kff*jointControlOutput[j] + motorParameters[j].frictionCompensation * (motorParam.kv*measuredMotorVelocities[j] + coulombFriction);
}
if (streamingOutput)
{
yarp::sig::Vector& output = portForStreamingPWM.prepare();
output = jointControlOutput;
portForStreamingPWM.write();
}
toEigenVector(jointControlOutput) = couplingMatricesFirmware.fromMotorTorquesToJointTorques * toEigenVector(jointControlOutput);
bool isNaNOrInf = false;
for(int j = 0; j < this->axes; j++)
{
jointControlOutput[j] = saturation(jointControlOutput[j], jointTorqueLoopGains[j].max_pwm, -jointTorqueLoopGains[j].max_pwm);
if (isnan(jointControlOutput[j]) || isinf(jointControlOutput[j])) { //this is not std c++. Supported in C99 and C++11
jointControlOutput[j] = 0;
isNaNOrInf = true;
}
}
if (isNaNOrInf) {
yWarning("Inf or NaN found in control output");
}
}