//______________________________________________________________________________
void beamPositionMonitorInterface::updateData( beamPositionMonitorStructs::monitorStruct * ms, const event_handler_args args )
{
    /// this could be better, with the type passed from the config
    const dbr_time_double * p = ( const struct dbr_time_double * ) args.dbr;
    beamPositionMonitorStructs::rawDataStruct * bpmdo = reinterpret_cast< beamPositionMonitorStructs::rawDataStruct *> (ms -> val);
    if( bpmdo->isAContinuousMonitorStruct)
    {
        for( auto && it1 : bpmObj.dataObjects )
        {
            it1.second.numShots = 1;
            it1.second.shotCount = 0;
        }
    }
    const dbr_double_t * value = &(p  -> value);
    size_t i = 0;
    updateTime( p->stamp, bpmdo->timeStamps[ bpmdo->shotCount ], bpmdo->strTimeStamps[ bpmdo->shotCount ]  );
    for( auto && it : bpmdo->rawBPMData[ bpmdo->shotCount ] )
    {
        it = *( &p->value + i);
        ++i;
    }

    bpmdo->pu1[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 1 ];
    bpmdo->pu2[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 2 ];
    bpmdo->c1[  bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 3 ];
    bpmdo->p1[  bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 4 ];
    bpmdo->pu3[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 5 ];
    bpmdo->pu4[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 6 ];
    bpmdo->c2[  bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 7 ];
    bpmdo->p2[  bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 8 ];

    bpmdo->x[ bpmdo->shotCount ] = calcX( bpmdo->name, bpmdo->pu1[ bpmdo->shotCount ], bpmdo->pu2[ bpmdo->shotCount ], bpmdo->c1[ bpmdo->shotCount ], bpmdo->p1[ bpmdo->shotCount ] );
    bpmdo->y[ bpmdo->shotCount ] = calcY( bpmdo->name, bpmdo->pu3[ bpmdo->shotCount ], bpmdo->pu4[ bpmdo->shotCount ], bpmdo->c2[ bpmdo->shotCount ], bpmdo->p2[ bpmdo->shotCount ] );
    bpmdo->q[ bpmdo->shotCount ] = calcQ( bpmdo->name, bpmdo->rawBPMData[ bpmdo -> shotCount ] );

    if( bpmdo -> isATemporaryMonitorStruct )
    {
        if( bpmdo -> numShots > -1 )
        {
            ++bpmdo -> shotCount;
        }
        if( bpmdo->shotCount == bpmdo->numShots )
        {
            bpmdo->appendingData = false;
//            message( "Collected ", bpmdo->shotCount, " shots for ", bpmdo->name );
            ms->interface->killCallBack( ms, bpmdo );//, bpmdo );
            monitoringData = false;
            bpmdo -> shotCount = 0;
        }
    }

}
Exemplo n.º 2
0
FindPI::FindPI(long int epsilon)
{
	double pi=0;
	#pragma omp parallel reduction(+:pi)
	{
		#pragma omp for
		for (int i = 0; i < epsilon; i++) {
			double x = calcX(i, epsilon);
			pi += (double)(4 / (1 + x*x));
		}
	}
	printf("MATH.PI=%f",pi/(double)epsilon);
}
Exemplo n.º 3
0
/**
   applies the system function to x
 */
void GenBaseSystemModel::apply(CVEC &x,
			       const CVEC &u,
			       const double deltaT)
{
  if((x.GetSize() >= _minXSize) && (u.GetSize() >= _minUSize))
    {
      //the size is important for the dimension of the Jacobians
      _sizeX = x.GetSize();
      //do the calculations
      calcX(x, u, deltaT);
      calcFJacobian(x, deltaT);
      calcWJacobian(x, deltaT);
    }
}
Exemplo n.º 4
0
void CEllipseWindow::OnTimer()
{
	RECT rect;
	tickCount++;
	::GetClientRect(handle, &rect);
	if (x == 0 && y == 0) {
		x = rect.left + aAxis;
		y = rect.top + bAxis;
	}
	else {
		calcX(rect);
		calcY(rect);
	}
	InvalidateRect(handle, &rect, FALSE);
}
Exemplo n.º 5
0
void FindPI::CriticalPI(long int epsilon)
{
	long int tmp;
	int current_thread;
	double sum = 0;
	#pragma omp parallel
	{
		double x_i = 0;
		#pragma omp for
		for (long int i = 0; i < epsilon; i++) {
			double x = calcX(i, epsilon);
			#pragma omp critical
			{
				sum += (double)(4 / (1 + x*x));
			}
		}
	}
	int zeros = 0;
	tmp = epsilon;
	while (tmp = tmp / 10) { zeros += 1; }
	printf("%4.*f\n", zeros - 1, sum / epsilon);
}
Exemplo n.º 6
0
// Update the gait runner and move servos to new positions
// Call it from your main loop or via the scheduler to do it in the background
// NB There is no point scheduling any faster than 20ms as that is the servo refresh rate
// Return true if an animation is playing
boolean gaitRunnerProcess(G8_RUNNER* runner){

	if(!gaitRunnerIsPlaying(runner) || runner->speeds==null){
		return FALSE;
	}

	TICK_COUNT  now = clockGetus();
	int16_t  interval = (now - runner->startTime)>>16;
	if(interval == 0){
		return TRUE;
	}

	// There has been a noticeable change in time
	runner->startTime = now;
	if(runner->backwards){
		interval *= -1;
	}
	interval *= runner->speed;

	// Re-check as drive speed could be zero
	if(interval == 0){
		return TRUE;
	}

	// Locate the current animation
	const G8_ANIMATION* animation = &runner->animations[runner->animation];

	// Update the current time with the new interval
	int16_t currentTime = runner->currentTime + interval;
	if(currentTime >= runner->totalTime){
		// We have finished playing the animation
		if(pgm_read_byte(&animation->sweep)==FALSE){
			currentTime %= runner->totalTime;			// Set back to start of loop
			if(runner->repeatCount){
				runner->repeatCount -= 1;				// One less frame to go
				if(runner->repeatCount==0){
					runner->playing = FALSE;			// we have reached the end
					currentTime = 0;					// set servos to final position
				}
			}
		}else{
			// Start going backwards through the animation
			currentTime = runner->totalTime - (currentTime - runner->totalTime);
			runner->backwards = TRUE;
		}
	}else if(currentTime < 0){
		// We have moved before the start
		if(pgm_read_byte(&animation->sweep)==FALSE){
			currentTime = runner->totalTime + currentTime;
			if(runner->repeatCount){
				runner->repeatCount += 1;				// One more frame to go
				if(runner->repeatCount==0){
					runner->playing = FALSE;			// we have reached the end
					currentTime = 0;					// set servos to start position
				}
			}
		}else{
			// We have completed a sweep
			runner->backwards = FALSE;
			currentTime = -currentTime;

			if(runner->repeatCount){
				runner->repeatCount -= 1;			// One less frame to go
				if(runner->repeatCount==0){
					runner->playing = FALSE;		// we have reached the end
					currentTime = 0;				// set servos to initial position
				}
			}
		}
	}
	runner->currentTime = currentTime; // range is 0....totalTime

	// Current time in the range 0...SCALE_X
	uint16_t frameTime = interpolateU(currentTime, 0,runner->totalTime, 0, SCALE_X);
	uint16_t frameStartTime = 0;
	uint16_t frameEndTime = SCALE_X;

	// Locate the correct frame
	const G8_FRAME* frame = (const G8_FRAME*)pgm_read_word(&animation->frames);
	uint8_t i;
	for(i = pgm_read_byte(&animation->numFrames)-1; i>0; i--){
		const G8_FRAME* f = &frame[i];
		frameStartTime = pgm_read_word(&f->time);
		if(frameStartTime <= frameTime){
			frame = f;
			break;
		}
		frameEndTime = frameStartTime;
		frameStartTime = 0;
	}
	runner->frame = i;

#ifdef DEBUG
rprintf("\n%u,%d",i,currentTime);
#endif

	// Now have:- frameStartTime <= frameTime <= frameEndTime

	// We now need to find the distance along the curve (0...1) that represents
	// the x value = frameTime

	// First guess from 0..1
	uint16_t frameTimeOffset = frameTime-frameStartTime;
	double distanceGuess = ((double)(frameTimeOffset)) / ((double)(frameEndTime-frameStartTime));

	const G8_LIMB_POSITION* limb = (const G8_LIMB_POSITION*)pgm_read_word(&frame->limbs);
	for(uint16_t l = 0; l < runner->num_actuators; l++, limb++){
		double distanceMin = 0.0;
		double distanceMax = 1.0;
		double distance = distanceGuess;

		// Find the correct distance along the line for the required frameTime
		for(uint8_t iterations=0; iterations<20; iterations++){
			uint16_t actualX = calcX(limb, distance);
			if(actualX == frameTimeOffset) break;	// Found it

			if( actualX < frameTimeOffset){
				// We need to increase t
				distanceMin = distance;
			}else{
				distanceMax = distance;
			}

			// Next guess is half way between
			distance = distanceMin + ((distanceMax - distanceMin) / 2);
		}

		// We now know the distance
		runner->speeds[l] = calcY(limb,distance);

#ifdef DEBUG
rprintf(",%d",speed);
#endif
	}	// next limb

#ifndef DEBUG
	// Set all the servo speeds in quick succession
	for(uint16_t l = 0; l < runner->num_actuators; l++){
		__ACTUATOR* servo = (__ACTUATOR*)pgm_read_word(&runner->actuators[l]);
		int16_t speed = (int16_t)(runner->speeds[l]) + (int16_t)(runner->delta[l]);
		speed = CLAMP(speed,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX);
		__act_setSpeed(servo,(DRIVE_SPEED)speed);
	}
#endif

	return gaitRunnerIsPlaying(runner);
}
Exemplo n.º 7
0
void OpenGLSceneGen::DrawMap()
{
    glLoadIdentity();
    glTranslatef(0.0f, 0.0f, -1); //restore

    float maxX = 0.49f;
    float maxY = 0.34f;

    float sizeX = .014f;
    float sizeY = .022f;

    int temp = 0;
    int h_MOD = 0;
    int W_MOD = 0;

    width_scr = 80;  //no scrolling
    height_scr = 39;

    /*int h_MOD=offset.y-height_scr/2; //Y-offset to centre character
    if(h_MOD < 0) h_MOD=0 ;
    else if(h_MOD > DUNGEON_SIZE_H-height_scr) h_MOD = DUNGEON_SIZE_H-height_scr;

    int W_MOD=offset.x-width_scr/2; //X-offset to centre character
    if(W_MOD < 0) W_MOD=0;
    else if(W_MOD > DUNGEON_SIZE_W-width_scr) W_MOD = DUNGEON_SIZE_W-width_scr;

    h_MOD = (h_MOD/5 * 5); //make screen move in steps (this calc works because it is using integers)
    W_MOD = (W_MOD/12 * 12);*/

    if (dLevel->getMapLight() == DungeonLevel::eNoFound)
         addShadows = false; 
    else
        addShadows = true; 

    freetype::prePrintChar(map_font);

    for (int h = 0; h < height_scr; h++)
    {
        for (int w = 0; w < width_scr; w++)
        {
            cell & currentCell = dLevel->getCell(w + W_MOD, h + h_MOD);

            /*if (World.GetCurrentLevel() == 0) // show all level
                ;
            else*/ if (!showAll && !currentCell.terrain.found) //don't show
                continue;

            //display extra symbol if lit
            if ((currentCell.getSymbol() && currentCell.terrain.light) || (currentCell.getSymbol() && showAll))
            {
                Symbol* symbol = currentCell.getSymbol();
                glColor3ub(symbol->mColour1, symbol->mColour2, symbol->mColour3);

                freetype::printChar(map_font, calcX(w), calcY(h), symbol->mSymbol);
            }
            //display creature if lit
            else if ((currentCell.GetMonster() && currentCell.terrain.light) || (currentCell.GetMonster() && showAll))
            {
                Monster* monster = currentCell.GetMonster();
                glColor3ub(monster->color1, monster->color2, monster->color3);

                freetype::printChar(map_font, calcX(w), calcY(h), monster->symbol);

                if (World.getMonsterManager().FindMonsterData(monster)->GetState() == asleep && World.getMonsterManager().FindMonsterData(monster)->Name() != "rotting ghoul")
                    freetype::printChar(map_font, calcX(w), calcY(h), '-');

            }
            //display items
            else if (currentCell.getItem() && (addShadows || currentCell.terrain.light))
            {
                Item * item = currentCell.getItem();
                glColor3ub(item->color1, item->color2, item->color3);
                
                // update colour on screen if better than item held
                if (!item->hasBrand() && !item->hasResistance())
                {
                    if(World.getMonsterManager().getMonsterItems().isBetter(*World.getMonsterManager().Player(), *item))
                        glColor3ub(0, 128, 255);
                }
                freetype::printChar(map_font, calcX(w), calcY(h), currentCell.getItem()->symbol);
            }
            else if (currentCell.terrain.light) //display terrain
            {
                glColor3ub((currentCell.terrain.color1), (currentCell.terrain.color2), (currentCell.terrain.color3));
                           
                //special water  effect
                if (currentCell.terrain.type == deepWater && currentCell.terrain.light) //make water flow
                {
                    int flow = World.GetTurns();

                    if ((h + flow) % 3 == 0)
                        glColor3ub(0, 64, 128);
                    else if (((h + flow) + 1) % 3 == 0)
                        glColor3ub(0, 12, 128);
                    else
                        glColor3ub(0, 128, 255);
                }
                freetype::printChar(map_font, calcX(w), calcY(h), currentCell.terrain.symbol);
                //freetype::printChar(map_font, 0 + (w*9.1f), (float)height_scr_offset - (h*14), '.');

            }
            else if (addShadows)//add night effect
            {
                glColor3ub((GLubyte)(currentCell.terrain.color1*shadowStrength),
                    (GLubyte)(currentCell.terrain.color2*shadowStrength),
                    (GLubyte)(currentCell.terrain.color3*shadowStrength));

                freetype::printChar(map_font, calcX(w), calcY(h), currentCell.terrain.symbol);
                //freetype::printChar(map_font, 0 + (w*9.1f), (float)height_scr_offset - (h*14.5f), currentCell.terrain.symbol);
            }
            //add extra characters
            if (currentCell.show_target != 0) //show target
            {
                glColor3ub(0xff, 0xff, 0xff);
                freetype::printChar(map_font, calcX(w), calcY(h), 0);
            }
            if (currentCell.show_path != none) //show target path (overlay)
            {
                if (currentCell.show_path == clear)
                    glColor3ub(0xff, 0xff, 0xff);
                else if (currentCell.show_path == blocked)
                    glColor3ub(0xff, 0, 0);
                freetype::printChar(map_font, calcX(w), calcY(h) , '*');
            }
        }
    }
    freetype::postPrintChar();
}
    void update(monitor_odometry_target_data &data, monitor_odometry_target_config config)
    {
        /* protected region user update on begin */
    	//Abstand des neue KS vom Roboter (bei Nullgeschwindigkeit).

//    	config.kindOf_primary_filter = 1;
//    	config.kindOf_secondary_filter = 5;
//    	config.kindOf_target_alignment = 0;
//    	config.noise_threshold = 0.02;
//    	config.mean_time_threshold = 2;
//    	config.vel_time_threshold = 2;
//    	config.vel_threshold = 0.05;
//    	config.kindOf_stagnancy_override = 0;
//    	config.mean_vel_threshold = 0.01;
//    	config.inner_radius = 1.0;
//    	config.outer_radius = 2.0;
//    	config.max_vel = 0.30;
//    	config.min_vel = 0.1;

    	double output_z = 0;
    	double radius_of_robot = 1;

    	ROS_INFO("********************* stage I ***********************");
    	/*
    	 * stage:						(I)
    	 * input odometry data:
    	 * velocity in x;
    	 * velocity in y;
    	 * header:
    	 * timestamp stamp;
    	 * frame_id;
    	 *
    	 */
    	x_ = data.in_odometry.twist.twist.linear.x;
    	y_ = data.in_odometry.twist.twist.linear.y;
    	header.frame_id = config.base_frame_id;
    	header.stamp = data.in_odometry.header.stamp;

    	/*
    	 * info:
    	 */
    	//TODO: delete this:
    	ROS_INFO("input:");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);
    	//end delete this;

//    	std::fstream f;
//    	f.open("testlog_x.dat", std::ios::out | std::ios::app);
//    	f << x_ << std::endl;
//    	f.close();
//
//    	f.open("testlog_y.dat", std::ios::out | std::ios::app);
//    	f << y_ << std::endl;
//    	f.close();

    	//TODO:
    	ROS_INFO("not good for performace but do anyway!");

    	velocity_ = calcAbsVec(x_, y_);
    	//TODO: just for now:
    	if (velocity_ > config.max_vel) {
    		ROS_INFO("ERROR vel > max_vel");
    	}

//    	velocity_ = calcVel(x_, y_, config.max_vel);
    	velList = updateVelList(velList, velocity_, header.stamp.toSec(), config.vel_time_threshold);
    	double meanVel = calcMeanVel(velList);

    	ROS_INFO("********************* stage II **********************");
    	/*
    	 * stage:						(II)
    	 * primary filters:
    	 */
    	if (config.kindOf_primary_filter == 0) {
    		ROS_INFO("no primary filter active ...");
//    		velocity_ = calcVel(x_, y_, config.max_vel);
    	}
    	else if (config.kindOf_primary_filter == 1) {
    		ROS_INFO("noise suppression filter active ...");
    		x_ = noiseSuppression(x_ , config.noise_threshold);
    		y_ = noiseSuppression(y_ , config.noise_threshold);
//    		velocity_ = calcVel(x_,y_, config.max_vel);
    	}
    	else if (config.kindOf_primary_filter == 2) {
    		ROS_INFO("velocity based suppression filter active ...");
//    		velocity_ = calcVel(x_,y_, config.max_vel);
    		if (velocity_ < config.vel_threshold) {
    			x_ = 0.0;
    			y_ = 0.0;
    			velocity_ = 0.0;
    		}
    	}
    	else if (config.kindOf_primary_filter == 3) {
    		ROS_INFO("combined suppression filter active ...");
    		x_ = noiseSuppression(x_ , config.noise_threshold);
			y_ = noiseSuppression(y_ , config.noise_threshold);
			velocity_ = calcVel(x_,y_, config.max_vel);
			if (velocity_ < config.vel_threshold) {
				x_ = 0.0;
				y_ = 0.0;
				velocity_ = 0.0;
				//TODO: Ist das nicht gefährlich??? Der Roboter könnte sich bewegen aber die Durchschnittsgeschw.
				// über 2-3 Sekunden trotzdem immer null sein???
			}
    	}
    	else
    		ROS_WARN("Error - 0001 - primary filter parameter out of bounds!");

    	ROS_INFO("after primary filters:");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);

    	//save velocity data
//    	std::fstream f;
//		f.open("testlog_velocity.dat", std::ios::out | std::ios::app);
//		f << velocity_ << std::endl;
//		f.close();


    	ROS_INFO("********************* stage III *********************");
    	/*
    	 * stage:						(III)
    	 * target alignment:
    	 */
    	//TODO:
    	if (config.kindOf_target_alignment == 0) {
    		ROS_INFO("no target alignment active ...");
    		z_ = (double) hight;
    	}
    	else {
    		//for all cases:
    		int overrideZ = checkForOverrideZ(config.min_vel, config.max_vel, velocity_);
    		if (config.kindOf_target_alignment == 1) {
				ROS_INFO("vertical target alignment active ...");
				z_ = (velocity_ / config.max_vel) * (double) hight;
			}
			else if (config.kindOf_target_alignment == 2) {
				ROS_INFO("linear target alignment active ...");
				z_ = (velocity_ / config.max_vel) * (double) hight;
				d_ = z_ * ((config.outer_radius - config.inner_radius) / (double) hight);
			}
			else if (config.kindOf_target_alignment == 3) {
				ROS_INFO("elliptical target aligment active ...");
				z_ = -1 * sqrt( 1 - (((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
				d_ = (velocity_ / config.max_vel) * (config.outer_radius - config.inner_radius);
			}
			else if (config.kindOf_target_alignment == 4) {
				ROS_INFO("elliptical target alignment with mean velocity active...");
				z_ = -1 * sqrt( 1 - (((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
				d_ = (meanVel / config.max_vel) * (config.outer_radius - config.inner_radius);
			}
			else {
				ROS_WARN("Error - 0002 - target alignment parameter out of bounds!");
			}

    		ROS_INFO("OVERRIDE BEFORE IF_ = %u",overrideZ);
    		if (overrideZ == 0) {
    			z_ = 0.0;
    			ROS_INFO("OVI 0");
    		}
    		else if (overrideZ == 1) {
    			z_ = (double) hight;
    			ROS_INFO("OVI 1=hight");
    		}
    		else if (overrideZ == 3) {
    			ROS_ERROR("Error - 0003 - override parameter out of bounds!");
    		}
    		else {
    			ROS_INFO("OVI wahrscheinlich 2 also nix tun!");
    		}

    	}


    	/*
    	 * stage:						(IV)
    	 * secondary filters:
    	 */
    	ROS_INFO("********************* stage IV **********************");
    	if (config.kindOf_secondary_filter == 0) {
    		ROS_INFO("no secondary filter active ...");
    	}
    	else if (config.kindOf_secondary_filter == 1) {
    		ROS_INFO("mean filter active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanXY = calcMeanXY(list_of_values_xystamp_);
    		x_ = meanXY.at(0);
    		y_ = meanXY.at(1);
//    		ROS_INFO("Anzahl Elemente: %lu", list_of_values_xystamp_.size());
//    		ROS_INFO("meanX %f und meanY %f", meanXY.at(0), meanXY.at(1));
    	}
    	else if (config.kindOf_secondary_filter == 2) {
    		ROS_INFO("median filter active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> medianXY = calcMedianXY(list_of_values_xystamp_);
    		x_ = medianXY.at(0);
    		y_ = medianXY.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 3) {
    		ROS_INFO("quantified mean filter slope active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanSlope = calcQuantifiedMeanSlope(list_of_values_xystamp_);
    		x_ = meanSlope.at(0);
    		y_ = meanSlope.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 4) {
    		ROS_INFO("quantified mean filter stairs active ...");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanStairs = calcQuantifiedMeanStairs(list_of_values_xystamp_);
    		x_ = meanStairs.at(0);
    		y_ = meanStairs.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 5) {
    		ROS_INFO("quantified mean filter square active");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> meanSquare = calcQuantifiedMeanSquare(list_of_values_xystamp_);
    		x_ = meanSquare.at(0);
    		y_ = meanSquare.at(1);
    	}
    	else if (config.kindOf_secondary_filter == 6) {
    		ROS_INFO("quantified median filter slope active");
    		list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
    		std::vector<double> medianSlope = calcQuantifiedMedianSlope(list_of_values_xystamp_);
    		x_ = medianSlope.at(0);
    		y_ = medianSlope.at(1);
    	}
    	else
    		ROS_WARN("Error - 0003 - secondary filter parameter out of bounds!");

    	ROS_INFO("after secondary filters:");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);

    	/*
    	 * stage:					(V)
    	 * output production:
    	 */
//    	yaw_ = noZeroArcTan(y_,x_);

    	//for stag test:
    	//listOfValuesForMeanYaw_ = updateListOfDouble(listOfValuesForMeanYaw_, yaw_, 100);



    	//end of stag test
//    	yaw_ = noZeroArcTan(y_,x_);

    	/*
    	 * stage:					(V)
    	 * stagnancy override:
    	 */
    	ROS_INFO("********************* stage V ***********************");
    	if (config.kindOf_stagnancy_override == 0) {
    		ROS_INFO("no stagnancyoverride active ...");
    	}
    	else if (config.kindOf_stagnancy_override == 1) {
    		ROS_INFO("stagnancy override active ...");
//    		velList = updateVelList(velList, velocity_, header.stamp.toSec());
//    		double meanVel = calcMeanVel(velList);
    		if (meanVel < config.mean_vel_threshold) {
    			stagnancy_override_counter++;
    			z_ = (double) hight;
    			d_ = 0.0;
    			//wieder zurück drehen???
    		}
    	}
    	else if (config.kindOf_stagnancy_override == 2) {
    		ROS_INFO("stagnancy override: twist back torso");
    		if (meanVel < config.mean_vel_threshold) {
//    			yaw_ = calcMean(listOfValuesForMeanYaw_);
//    			ROS_INFO("Mean yaw_ %f",yaw_);
    		}
    	}
    	else
    		ROS_ERROR("Error - 0004 - stagnancy override parameter out of bounds!");

    	/*
    	 * stage:					(VI)
    	 * output calculator:
    	 */
    	ROS_INFO("********************* stage VI **********************");
    	yaw_ = newArcTan(x_,y_);
//    	ROS_INFO("d_ = %f",d_);
    	x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
    	y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);
//    	x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
//		y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);

    	ROS_INFO("Output: ---------------------");
    	ROS_INFO("x_ = %f",x_);
    	ROS_INFO("y_ = %f",y_);
    	ROS_INFO("yaw_ = %f",yaw_);
    	ROS_INFO("END ....................");
    	ROS_INFO("Schwellwert: %f", config.noise_threshold);
    	if (config.testbool) {
    		ROS_INFO("testbool is true");
    	}
    	else {
    		ROS_INFO("testbool is false");
    	}


    	/*
    	 * stage:					(VII)
    	 * tf
    	 */
    	ROS_INFO("********************* stage VII *********************");
    	frame_transform_output(tf::Vector3(x_,y_,z_));
    	stamped_transform_output.stamp_ = header.stamp;
    	stamped_transform_output.frame_id_ = config.base_frame_id;
    	stamped_transform_output.child_frame_id_ = config.lookat_frame_id;
    	tf::Quaternion q;
    	q.setRPY(0, 0, yaw_);
    	stamped_transform_output.setRotation(q);
    	stamped_transform_output.setOrigin(tf::Vector3(x_,y_,z_));
    	static tf::TransformBroadcaster br;
    	br.sendTransform(stamped_transform_output);

//    	/*
//    	 * evaluations:
//    	 */
//    	if (active_) {
//        	tf::Vector3 newOrigin;
//        	newOrigin.setX(x_);
//        	newOrigin.setY(y_);
//        	newOrigin.setZ(z_);
//        	double distance = calcEuklidianDistance(newOrigin,oldOrigin);
//        	quickAndDirty++;
//        	//Problem: Sprung von Null auf den Startwert wieder löschen!
//        	if (quickAndDirty > 4 ) {
//        		distances_list.push_back(distance);
//        	}
//
//    		double max_value = calcMax(distances_list);
//    		double sum_value = calcSum(distances_list);
//    		ROS_INFO("+++++++++++++++++++++++++++++++++++++++");
//    		ROS_INFO("Auswertung: ");/
//        	ROS_INFO("Distance = %f",distance);
//        	ROS_INFO("MAX = %f ", max_value);
//        	ROS_INFO("SUM = %f", sum_value);
//    //    	showList(distances_list);
//        	oldOrigin = newOrigin;
//
//        	//TODO:
//        	//eval: yaw:
//        	double yawDistance = calcYawDistance(yaw_, oldYaw);
//        	if (quickAndDirty > 4) {
//        		yaw_distances_list.push_back(yawDistance);
//        	}
//        	double yaw_max_value = calcMax(yaw_distances_list);
//        	double yaw_sum_value = calcSum(yaw_distances_list);
//        	ROS_INFO("++++++++++++++++++++++++++++++++++++++++");
//        	ROS_INFO("YawDistance = %f", yawDistance);
//        	ROS_INFO("MAX YAW = %f", yaw_max_value);
//        	ROS_INFO("SUM YAW = %f ", yaw_sum_value);
//        	oldYaw = yaw_;
//
//        	ROS_INFO("stagnacy_override_counter = %u", stagnancy_override_counter);
//    	}

        /* protected region user update end */
    }
Exemplo n.º 9
0
int main(void)
{
    // C MUST BE PRIME to ensure that it is co-prime with other values
	const BYTE C = 0x07;
    
    // D MUST be assigned the same value as Y
	BYTE D = 0x00;

    // Variable for the X, Y, etc. values calculated by the recipient
    BYTE calculated_X = 0x00;
    BYTE calculated_Y = 0x00;
    BYTE calculated_Q = 0x00;
    BYTE calculated_R = 0x00;
    BYTE calculated_Z = 0x00;
    BYTE calculated_Zprime = 0x00;
	
    bool root_found = false;
    
    BYTE Q = 0x00;
    BYTE R = 0x00;
    
    BYTE Z1 = 0x00;
    BYTE Z2 = 0x00;
    
    BYTE sender_Z1prime = 0x00;
    BYTE sender_Z2prime = 0x00;
    
    BYTE recipient_Zprime = 0x00;
    
    // Iterate through every possible combination of the X and Y byte values to verify
    // that it is possible to recover X and Y from Z given ANY combinations of values
    for (BYTE X = 0x00; X < 0x10; ++X)
    {
        // For each value of X iterate for each value of Y incremented by 0x01
        for (BYTE Y = 0x00; Y < 0x10; ++Y)
        {
            /**
             * Simulate the sender compressing the 2 bytes into a single byte, Z and
             * then sending Z to the recipient
             */
            D = Y;  // D MUST be same value as Y, D is not known to recipient
            Q = calcQ(&X, &Y, &C);
            R = calcR(&X, &Y, &D);
            
            // Calculate Z using both equivalent methods, Z = R XOR C = Q XOR D
            Z1 = calcZ(&R, &C);
            Z2 = calcZ(&Q, &D);
            
            // Calcualte Z' using both equivalent methods, Z' = R XNOR C = Q XNOR D
            sender_Z1prime = calcZprime(&R, &C);
            sender_Z2prime = calcZprime(&Q, &D);
            
            // Verify that Z using both methods is equivalent (This is an AXIOM OF THE
            // COMPRESSION ALGORITHM) so if it's wrong we have a major flaw
            if (Z1 != Z2)
            {
                printf("Z1 NOT EQUIVALENT TO Z2!\n");
                printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
                printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
                printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
                printf("Z1:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
                printf("Z2:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z2)), 0xF0 ^ Z2);
            }
            
            // Verify that Z' using both methods is equivalent (This is an AXIOM OF THE
            // COMPRESSION ALGORITHM) so if it's wrong we have a major flaw           
            if (sender_Z1prime != sender_Z2prime)
            {
                printf("Z1 PRIME NOT EQUIVALENT TO Z2 PRIME!\n");
                printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
                printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
                printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
                printf("Z1 PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ sender_Z1prime)), 0xF0 ^ sender_Z1prime);
                printf("Z2 PRIME:\t\t"BYTETOBINARYPATTERN", %d\n\n", BYTETOBINARY((0xF0 ^ sender_Z2prime)), 0xF0 ^ sender_Z2prime);            
            }
            
               
            /**
             * Simulate the recipient receiving a single byte, Z and getting the original
             * bytes X and Y from Z, the recipient knows the constant value C, which is prime
             */
             // The user can get Z' which is ~Z, this is the UNIQUE property that makes it 
             // possible to recover X and Y by using the equations of for Z and Z' given the
             // two values Z and Z' = ~Z
             recipient_Zprime = ~(Z1);
             
             // Verify that the Zprime the recipient gets from ~Z is ACTUALLY, the same Zprime
             // that the sender calculated using the equations (This is an AXIOM OF THE
             // COMPRESSION ALGORITHM), so if it's wrong we have a major flaw
            if (recipient_Zprime != sender_Z1prime)
            {
                printf("RECIPIENT'S Z PRIME NOT EQUIVALENT TO SENDER'S Z PRIME!!!\n");
                printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
                printf("Q:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
                printf("R:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
                printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
                printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
            }

            // Now the user can calculate X very easily given a unique property when C is prime and D == Y
            // The user has RECEIVED Z and BOTH recipient and sender KNOW C, UNKNOWN is Y == D
            calculated_X = calcX(&Z1, &C);
            
            // Verify that the CALCULATED X IS ACTUALLY EQUAL to the Sender's X value the recipients' 
            // calculated X should ALWAYS be equivalent to the sender's X when Y == D (This is an AXIOM OF THE
            // COMPRESSION ALGORITHM), so if it's wrong we have a major flaw
            if (calculated_X != X)
            {
                printf("RECIPIENT'S CALCULATED X NOT EQUIVALENT TO SENDER'S X!!!\n");
                printf("Q:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
                printf("R:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
                printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
                printf("RECIPIENT X:\t"BYTETOBINARYPATTERN"\t\t: %d\n\n", BYTETOBINARY(calculated_X), calculated_X);
            }
            
            /**
            calculated_Y = 0x00;
            calculated_Q = 0x00;
            calculated_R = 0x00;
            calculated_Z = 0x00;
            calculated_Zprime = 0x00;
            root_found = false;
            
            calculated_Q = calcQ(&calculated_X, &Y, &C);
            calculated_R = calcR(&calculated_X, &Y, &Y);

            calculated_Z = calcZ(&calculated_R, &C);
            calculated_Zprime = calcZprime(&calculated_Q, &Y);
            
            calculated_Y = calcY(&calculated_Zprime, &calculated_Q);

            printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
            printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
            printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
            printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
            printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
            printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
            printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
            printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
            printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
            printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
            printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n\n", BYTETOBINARY(calculated_X), 
                    BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
            */
            // Now solve for the unknown value Y, given the Z' value, X, and C, the only way to feasibly solve
            // for Y is to perform a bruteforce search given the known values and the equations for
            // Z' and Q. THERE MUST ONLY BE ONE ROOT, Y, GIVEN THE ROOT X , Z' and C (This is an AXIOM OF THE
            // COMPRESSION ALGORITHM), so if it's wrong we have a major flaw, in fact its IMPOSSIBLE to
            // reverse the compression unless Y == D is UNIQUE!!!!
            for (BYTE Y_TEST = 0x00; Y_TEST < 0x10; ++Y_TEST)
            {
                calculated_Q = calcQ(&calculated_X, &Y_TEST, &C);
                calculated_R = calcR(&calculated_X, &Y_TEST, &Y_TEST);
                
                calculated_Z = calcZ(&calculated_R, &C);
                calculated_Zprime = calcZprime(&calculated_Q, &Y_TEST);
                
                //printf("Calculated X: %d\n", calculated_X);
                //printf("Calculated Y: %d\n", Y_TEST);
                //printf("Calculated Q: %d\n", 0xF0 ^ calculated_Q);
                //printf("Calculated R: %d\n", 0xF0 ^ calculated_R);
                //printf("Calculated Z: %d\n",  0xF0 ^ calculated_Z);
                //printf("Calculated Z PRIME: %d\n\n", calculated_Zprime);
                
                /**
                printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
                printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
                printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
                printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
                printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
                printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
                printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
                printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
                printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
                printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
                printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n\n", BYTETOBINARY(calculated_X), 
                    BYTETOBINARY(Y_TEST), calculated_X, Y_TEST);
                */
                
                // If the calculated Z' given the calculated root Y matches the known Z' which is 
                // Z' = ~Z, then the root Y satisfies the equations and IS THE ROOT. Remember that
                // for the sender as well as the recipient Y == D, this is the unique property that
                // makes it possible to avoid the 0000/1111 cancellations of XOR              
                if (calculated_X == calcX(&calculated_Z, &C))
                {
                    if (Y_TEST == calcY(&calculated_Zprime, &calculated_Q))
                    {
                        if (Z1 == calculated_Z && recipient_Zprime == calculated_Zprime)
                        {
                            // X XOR Y SHOULD NOT BE EQUIVALENT TO Z, ONLY THE UNIQUE ROOT IS NOT A COMBINATION
                            // OF X XOR Y = Z
                            //if (Y_TEST == Y)
                            if ((calculated_X ^ Y_TEST) != calculated_Z)
                            {
                                calculated_Y = Y_TEST;
                                root_found = true;
                                break;
                            }
                        }
                    }
                }
            }
            // If the correct root is found I will be DELIGHTED, the calculated Y MUST
            // be UNIQUE and equivalent to the initial Y value of the sender, if it does
            // not match we have a major flaw,
            if (calculated_Y == Y)
            {
                printf("RECIPIENT'S CALCULATED Y EQUIVALENT TO SENDER'S Y!!!!!!!!!!!!!!!!!!!!\n");
                printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
                printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
                printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
                printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
                printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
                printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
                printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
                printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
                printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
                printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
                printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n", BYTETOBINARY(calculated_X), 
                    BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
            }
            // major flaw, back to the drawing board!
            else
            {
                printf("RECIPIENT'S CALCULATED Y NOT EQUIVALENT TO SENDER'S Y!!!!!!!!!!!!!!!!!!!!\n");
                printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
                printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
                printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
                printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
                printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
                printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
                printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
                printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
                printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
                printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
                printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n", BYTETOBINARY(calculated_X), 
                    BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
            }
        }
            /**
            Zprime = calcZprime(&Q, &D);
            
            calculated_Y = calcY(&Zprime, &Q);

            if ( X == calculated_X && Y == calculated_Y)
            {
            printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q); 
            printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
            printf("Z1:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
            printf("Z2:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z2)), 0xF0 ^ Z2);

            );
            //printf("Z':\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Zprime), Zprime);

            printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Q), Q); 
            printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(R), R);
            printf("Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Z), Z);
            printf("Z':\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Zprime), Zprime);

            printf("INITIAL X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
            printf("SOLVED X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n", 
            BYTETOBINARY(calculated_X), BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
            }*/
    }
	return 0;
}