-
Notifications
You must be signed in to change notification settings - Fork 1
/
ru_turn.c
90 lines (67 loc) · 1.48 KB
/
ru_turn.c
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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#include "ru_turn.h"
#include "ru_drive.h"
#include "ru_encoder.h"
#include "ru_config.h"
typedef enum {
TURNING,
DONE
} turn_state_t;
static turn_state_t turn_state;
char turn_done;
// ----------------------------------------------------------
// Parameters
double target_amount;
// ----------------------------------------------------------
static void Turning_Init(void);
static void Turning_Routine(void);
static void Done(void);
// ----------------------------------------------------------
void Turn_Meta_Init(double amount_rad)
{
target_amount = amount_rad;
Turning_Init();
turn_done = 0;
}
void Turn_Meta_Routine(void)
{
switch (turn_state) {
case TURNING: Turning_Routine(); break;
case DONE: return;
}
}
static void Done(void)
{
turn_state = DONE;
turn_done = 1;
}
// ----------------------------------------------------------
static float angle_at_start;
static float target_angle;
static void Turning_Init(void)
{
turn_state = TURNING;
angle_at_start = Encoder_Get_Angle();
target_angle = angle_at_start + target_amount;
}
static void Turning_Routine(void)
{
float angle;
angle = Encoder_Get_Angle();
printf("%d --> %d\n",
(int) (angle * 180.0 / PI),
(int) (target_angle * 180.0 / PI) );
if (angle <= target_angle && target_amount <= 0) {
Done();
return;
}
if (angle >= target_angle && target_amount >= 0) {
Done();
return;
}
if (target_amount < 0) {
Set_Auto_Tank_Drive(+127, -127);
}
else {
Set_Auto_Tank_Drive(-127, +127);
}
}