-
Notifications
You must be signed in to change notification settings - Fork 1
/
drive.c
100 lines (79 loc) · 2.14 KB
/
drive.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
91
92
93
94
95
96
97
98
99
100
#include "drive.h"
#include "sine.h"
#include "pwm.h"
#include "pid.h"
#include "encoder.h"
#define VF_SLOPE 938
#define VF_MAX_AMPLITUDE 1000
#define VF_MIN_AMPLITUDE 100
/*static*/ unsigned int abs(int x)
{
return (x < 0) ? (unsigned int) (-x) : (unsigned int) x;
}
/*static*/ unsigned int is_negative(int x)
{
return (x < 0) ? 1 : 0;
}
/*static*/ unsigned int vf_control(int frequency)
{
unsigned int amplitude;
frequency = abs(frequency);
/* Vf law */
amplitude = frequency * VF_SLOPE / 256;
/* Amplitude saturation */
if (amplitude > VF_MAX_AMPLITUDE)
{
amplitude = VF_MAX_AMPLITUDE;
}
else if (amplitude < VF_MIN_AMPLITUDE)
{
amplitude = VF_MIN_AMPLITUDE;
}
return amplitude;
}
int open_loop_acceleration(int reference, int acceleration)
{
static int current;
if (current == reference)
return current;
if (abs(current - reference) < acceleration) {
current = reference;
return current;
}
if (current > reference)
{
current -= acceleration;
} else {
current += acceleration;
}
return current;
}
void drive_open_loop(open_loop_params_t *params)
{
int ref_frequency = params->frequency;
int acceleration = params->acceleration;
int frequency = open_loop_acceleration(ref_frequency, acceleration);
sine_param_t sine_params;
sine_params.direction = is_negative(frequency);
sine_params.amplitude_pwm = vf_control(abs(frequency));
sine_params.freq_m = abs(frequency);
sine_set_params(&sine_params);
}
void drive_halt(void)
{
pwm_output_disable();
sine_reset();
}
//void drive_close_loop(int ref_speed)
//{
// int measure = encoder_get_speed();
//
// int pid_output = pid_controller(ref_speed, measure);
//
// sine_param_t sine_params;
// sine_params.direction = is_negative(pid_output);
// sine_params.amplitude_pwm = vf_control(abs(pid_output));
// sine_params.freq_m = abs(pid_output);
//
// sine_set_params(&sine_params);
//}