blob: 2607001868fd6630428cf6dc3dcc41ab929f2eb5 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
|
#ifndef _TOOLS_H
#define _TOOLS_H
#include <stdlib.h>
#include <math.h>
#include "defines.h"
#ifndef new
void* operator new(size_t sz);
void operator delete(void* p);
#endif
#define max(a, b) (((a) > (b)) ? (a) : (b))
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define sign(a) (((a) < 0) ? (-1) : (1))
inline float easyAngle(float angle)
{
while(angle < 0)
{
angle += 2.0f * PI;
}
while(angle >= 2.0f * PI)
{
angle -= 2.0f * PI;
}
return angle;
}
inline void sleep(uint16 sec)
{
for (uint16 s = 0; s < sec; s++) {
for (uint32 i = 0; i < 1405678; i++) {
asm volatile("nop");
}
}
};
inline void msleep(uint16 msec)
{
for (uint16 s = 0; s < msec; s++) {
for (uint16 i = 0; i < 1405; i++) {
asm volatile("nop");
}
}
};
inline void usleep(uint16 usec)
{
for (uint16 s = 0; s < usec; s++) {
for (uint8 i = 0; i < 3; i++) {
asm volatile("nop");
}
}
};
inline float distance2d(float x1, float y1, float x2, float y2)
{
return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
}
inline float direction2d(float x1, float y1, float x2, float y2)
{
return atan2((y2 - y1), (x2 - x1));
}
#endif
|
