-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathServoTester.vhd
170 lines (104 loc) · 5.76 KB
/
ServoTester.vhd
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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
--subtype elements is std_logic_vector(15 downto 0);
--type 16bit_array is array (0 to 127) of elements;
--signal arr : 16bit_array ;
library IEEE;
use IEEE.std_logic_1164.all;
use IEEE.numeric_std.all;
entity ServoTester is
port (
CLOCK_50 : in std_logic;
SW : in std_logic_vector(3 downto 0);
LED : out std_logic_vector(7 downto 0);
GPIO_0 : out std_logic_vector(33 downto 0);
GPIO_1 : out std_logic_vector(33 downto 0)
);
end ServoTester;
architecture ServoTester_a of ServoTester is
--type vector_2 is array (natural range <>, natural range <>) of std_logic;
signal s_RESET : std_logic;
signal s_pwmSignal : std_logic_vector(7 downto 0);
signal s_activateServo : std_logic_vector(7 downto 0);
signal s_TL, s_TR : std_logic;
signal s_debouncedGPIO1 : std_logic_vector(31 downto 0);
signal s_LUT : std_logic_vector(9 downto 0);
--signal s_LUT : vector_2(7 downto 0, 9 downto 0); -- (servo ID, bit index)
signal s_dutycycle : std_logic_vector(9 downto 0);
--signal s_dutycycle : vector_2(7 downto 0, 9 downto 0); -- (servo ID, bit index)
signal s_TICK : std_logic;
signal s_counter : std_logic_vector(11 downto 0);
--signal s_counter : vector_2(7 downto 0, 11 downto 0); -- (servo ID, bit index)
--signal s_centerCorrection : integer;
begin
--@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@--
--========================================================================--
-- SIGNALS --
--========================================================================--
--@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@--
s_RESET <= '1';
--s_dutycycle <= "0111111111";
s_activateServo <= "11111111";
--========================================================================--
-- servo output --
--========================================================================--
GPIO_1(6) <= (SW(0) XOR (s_pwmSignal(0) AND s_activateServo(0))); -- S1
GPIO_0(24) <= (SW(0) XOR (s_pwmSignal(1) AND s_activateServo(1))); -- S2
LED(0) <= (SW(0) XOR (s_pwmSignal(0) AND s_activateServo(0))); -- S1
LED(1) <= (SW(0) XOR (s_pwmSignal(1) AND s_activateServo(1))); -- S2
LED(2) <= '1';
LED(3) <= '0';
LED(4) <= '1';
LED(5) <= '0';
LED(6) <= '1';
LED(7) <= '0';
--50000
--s_centerCorrection <= (to_integer(signed(SW)) * 1000);
--@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@--
--========================================================================--
-- SUPPORTING COMPONENTS --
--========================================================================--
--@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@--
--========================================================================--
-- PRESCALER --
--========================================================================--
prescaler: entity work.prescaler(prescaler_a) generic map (scale => 40000)
port map (CLK => CLOCK_50,
RESET_n => s_RESET,
TICK => s_TICK
);
--========================================================================--
-- COUNTERS --
--========================================================================--
counter_1: entity work.counter(counter_a) generic map (max_g => 4096, -- maximum number
outBit_g => s_counter'length, -- number of output bit
initialValue_g => 0 -- the initial value after reset
)
port map (CLK => s_TICK,
RESET_n => s_RESET,
TC => s_counter--(0)
);
--========================================================================--
-- LOOK UP TABLES --
--========================================================================--
SinusLUT_1: entity work.SinusLUT(SinusLUT_a) port map (LUT_IN => s_counter,--(0),
LUT_OUT => s_LUT--(0)
);
--@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@--
--========================================================================--
-- SERVOS --
--========================================================================--
--@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@--
servo_1: entity work.PWMServo(PWMServo_a) generic map (invertHorn_g => '1',
centerCorr_g => 16000 )
port map (CLK => CLOCK_50,
RESET_n => s_RESET,
DUTYCYCLE => s_LUT,
PWMOut => s_pwmSignal(0)
);
servo_2: entity work.PWMServo(PWMServo_a) generic map (invertHorn_g => '0',
centerCorr_g => 88000 )
port map (CLK => CLOCK_50,
RESET_n => s_RESET,
DUTYCYCLE => s_LUT,
PWMOut => s_pwmSignal(1)
);
end ServoTester_a;