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);
}
// 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;
}
}
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];
}
}
}
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;
}
