Exemplo n.º 1
0
bool intersects(SkRect tRect, float tRot, TextDrawInfo* s)
{
	float sRot = s->pathRotate;
	if (absFloat(tRot) < M_PI / 15 && absFloat(sRot) < M_PI / 15) {
		return SkRect::Intersects(tRect, s->bounds);
	}
	float dist = sqrt(sqr(tRect.centerX() - s->bounds.centerX()) + sqr(tRect.centerY() - s->bounds.centerY()));
	if(dist < 3) {
		return true;
	}
	SkRect sRect = s->bounds;

	// difference close to 90/270 degrees
	if(absFloat(cos(tRot-sRot)) < 0.3 ){
		// rotate one rectangle to 90 degrees
		tRot += M_PI_2;
		tRect = SkRect::MakeXYWH(tRect.centerX() -  tRect.height() / 2, tRect.centerY() -  tRect.width() / 2,
				tRect.height(), tRect.width());
	}

	// determine difference close to 180/0 degrees
	if(absFloat(sin(tRot-sRot)) < 0.3){
		// rotate t box
		// (calculate offset for t center suppose we rotate around s center)
		float diff = atan2(tRect.centerY() - sRect.centerY(), tRect.centerX() - sRect.centerX());
		diff -= sRot;
		float left = sRect.centerX() + dist* cos(diff) - tRect.width()/2;
		float top = sRect.centerY() - dist* sin(diff) - tRect.height()/2;
		SkRect nRect = SkRect::MakeXYWH(left, top, tRect.width(), tRect.height());
		return SkRect::Intersects(nRect, sRect);
	}

	// TODO other cases not covered
	return SkRect::Intersects(tRect, sRect);
}
Exemplo n.º 2
0
// remodifier la valeur de la PWM
void UpdateGrandeVoile(){
	// rapports PWM pour les positions max du servo moteur
	float rapport_max = 0.925f;
	float rapport_min = 0.82f;
	
	float angle_girouette, nouveau_rapport ;
	
	angle_girouette = absFloat(GetAngle());
	
	//Si angle_girouette compris entre 45 et 180° -> (theta - 45) / 135
	//Si angle_girouette compris entre 0 et 45 		-> 0
	if (angle_girouette < 45.0 ){
		nouveau_rapport = 0.0 ;
	}	else {
		nouveau_rapport = (angle_girouette - 45.0) / 135.0;
	}
	
	if (nouveau_rapport > 1.0)
		nouveau_rapport = 1.0;
	
	nouveau_rapport = (nouveau_rapport * (rapport_max-rapport_min)) + rapport_min;
	
	//changement angle servo : modification de la période PWM
	update_PWM(TIM_SERVO, nouveau_rapport, voie);
}
float compassDif(float one, float two)
{
  float highVal=one+360;
  float lowVal=one-360;
  float normalDifference=absFloat(two-one);
  float highDifference=absFloat(two-highVal);
  float lowDifference=absFloat(two-lowVal);
  if (lowDifference<normalDifference && lowDifference<highDifference)
  {
    return lowDifference;
  }
  else if (normalDifference<highDifference)
  {
    return normalDifference;
  }
  else
  {
    return highDifference;
  }
}
Exemplo n.º 4
0
void pulse() {
	const signed char bitesize = 30;
	for (unsigned char i=0; i<NUMCH; i++) {
		float rnd = myRand();
		speed[i] += 20*(rnd - .5);	
		if (rnd < 0.01) {
			speed[i] *= 1.1;
		} else { 
			speed[i] = speed[i] * 0.99;
		}
		if (rnd < 0.5 && rnd > 0.4993) { //every 10000 or so, add bias of up to 10000
			buffA[i] = myRand() * 60000 - 30000;
		}
		if (rnd < 0.6 && rnd > 0.599) {
			speed[i] = -speed[i];
		}
			
		
		if (absFloat(speed[i]) > SCHARMAX) {
			speed[i] *= 0.5;
		}
		signed char dBright = myRound(speed[i]);
		if (buffA[i] != 0 && dBright <= SCHARMAX-bitesize && dBright >= -SCHARMAX+bitesize) {
			if (buffA[i] < 0) {
				dBright-=10;
				buffA[i]+=10;
			} else {
				dBright+=10;
				buffA[i]-=10;
			}
			if (absFloat(buffA[i]) < 10) {
				buffA[i] = 0;
			}
		}
		signed char bndRes = boundShort(&brightness[i], &dBright);
		if (bndRes != 0) {
			speed[i] = -.5*speed[i];
		}
	}

}
Exemplo n.º 5
0
float sqrtLocal(float num)
{
	// if input num is 0, return 0
	if (num == 0)
		return 0;

	/*
		Setup the initial estimation, refer wikipedia's Rough estimation:
		https://en.wikipedia.org/wiki/Methods_of_computing_square_roots
		
		Suppose: 
		sqrt(S) = sqrt(a) * 10^n
		sqrt(S) = { 2 * 10^n, if a < 10
		            6 * 10^n, if a >= 10 }

		Since for this assignment, we always calculate the sqrt of random
		number between 0 ~ 3, thus a < 10, and n = 0
		Hence the initial value would be set as 2 * 10^0 = 2
	*/

	// setup the record for last generation and current generation value
	float numLast = 2, numCurrent;
	int curGeneration = 0;
	while (1)
	{
		// record the loop count
		++count;
		++curGeneration;
		// apply Newton's method to calculate the sqrt value
		numCurrent = (numLast + (num / numLast)) / 2;
		// judge if the accuracy is enough, 10^-4 = 0.0001
		if (absFloat(numCurrent - numLast) <= 0.0001)
		{
			if (curGeneration > maxGeneration)
			{
				// record the max generation
				maxGeneration = curGeneration;
			}
			return numCurrent;
		}
		// put the current value as last value to prepare for next loop
		numLast = numCurrent;
	}
}
int main(void)
{  
	int i = 0;
	for(; i<8; i++) {
		sensors[i] = 0;
	}

	//Needs to be before sei.. don't ask why..
	//Also needs a delay from start up..
	_delay_ms(1000);
	InitLCD();

	sei();
	SETUP_SENSOR_IO();

	CLEAR_BIT(DDRE, PE5);//Input pin 
	
	ENABLE_EXTERNAL_INTERRUPT(INT5);//External Interrupt Request 5 INT5 enabled*/

	InitServo(0);
	InitMotor();	
	SetupTachometer();
	float currentDirection = 0;
	float targetDirection = 0;

	bool terminated = false;
	currentSpeed = 10;
	tachoPulsesPerSecond = 0;
	for (;;)
	{	

		if(updateScreen) {
			updateScreen = false;
			ClearScreen();
			WriteText("State:",1);
			WriteText_StartingFrom(status,2,7);
			WriteText("Lap:",3);
			WriteText_StartingFrom(lap,4,7);

			WriteText("Ticker:",5);
			WriteText_StartingFrom(time,6,7);

			WriteText("cycle:",7);
			WriteText_StartingFrom(cyc,8,7);
			
		}

		//Terminated when lapCounter > 3
		if(!terminated) {		

			float direction = GetFilteredSensorValue();

			if (lapCounter > 3) {			
				SetMotorSpeed(0);
				MoveServo(0);

				terminated = false;
				status = "Terminated";
				updateScreen = true;
				driveFlag = false;
				lapCounter = 0;
				continue;

			}
		
			targetDirection = direction * 45.0;
			if (absFloat(targetDirection) >= absFloat(currentDirection)) {
				currentDirection = targetDirection;				
			}
			else {
				currentDirection += (targetDirection - currentDirection);
			}

			MoveServo(currentDirection);
			
			if (driveFlag) {
				//Calculate new motor speed and accelerate/deccelerate
				float desiredPulsePerSecond = 9 + (1.0 - absFloat(direction)) * 5;
				float difference = desiredPulsePerSecond - tachoPulsesPerSecond;
				float coefficient = 0.015;
				if (difference < 0) {
					coefficient = -0.015;
				}
				currentSpeed += 20 * coefficient;

				if(currentSpeed>80) {
					currentSpeed = 80;
				}
				if(currentSpeed<0) {
					currentSpeed = 0;
				}

				SetMotorSpeed(currentSpeed);	
			}
		}
	}
	return 0;
}