Svenska ElektronikForumet

Kod: Markera allt

/* ARCTAN för vinklar mellan 0 och 90 grader. */
uint8_t ARCTAN(uint16_t x, uint16_t y)
{
	uint32_t	kvot;
	uint8_t	vinkel;

	kvot=y*256;
	kvot/=x;
	
	for (vinkel=0; vinkel<90; vinkel++)
	{
		if ( (uint16_t)kvot < tableTAN256[vinkel]) break;
	}
	
	return vinkel;
}

/* Tabell med värdena tan(v*PI/180)*256 där v = 0.5, 1.5, 2.5 ... 89.5 */
static const uint16_t PROGMEM tableTAN256[90] = {
2,
6,
11,
15,
20,
24,
29,
33,
38,
42,
47,
52,
56,
61,
66,
70,
75,
80,
85,
90,
95,
100,
106,
111,
116,
122,
127,
133,
138,
144,
150,
156,
163,
169,
175,
182,
189,
196,
203,
211,
218,
226,
234,
242,
251,
260,
269,
279,
289,
299,
310,
321,
333,
345,
358,
372,
386,
401,
417,
434,
452,
471,
491,
513,
536,
561,
588,
618,
649,
684,
722,
765,
811,
864,
923,
989,
1066,
1154,
1258,
1381,
1529,
1712,
1944,
2246,
2658,
3252,
4185,
5863,
9776,
29334
};

Kod: Markera allt

struct coordFloat {
  float x, y, z;
};

struct coordFloat axisX = {1, 0, 0}, axisY = {0, 1, 0}, axisZ = {0, 0, 1};

float accAngleCalc(struct coordFloat a, struct coordFloat b, float c) {
  return( (c < 0) ? 180.0 - vectAngle(a, b) : vectAngle(a, b));
}

float vectAngle(struct coordFloat a, struct coordFloat b) {
  float c;
  c = vectDotProduct(a, b) / (vectMagnitude(a) * vectMagnitude(b));
  return(atan(sqrt( (1 - sq(c)) / sq(c) )) * 180 / PI);
}

void vectCrossProduct(struct coordFloat a, struct coordFloat b, struct coordFloat *c) {
  c->x = a.y * b.z - a.z * b.y;
  c->y = a.z * b.x - a.x * b.z;
  c->z = a.x * b.y - a.y * b.x;
}

float vectDotProduct(struct coordFloat a, struct coordFloat b) {
  return(a.x * b.x + a.y * b.y + a.z * b.z);
}

float vectMagnitude(struct coordFloat a) {
  return(sqrt(sq(a.x) + sq(a.y) + sq(a.z)));
}