60 |
60 |
61 static EtherCAT_slave_t ecat_slaves[] = |
61 static EtherCAT_slave_t ecat_slaves[] = |
62 { |
62 { |
63 // Block 1 |
63 // Block 1 |
64 ECAT_INIT_SLAVE(Beckhoff_EK1100, 0), |
64 ECAT_INIT_SLAVE(Beckhoff_EK1100, 0), |
65 ECAT_INIT_SLAVE(Beckhoff_EL4102, 0), |
65 ECAT_INIT_SLAVE(Beckhoff_EL3102, 0) |
66 ECAT_INIT_SLAVE(Beckhoff_EL1014, 0), |
|
67 ECAT_INIT_SLAVE(Beckhoff_EL3162, 0), |
|
68 ECAT_INIT_SLAVE(Beckhoff_EL2004, 0), |
|
69 ECAT_INIT_SLAVE(Beckhoff_EL3102, 0), |
|
70 ECAT_INIT_SLAVE(Beckhoff_EL2004, 0), |
|
71 |
|
72 // Block 2 |
|
73 ECAT_INIT_SLAVE(Beckhoff_EK1100, 1), |
|
74 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1), |
|
75 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1), |
|
76 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1), |
|
77 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1), |
|
78 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1), |
|
79 ECAT_INIT_SLAVE(Beckhoff_EL2004, 1), |
|
80 ECAT_INIT_SLAVE(Beckhoff_EL2004, 1), |
|
81 ECAT_INIT_SLAVE(Beckhoff_EL2004, 1), |
|
82 ECAT_INIT_SLAVE(Beckhoff_EL2004, 1), |
|
83 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1), |
|
84 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1), |
|
85 ECAT_INIT_SLAVE(Beckhoff_EL1014, 1) |
|
86 }; |
66 }; |
87 |
67 |
88 #define ECAT_SLAVES_COUNT (sizeof(ecat_slaves) / sizeof(EtherCAT_slave_t)) |
68 #define ECAT_SLAVES_COUNT (sizeof(ecat_slaves) / sizeof(EtherCAT_slave_t)) |
89 |
69 |
90 #define USE_MSR_LIB |
70 #define USE_MSR_LIB |
94 int dig1; |
74 int dig1; |
95 #endif |
75 #endif |
96 |
76 |
97 /****************************************************************************** |
77 /****************************************************************************** |
98 * |
78 * |
99 * Function: next2004 |
|
100 * |
|
101 *****************************************************************************/ |
|
102 |
|
103 static int next2004(int *wrap) |
|
104 { |
|
105 static int i = 0; |
|
106 unsigned int j = 0; |
|
107 |
|
108 *wrap = 0; |
|
109 |
|
110 for (j = 0; j < ECAT_SLAVES_COUNT; j++) |
|
111 { |
|
112 i++; |
|
113 |
|
114 i %= ECAT_SLAVES_COUNT; |
|
115 |
|
116 if (i == 0) *wrap = 1; |
|
117 |
|
118 if (ecat_slaves[i].desc == Beckhoff_EL2004) |
|
119 { |
|
120 return i; |
|
121 } |
|
122 } |
|
123 |
|
124 return -1; |
|
125 } |
|
126 |
|
127 /****************************************************************************** |
|
128 * |
|
129 * Function: msr_controller_run() |
79 * Function: msr_controller_run() |
130 * |
80 * |
131 *****************************************************************************/ |
81 *****************************************************************************/ |
132 |
82 |
133 static void msr_controller_run(void) |
83 static void msr_controller_run(void) |
134 { |
84 { |
135 static int ms = 0; |
|
136 static int cnt = 0; |
|
137 static int firstrun = 1; |
|
138 static unsigned int debug_counter = 0; |
85 static unsigned int debug_counter = 0; |
139 |
|
140 static int klemme = 0; |
|
141 static int kanal = 0; |
|
142 static int up_down = 0; |
|
143 int wrap = 0; |
|
144 |
|
145 unsigned long t_bus_start, t_bus_end; |
|
146 |
86 |
147 // Prozessdaten lesen |
87 // Prozessdaten lesen |
148 msr_jitter_run(MSR_ABTASTFREQUENZ); |
88 msr_jitter_run(MSR_ABTASTFREQUENZ); |
149 |
89 |
150 if (firstrun) { |
90 if (debug_counter == 0) { |
151 klemme = next2004(&wrap); |
91 ecat_master->debug_level = 2; |
152 firstrun = 0; |
92 } |
153 } |
93 |
154 |
94 // Prozessdaten schreiben |
155 ms++; |
95 |
156 ms %= 1000; |
96 if (EtherCAT_process_data_cycle(ecat_master, 0, 40) < 0) |
157 if (cnt++ > 200) |
97 ecat_timeouts++; |
158 { |
|
159 cnt = 0; |
|
160 |
|
161 if (++kanal > 3) |
|
162 { |
|
163 kanal = 0; |
|
164 klemme = next2004(&wrap); |
|
165 |
|
166 if (wrap == 1) |
|
167 { |
|
168 if (up_down == 1) up_down = 0; |
|
169 else up_down = 1; |
|
170 } |
|
171 } |
|
172 } |
|
173 |
|
174 if (klemme >= 0) { |
|
175 EtherCAT_write_value(&ecat_slaves[klemme], kanal, up_down); |
|
176 } |
|
177 |
98 |
178 if (debug_counter == 0) { |
99 if (debug_counter == 0) { |
179 ecat_master->debug_level = 0; |
100 ecat_master->debug_level = 0; |
180 } |
101 } |
181 |
102 |
182 // Prozessdaten schreiben |
103 value = EtherCAT_read_value(&ecat_slaves[1], 0); |
183 |
|
184 rdtscl(t_bus_start); |
|
185 |
|
186 if (EtherCAT_process_data_cycle(ecat_master, 0, 40) < 0) |
|
187 ecat_timeouts++; |
|
188 |
|
189 if (EtherCAT_process_data_cycle(ecat_master, 1, 40) < 0) |
|
190 ecat_timeouts++; |
|
191 |
|
192 rdtscl(t_bus_end); |
|
193 ecat_bus_time = TSC2US(t_bus_start, t_bus_end); |
|
194 |
|
195 if (debug_counter == 0) { |
|
196 ecat_master->debug_level = 0; |
|
197 } |
|
198 |
|
199 // Daten lesen und skalieren |
|
200 #ifdef USE_MSR_LIB |
|
201 value = EtherCAT_read_value(&ecat_slaves[5], 0) / 3276.0; |
|
202 dig1 = EtherCAT_read_value(&ecat_slaves[2], 0); |
|
203 #endif |
|
204 |
104 |
205 debug_counter++; |
105 debug_counter++; |
206 if (debug_counter >= MSR_ABTASTFREQUENZ * 5) debug_counter = 0; |
106 if (debug_counter >= MSR_ABTASTFREQUENZ * 5) debug_counter = 0; |
207 } |
107 } |
208 |
108 |