//////////////////////////////////////////////////////////////////////////////////
// This function will calculate the value of a point at the progression passed in
// argument using the B-Spline algorithm with 5 points.
//
// i_progression : The value of the progression we want to find a value for.
// i_P0, i_P1, i_P2, i_P3, i_P4 : The 5 points making the curve.
// o_result : The result of the calculation.
//
// Returns true if successful, false otherwise.
//////////////////////////////////////////////////////////////////////////////////
bool BSpline::getDerivativeOrder0( double i_progression,
const Vector &i_P0,
const Vector &i_P1,
const Vector &i_P2,
const Vector &i_P3,
const Vector &i_P4,
Vector &o_result )
{
validateProgression( i_progression );
if( m_BSplineType != INTERPOLATION_ON_5_POINTS )
return false;
FMatrix l_U0( 1, 5 );
l_U0( 0, 0 ) = i_progression * i_progression * i_progression * i_progression; // u^4
l_U0( 0, 1 ) = i_progression * i_progression * i_progression; // u^3
l_U0( 0, 2 ) = i_progression * i_progression; // u^2
l_U0( 0, 3 ) = i_progression; // u^1
l_U0( 0, 4 ) = 1.0f; // u^0
calculateResult( l_U0, i_P0, i_P1, i_P2, i_P3, i_P4, o_result );
return true;
}
//--------------------------------------------------------------
void in1klap::update(){
if(appState==4){
currentBeat=metro.getBeatNumber();
if(!metro.isActive()){
recorder.stop();
vector<pair<int,string> >& p=recorder.getRecordedPresses();
vector<pair<int,string> > e=getExpectedVector(chosenSequence,metro.getBpm(),chosenTestStyle==2);
vector<int> b=getExpectedBeatVector(chosenSequence,chosenTestStyle==2);
pair<string, double> r;
expectedVector=e;
receivedVector=p;
r=calculateResult(e,p,b,chosenTestStyle==2);
accuracy=r.first;
precision=ofToString(r.second,2);
if(r.second>60){
precisionLabel=translationLines[22];//"Slecht";
} else if(r.second>50){
precisionLabel=translationLines[21];//"Minder dan gemiddeld";
} else if(r.second>30){
precisionLabel=translationLines[20];//"Gemiddeld";
} else if(r.second>15){
precisionLabel=translationLines[19];//"Goed";
} else if(r.second<15){
precisionLabel=translationLines[18];//"Zeer goed";
}
appState++;
}
}
}
//////////////////////////////////////////////////////////////////////////////////
// This function will calculate the value of a point third derivative at the
// progression passed in argument using the B-Spline algorithm with 5 points.
//
// i_progression : The value of the progression we want to find a value for.
// i_P0, i_P1, i_P2, i_P3, i_P4 : The 5 points making the curve.
// o_result : The result of the calculation.
//
// Returns true if successful, false otherwise.
//////////////////////////////////////////////////////////////////////////////////
bool BSpline::getDerivativeOrder3( double i_progression,
const Vector &i_P0,
const Vector &i_P1,
const Vector &i_P2,
const Vector &i_P3,
const Vector &i_P4,
Vector &o_result )
{
validateProgression( i_progression );
if( m_BSplineType != INTERPOLATION_ON_5_POINTS )
return false;
FMatrix l_U3( 1, 5 );
l_U3( 0, 0 ) = 24.0f * i_progression; // 24*u^1
l_U3( 0, 1 ) = 6.0f; // 6*u^0
l_U3( 0, 2 ) = 0.0f; // 0
l_U3( 0, 3 ) = 0.0f; // 0
l_U3( 0, 4 ) = 0.0f; // 0
calculateResult( l_U3, i_P0, i_P1, i_P2, i_P3, i_P4, o_result );
return true;
}
int main(int argc, char const *argv[])
{
int number = getNumber();
int digit = getDigit();
calculateResult(number, digit);
return 0;
}
void Correction::notation(operande resultat[], bool isGood[])
{
calculateResult(resultat);
for(int i = 0; i < 10; i++)
if(!(isGood[i] = (resultat[i] == _answers[i]) ))
--_note;
}
int main(){
char allStar = '\0', regularMVP = '\0', worldMVP = '\0', goldGlove = '\0', silverSlug = '\0',
homeRun = '\0', battingAve = '\0', gender = '\0';
int bonus = 0, num1 = 0, num2 = 0, num3 = 0, num4 = 0, num5 = 0,
activityLevel = 0, menuChoice = 0, i = 0;
double weight = 0, height = 0, age = 0, bmr = 0, calories = 0, result = 0;
FILE *inputFile = NULL;
//PROBLEM 1
inputFile = openInputFile();
/*
gender = readCharacter(inputFile);
age = readNumber(inputFile);
weight = readNumber(inputFile);
height = readNumber(inputFile);
bmr = computeBMR(gender, weight, height, age);
activityLevel = determineActivityLevel();
calories = computeCalories(bmr, activityLevel);
printf("Calories needed per day are: %.0lf\n", calories);
//PROBLEM 2
allStar = getBaseballAchievements("All-Star Game appearance");
bonus += determineBonus(allStar, 25000);
regularMVP = getBaseballAchievements("Regular Season MVP");
bonus += determineBonus(regularMVP, 75000);
worldMVP = getBaseballAchievements("World Series MVP");
bonus += determineBonus(worldMVP, 100000);
goldGlove = getBaseballAchievements("Gold Glove award");
bonus += determineBonus(goldGlove, 50000);
silverSlug = getBaseballAchievements("Silver Slugger award");
bonus += determineBonus(silverSlug, 35000);
homeRun = getBaseballAchievements("Home run champ");
bonus += determineBonus(homeRun, 25000);
battingAve = getBaseballAchievements("Batting average champ");
bonus += determineBonus(battingAve, 25000);
printf("Total player bonus is %d\n", bonus);
int i = 0;*/
//PROBLEM 3
num1 = readInteger(inputFile);
num2 = readInteger(inputFile);
num3 = readInteger(inputFile);
num4 = readInteger(inputFile);
num5 = readInteger(inputFile);
while (i < 3){
menuChoice = displayMenu();
result = calculateResult(menuChoice, num1, num2, num3, num4, num5);
displayResult(menuChoice, result);
i++;
}
return 0;
}
char* Parser::regularExpression(char *expression) {
trimBothParanthesis(expression);
//If there is no operators return expression(end condition).
operator_s op0 = findOperator(expression, 0);
if (op0.pos == -1) {
Alias_s a;
//Is it a stack call?
a = parseKeywords(expression);
if (strlen(a.value) == 0) {
memcpy(a.value, expression, a.len);
}
return a.value;
}
int len = strlen(expression);
operator_s op1 = findOperator(expression, op0.pos + op0.len);
if (op1.pos != -1) {
//Negative number.
if (op0.pos != -1 && op1.op[0] == '-' && op1.pos == op0.pos +1) {
int len = strlen(expression);
} else {
//If there is a second operator set len = operator.
len = op1.pos;
}
}
memcpy(tmpLhs, expression, len);
tmpLhs[len] = '\0';
int rhsLen = strlen(expression) - len;
memcpy(tmpRhs, expression + len, rhsLen);
tmpRhs[rhsLen] = '\0';
char *res = calculateResult(tmpLhs);
len = strlen(res);
memcpy(tmpStr, res, len);
tmpStr[len] = '\0';
strcat(tmpStr, tmpRhs);
tmpStr[len + strlen(tmpRhs)] = '\0';
//Negative number.
if (isNegativeNumber(tmpStr)) {
return tmpStr;
}
//recursion.
regularExpression(tmpStr);
return tmpStr;
}
void TriangulationWidget::receiveInputUpdate(int id, bool valid)
{
if (valid)
validBitmap |= (1 << id);
else
validBitmap &= ~(1 << id);
if (validBitmap + 1 == (1 << N_INPUTS))
calculateResult();
else
clearResult();
}
void GameClass::update()
{
static int stopTimer = 0;
if (stopping)
{
stopTimer += MS_PER_FRAME;
for (int i = 0; i < STRIPES_NUMBER; ++i)
{
if (stripesSpeeds[i] > 0.0f)
{
if (stopTimer >= 300 &&
abs((int)offsets[i]) < OFFSET_EPS)
{
stripesSpeeds[i] = 0.0f;
stopTimer = 0;
}
break;
} else if (i == STRIPES_NUMBER - 1 && stopping) {
// everything's been stopped
stopping = false;
stopped = true;
startButton->setEnabled(true);
calculateResult();
}
}
} else {
startTimer += MS_PER_FRAME;
if (!stopped && startTimer >= gameTime)
stopGame();
}
// updating stripes
stripeHeight = stripeWidth * scaleFactorY;
for (int i = 0; i < STRIPES_NUMBER; ++i)
{
offsets[i] -= stripesSpeeds[i];
if (offsets[i] < -stripeHeight)
{
offsets[i] += stripeHeight;
for (int j = SQUARES_NUMBER; j >= 1; --j)
stripes[i][j] = stripes[i][j - 1];
stripes[i][0] = rand() % items.size();
}
}
Graphics::redraw();
}
//////////////////////////////////////////////////////////////////////////////////
// This function will calculate the value of a point first derivative at the
// progression passed in argument using the B-Spline algorithm with 4 points.
//
// i_progression : The value of the progression we want to find a value for.
// i_P0, i_P1, i_P2, i_P3 : The 4 points making the curve.
// o_result : The result of the calculation.
//
// Returns true if successful, false otherwise.
//////////////////////////////////////////////////////////////////////////////////
bool BSpline::getDerivativeOrder1( double i_progression,
const Vector &i_P0,
const Vector &i_P1,
const Vector &i_P2,
const Vector &i_P3,
Vector &o_result )
{
validateProgression( i_progression );
if( m_BSplineType != INTERPOLATION_ON_4_POINTS )
return false;
FMatrix l_U1( 1, 4 );
l_U1( 0, 0 ) = 3.0f * i_progression * i_progression; // 3*u^2
l_U1( 0, 1 ) = 2.0f * i_progression; // 2*u^1
l_U1( 0, 2 ) = 1.0f; // 1*u^0
l_U1( 0, 3 ) = 0.0f; // 0
calculateResult( l_U1, i_P0, i_P1, i_P2, i_P3, o_result );
return true;
}
void CalibrateAction::executeCB(const calibrate_twist::CalibrateGoalConstPtr &goal)
{
success = true;
// read all necessary parameters
ros::NodeHandle nhPriv("~");
nhPriv.getParamCached("odo_cache_depths", odo_cache_depths);
nhPriv.getParamCached("stability_timeout", stability_timeout);
nhPriv.getParamCached("stability_intervalDuration", stability_intervalDuration);
nhPriv.getParamCached("stability_xThreshold", stability_xThreshold);
nhPriv.getParamCached("stability_zThreshold", stability_zThreshold);
nhPriv.getParamCached("calibration_calc_interval", calibration_calc_interval);
nhPriv.getParamCached("tfFixedFrame", tfFixedFrame);
nhPriv.getParamCached("robotFrame", robotFrame);
nhPriv.getParamCached("cmdVelTopic", cmdVelTopic);
nhPriv.getParamCached("minStabilityDuration", minStabilityDuration);
nhPriv.getParamCached("transforms_interval_size", transforms_interval_size);
nhPriv.getParamCached("cal_costmap", cal_costmap);
nhPriv.getParamCached("traj_sim_granularity_", traj_sim_granularity_);
nhPriv.getParamCached("traj_dist_threshold", traj_dist_threshold);
nhPriv.getParamCached("accel_max_x", accel_max_x);
nhPriv.getParamCached("accel_max_y", accel_max_y);
nhPriv.getParamCached("accel_max_theta", accel_max_theta);
nhPriv.getParamCached("min_time_clear", min_time_clear);
goal_ = *goal;
listener = new tf::TransformListener((goal_.duration)*2); // set cache time twice the time of the calibr. run
cost_map = new costmap_2d::Costmap2DROS(cal_costmap, *listener);
voronoi_.initializeEmpty(cost_map->getSizeInCellsX(), cost_map->getSizeInCellsY(), true);
//ros::Subscriber cm_sub = nh_.subscribe("~/move_base/local_costmap/obstacles", 1, CalibrateAction::costmapCB);
message_filters::Subscriber<nav_msgs::Odometry> sub(nh_, "/odom", 1);
odo_cache = new message_filters::Cache<nav_msgs::Odometry> (sub, odo_cache_depths);
estTraj_pub = nh_.advertise<geometry_msgs::PoseArray>("est_traj_", 10);
calcTraj_pub = nh_.advertise<geometry_msgs::PoseArray>("calc_traj_", 10);
twist_pub = nh_.advertise<geometry_msgs::Twist>(cmdVelTopic,1);
voronoi_pub = nh_.advertise<visualization_msgs::MarkerArray>("voronoi_marker", 10);
// necessary? how to initialize as empty?
zero_twist.angular.x = 0;
zero_twist.angular.y = 0;
zero_twist.angular.z = 0;
zero_twist.linear.x = 0;
zero_twist.linear.y = 0;
zero_twist.linear.z = 0;
// publish info to the console for the user
ROS_INFO("%s: Executing, running calibration run for %f seconds with linear speed %f, angular speed %f", action_name_.c_str(), goal_.duration.toSec(), goal_.twist_goal.linear.x, goal_.twist_goal.angular.z);
// update voronoi at first, else no distance check is possible
updateVoronoi(); // load values from costmap and push them into voronoi
//**************************************************
// bringing the robot to the goal speed
//**************************************************
bool stability_reached = false;
bool check = estimateFullPathIsClear(goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, goal_.duration.toSec(), accel_max_x, accel_max_y, accel_max_theta);
ROS_INFO("Full check results in %i", check);
if(checkPath(0, 0, 0, goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, accel_max_x, accel_max_y, accel_max_theta))
{
stability_reached = bringupGoalSpeed();
}
// not stopping robot here because we want to have continuous movement!
if(!stability_reached)
{
twist_pub.publish(zero_twist); // safety first, stop robot
as_.setAborted();
ROS_INFO("%s: Aborted. No stability reached within timeout", action_name_.c_str());
success = false;
return;
}
//**************************************************
// starting calibration run for given duration
//**************************************************
else if(success)
{
ROS_INFO("Speed up successful, starting calibration run");
if(checkPath(goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, goal_.duration.toSec(),
goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, 0.0, 0.0, 0.0))
{
success = startCalibrationRun(); // check whether we finished the run successfully or not
}
else
{
twist_pub.publish(zero_twist); // safety first, stop robot
as_.setAborted();
ROS_INFO("%s: Aborted. No space to drive planned trajectory", action_name_.c_str());
success = false;
return;
}
}
// end of movement, therefore robo is stopped
twist_pub.publish(zero_twist); // safety first, stop robot
//**************************************************
// calculating the result
//**************************************************
if(success)
{
ROS_INFO("Calibration run finished, calculating result...");
calculateResult();
}
else
{
ROS_INFO("Calibration aborted");
as_.setAborted();
return;
}
//**************************************************
// publishing the result
//**************************************************
if(success)
{
result_.calibrated_result = twistWCFromTf;
result_.odo_result = twistWCFromOdometry;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
// callback finished
}
