void TriskarSmallTorso::callbackUpdateTriskarUpper(const theatre_bot::Vector32::ConstPtr& msg) {
current_angle = static_cast<float>(msg->y/180.0*M_PI);
//std::cout<<"Current "<<current_angle<<" desire "<<desire_angle_to_move<<std::endl;
if(is_moving||is_moving_emotional) {
std::cout<<desire_angle_to_move-angle_error<<" -> "<<current_angle<<" "<<desire_angle_to_move<<std::endl;
if(current_angle>=(desire_angle_to_move-angle_error)&¤t_angle<=(desire_angle_to_move+angle_error)) {
//std::cout<<"It has finish"<<std::endl;
is_moving = false;
is_moving_emotional = false;
theatre_bot::ActionExecutionMessage message;
message.coming_from = this->action_name_move;
message.coming_to = "";
message.message = "action_finished";
this->stopMoveTorsoAction();
pub_action_synch->publish(message);
} else if(!(is_oscillating||is_oscillating_emotional)) {
if(fabsf(last_angle - current_angle) < minimum_change_angle) {
temp_angle = temp_angle + delta_time*velocity_move*sgn<float>(desire_angle_to_move-current_angle);
} else {
temp_angle = delta_time*velocity_move*sgn<float>(desire_angle_to_move-current_angle);
last_angle = current_angle;
}
float temp_desire_angle = current_angle + temp_angle;
if((desire_angle_to_move>0&&temp_desire_angle>desire_angle_to_move)||(temp_desire_angle<desire_angle_to_move&&desire_angle_to_move<=0)) {
temp_desire_angle = desire_angle_to_move;
}
std::cout<<"temp Desire angle "<<temp_desire_angle<<" to move "<<desire_angle_to_move<<" difference "<<delta_time*velocity_move*sgn<float>(desire_angle_to_move-current_angle)<<std::endl;
verifyRange(&temp_desire_angle,min_angle,max_angle);
sendMessage(temp_desire_angle);
}
}
if(is_oscillating||is_oscillating_emotional) {
//std::cout<<"Oscillating torso "<<is_oscillating<<" or "<<is_oscillating_emotional<<" velocity "<<velocity_oscillate<<" current angle "<<current_angle<<" min "<<min_angle<<" desire angel to move "<<desire_angle_to_move<<" desire angle to oscillate "<<desire_angle_to_oscillate<<std::endl;
float desire_velocity = this->updateOscillation(velocity_oscillate,current_angle,min_angle,max_angle,desire_angle_to_move,desire_angle_to_oscillate,angle_error,&forward_direction_x);
//float next_position = current_angle + desire_velocity *delta_time;
if(fabsf(last_angle - current_angle) < minimum_change_angle) {
//std::cout<<" It is less than minimum"<<std::endl;
temp_angle = temp_angle + delta_time*desire_velocity;
} else {
//std::cout<<"It is more than the minimum change"<<std::endl;
temp_angle = delta_time*desire_velocity;
last_angle = current_angle;
}
//std::cout<<"The delta angle "<<temp_angle<<std::endl;
float next_position = current_angle + temp_angle;
if((next_position>desire_angle_to_move+desire_angle_to_oscillate)||(next_position<desire_angle_to_move-desire_angle_to_oscillate)) {
next_position = desire_angle_to_move;
}
//std::cout<<" current angle "<<current_angle<<" vel: "<<velocity_oscillate<<" oscillation "<<desire_angle_to_oscillate<<" angle "<<desire_angle_to_move<<" "<<desire_velocity<<" "<<next_position<<std::endl;
verifyRange(&next_position,min_angle,max_angle);
sendMessage(next_position);
}
}
void BoxCurve::draw(QPainter *painter,
const QwtDiMap &xMap, const QwtDiMap &yMap, int from, int to)
{
if ( !painter || dataSize() <= 0 )
return;
int size = dataSize();
if (to < 0)
to = size - 1;
if (verifyRange(from, to) > 0)
{
painter->save();
painter->setPen(QwtPlotCurve::pen());
double *dat = new double[size];
for (int i = from; i<= to; i++)
dat[i] = y(i);
drawBox(painter, xMap, yMap, dat, size);
drawSymbols(painter, xMap, yMap, dat, size);
painter->restore();
delete[] dat;
}
}
/*!
Draw an interval of the curve
\param painter Painter
\param xMap Maps x-values into pixel coordinates.
\param yMap Maps y-values into pixel coordinates.
\param canvasRect Contents rect of the canvas
\param from Index of the first point to be painted
\param to Index of the last point to be painted. If to < 0 the
curve will be painted to its last point.
\sa drawCurve(), drawSymbols(),
*/
void QwtPlotCurve::drawSeries( QPainter *painter,
const QwtScaleMap &xMap, const QwtScaleMap &yMap,
const QRectF &canvasRect, int from, int to ) const
{
if ( !painter || dataSize() <= 0 )
return;
if ( to < 0 )
to = (int)dataSize() - 1;
if ( verifyRange( (int)dataSize(), from, to ) > 0 )
{
painter->save();
painter->setPen( d_data->pen );
/*
Qt 4.0.0 is slow when drawing lines, but it's even
slower when the painter has a brush. So we don't
set the brush before we really need it.
*/
drawCurve( painter, d_data->style, xMap, yMap, canvasRect, from, to );
painter->restore();
if ( d_data->symbol &&
( d_data->symbol->style() != QwtSymbol::NoSymbol ) )
{
painter->save();
drawSymbols( painter, *d_data->symbol,
xMap, yMap, canvasRect, from, to );
painter->restore();
}
}
}
/*!
\brief Draw an intervall of the curve
\param painter Painter
\param xMap maps x-values into pixel coordinates.
\param yMap maps y-values into pixel coordinates.
\param from index of the first point to be painted
\param to index of the last point to be painted. If to < 0 the
curve will be painted to its last point.
\sa QwtCurve::drawCurve, QwtCurve::drawDots,
QwtCurve::drawLines, QwtCurve::drawSpline,
QwtCurve::drawSteps, QwtCurve::drawSticks
*/
void QwtCurve::draw(QPainter *painter,
const QwtDiMap &xMap, const QwtDiMap &yMap, int from, int to)
{
if ( !painter || dataSize() <= 0 )
return;
if (to < 0)
to = dataSize() - 1;
if ( verifyRange(from, to) > 0 )
{
painter->save();
painter->setPen(d_pen);
QBrush b = d_brush;
if ( b.style() != Qt::NoBrush && !b.color().isValid() )
b.setColor(d_pen.color());
painter->setBrush(b);
drawCurve(painter, d_style, xMap, yMap, from, to);
painter->restore();
if (d_sym.style() != QwtSymbol::None)
{
painter->save();
drawSymbols(painter, d_sym, xMap, yMap, from, to);
painter->restore();
}
}
}
/*!
\brief Draw an interval of the curve
\param painter Painter
\param azimuthMap Maps azimuth values to values related to 0.0, M_2PI
\param radialMap Maps radius values into painter coordinates.
\param pole Position of the pole in painter coordinates
\param from index of the first point to be painted
\param to index of the last point to be painted. If to < 0 the
curve will be painted to its last point.
\sa drawCurve(), drawSymbols(),
*/
void QwtPolarCurve::draw( QPainter *painter,
const QwtScaleMap &azimuthMap, const QwtScaleMap &radialMap,
const QwtDoublePoint &pole, int from, int to ) const
{
if ( !painter || dataSize() <= 0 )
return;
if ( to < 0 )
to = dataSize() - 1;
if ( verifyRange( dataSize(), from, to ) > 0 )
{
painter->save();
painter->setPen( d_data->pen );
drawCurve( painter, d_data->style,
azimuthMap, radialMap, pole, from, to );
painter->restore();
if ( d_data->symbol->style() != QwtSymbol::NoSymbol )
{
painter->save();
drawSymbols( painter, *d_data->symbol,
azimuthMap, radialMap, pole, from, to );
painter->restore();
}
}
}
//! Main C entry point
//!
//! Assumes ramBuffer is set up beforehand
//!
void entry(void) {
FlashData_t *flashData; // Handle on programming data
// Disable COP
*(gFlashProgramHeader.soptAddress+0x0A) = 0x00;
// Handle on programming data
flashData = gFlashProgramHeader.flashData;
// Indicate not complete
flashData->flags &= ~IS_COMPLETE;
// No errors so far
flashData->errorCode = FLASH_ERR_OK;
if (flashData->controller == NULL) {
setErrorCode(FLASH_ERR_ILLEGAL_PARAMS);
}
// Clear invalid/unused address bits
// A23 is used for Flash block number
flashData->address &= 0x008FFFFFUL;
initFlash(flashData);
eraseFlashBlock(flashData);
programPartition(flashData) ;
eraseRange(flashData);
blankCheckRange(flashData);
programRange(flashData);
verifyRange(flashData);
// Indicate completed
setErrorCode(FLASH_ERR_OK);
}
/*!
\brief Draw an interval of the curve
\param painter Painter
\param xMap maps x-values into pixel coordinates.
\param yMap maps y-values into pixel coordinates.
\param from index of the first point to be painted
\param to index of the last point to be painted. If to < 0 the
curve will be painted to its last point.
\sa drawCurve(), drawSymbols(),
*/
void QwtPlotCurve::draw(QPainter *painter,
const QwtScaleMap &xMap, const QwtScaleMap &yMap,
int from, int to) const
{
if (!painter)
return;
const int numPoints = (int)dataSize();
if (to < 0 || to >= numPoints)
to = numPoints - 1;
if (verifyRange(numPoints, from, to) > 0){
painter->save();
painter->setPen(QwtPainter::scaledPen(d_data->pen));
/*
Qt 4.0.0 is slow when drawing lines, but it's even
slower when the painter has a brush. So we don't
set the brush before we really need it.
*/
drawCurve(painter, d_data->style, xMap, yMap, from, to);
painter->restore();
if (d_data->symbol->style() != QwtSymbol::NoSymbol){
painter->save();
drawSymbols(painter, *d_data->symbol, xMap, yMap, from, to);
painter->restore();
}
}
}
void View::Report::VolumeReportDialog::createConnections()
{
connect(_anyRadioButton, SIGNAL(toggled(bool)),
this, SLOT(toggledOnRadioButton()));
connect(_rangeRadioButton, SIGNAL(toggled(bool)),
this, SLOT(toggledOnRadioButton()));
connect(_rangeRadioButton, SIGNAL(toggled(bool)),
this, SLOT(verifyRange()));
connect(_beginDateDateEdit, SIGNAL(dateChanged(QDate)),
this, SLOT(verifyRange()));
connect(_endDateDateEdit, SIGNAL(dateChanged(QDate)),
this, SLOT(verifyRange()));
connect(_okPushButton, SIGNAL(clicked()),
this, SLOT(accept()));
connect(_cancelPushButton, SIGNAL(clicked()),
this, SLOT(reject()));
}
void TriskarSmallTorso::OscillateTorsoAction(Amplitude parameter) {
//std::cout<<"Executing oscillate: "<<parameter.getDistanceX()<<" "<<parameter.getDistanceY()<<" "<<parameter.getDistanceZ()<<std::endl;
desire_angle_to_oscillate = parameter.getDistanceX();
verifyRange(&desire_angle_to_oscillate,min_angle,max_angle);
//std::cout<<"Desire Angle "<<desire_angle_to_oscillate<<std::endl;
if(!is_oscillating_emotional) {
is_oscillating = true;
is_oscillating_emotional = false;
}
}
void TriskarSmallTorso::MoveTorsoAction(Amplitude parameter) {
//std::cout<<"Executing move"<<std::endl;
is_moving_emotional = false;
desire_angle_to_move = parameter.getDistanceX();
verifyRange(&desire_angle_to_move,min_angle,max_angle);
//std::cout<<"Angle "<<desire_angle_to_move<<std::endl;
if(!is_oscillating&&!is_oscillating_emotional) {
is_moving = true;
//Send command
//float temp_position = this->sgn<float>(desire_angle_to_move);//*velocity_move*delta_time;
//temp_position += current_angle;
//verifyRange(&temp_position,min_angle,max_angle);
//this->sendMessage(desire_angle_to_move);
}
}
main(int ac, char **av)
{
if (ac < 3)
Usage();
if (!strcmp(av[1], "-s"))
itob(ac, av);
else if (!strcmp(av[1], "-i"))
btoi(ac, av);
else if (!strcmp(av[1], "-c"))
check(ac, av);
else if (!strcmp(av[1], "-r"))
verifyRange(ac, av);
else
Usage();
exit(0);
}
/**
* Main C entry point
*
* Assumes ramBuffer is set up beforehand
*/
void entry(void) {
// Set the interrupt vector table position
SCB_VTOR = (uint32_t)__vector_table;
// Handle on programming data
FlashData_t *flashData = gFlashProgramHeader.flashData;
initFlash(flashData);
eraseFlashBlock(flashData);
programPartition(flashData) ;
eraseRange(flashData);
blankCheckRange(flashData);
programRange(flashData);
verifyRange(flashData);
#ifndef DEBUG
// Indicate completed & stop
setErrorCode(FLASH_ERR_OK);
#endif
}
void QwtBarCurve::draw(QPainter *painter,
const QwtDiMap &xMap, const QwtDiMap &yMap, int from, int to)
{
if ( !painter || dataSize() <= 0 )
return;
if (to < 0)
to = dataSize() - 1;
if ( verifyRange(from, to) > 0 )
{
painter->save();
painter->setPen(QwtPlotCurve::pen());
painter->setBrush(QwtPlotCurve::brush());
int dx,dy,ref,bar_width;
if (bar_style == Vertical)
ref= yMap.transform(1e-100); //smalest positive value for log scales
else
ref= xMap.transform(1e-100);
int i;
if (bar_style == Vertical)
{
dx = abs(xMap.transform(x(from+1))-xMap.transform(x(from)));
for (i=from+2; i<to; i++)
{
int min = abs(xMap.transform(x(i+1))-xMap.transform(x(i)));
if (min <= dx)
dx=min;
}
bar_width=int(dx*(1-bar_gap*0.01));
}
else
{
dy = abs(yMap.transform(y(from+1))-yMap.transform(y(from)));
for (i=from+2; i<to; i++)
{
int min = abs(yMap.transform(y(i+1))-yMap.transform(y(i)));
if (min <= dy)
dy=min;
}
bar_width=int(dy*(1-bar_gap*0.01));
}
const int half_width = int((0.5-bar_offset*0.01)*bar_width);
const int bw1 = bar_width+1;
for (i=from; i<=to; i++)
{
const int px = xMap.transform(x(i));
const int py = yMap.transform(y(i));
if (bar_style == Vertical)
{
if (y(i) < 0)
painter->drawRect(px-half_width, ref, bw1, (py-ref));
else
painter->drawRect(px-half_width, py, bw1, (ref-py+1));
}
else
{
if (x(i) < 0)
painter->drawRect(px, py-half_width, (ref-px), bw1);
else
painter->drawRect(ref, py-half_width,(px-ref), bw1);
}
}
painter->restore();
}
}
