-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathmain.cpp
192 lines (169 loc) · 7.26 KB
/
main.cpp
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
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
/****************************************************************************/
#include <sched.h>
#include "ecat.h"
#include "plc.h"
#include "ladder.h"
uint32_t s1_32=1, s2_32=2, m1_32=3, m2_32=4, cnt=2;
uint16_t s1_16=1, s2_16=2, m1_16=3, m2_16=4;
uint8_t in_el1004, in_el6900_outvar=0, bnt=0;
bool start_plc = 0;
/****************************************************************************/
void my_cyclic(void)
{
int cycle_counter = 0;
unsigned int blink = 0;
// set first wake time in a few cycles
wakeup_time = system_time_ns() + 10 * cycle_ns;
while (run) {
// wait for next period (using adjustable system time)
wait_period();
cycle_counter++;
if (!run) break;
// receive EtherCAT
ecrt_master_receive(master);
ecrt_domain_process(domain1);
if (*int_memory[0] > 0) {
ecrt_domain_process(domain2);
ecrt_domain_process(domain3);
ecrt_domain_process(domain4);
}
rt_check_domain_state();
if (!(cycle_counter % 1000)) rt_check_master_state();
if (!(cycle_counter % 200)) blink = !blink;
if( (in_el6900_outvar & 1) == 0 || start_plc == 0){
if(start_plc == 0)
EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x0A : 0x05);
else
EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x0F : 0x0);
EC_WRITE_U16(domain1_pd + off_control_out1, 0x6400);
EC_WRITE_U16(domain1_pd + off_control_out2+6+4, 0x6400);
}
else{
EC_WRITE_U8(domain1_pd + off_dig_out0, 0x0F);
if(toggle==0) EC_WRITE_U16(domain1_pd + off_control_out1, 0xe000);
else EC_WRITE_U16(domain1_pd + off_control_out1, 0xe400);
if(toggle==0) EC_WRITE_U16(domain1_pd + off_control_out2+6+4, 0xe000);
else EC_WRITE_U16(domain1_pd + off_control_out2+6+4, 0xe400);
}
toggle = !toggle;
if (!(cycle_counter % 2200)) { tgn = !tgn;}/*
if (tgn==0) {
EC_WRITE_U32(domain1_pd + off_velocity_out1, 0x10ff00);
EC_WRITE_U32(domain1_pd + off_velocity_out2+6, 0xffef00ff); }
else{
EC_WRITE_U32(domain1_pd + off_velocity_out1, 0xffef00ff);
EC_WRITE_U32(domain1_pd + off_velocity_out2+6, 0x10ff00); }
*/
if (*int_memory[0]==2) {
EC_WRITE_U32(domain1_pd + off_velocity_out1, 0x10ff00);
EC_WRITE_U32(domain1_pd + off_velocity_out2+6+8, 0xffef00ff); }
else{
EC_WRITE_U32(domain1_pd + off_velocity_out1, 0xffef00ff);
EC_WRITE_U32(domain1_pd + off_velocity_out2+6+8, 0x10ff00);
}
if (*int_memory[0] > 0) {
start_plc = 1;
s1_32 = EC_READ_U32(domain3_pd + off_el1904_in);
s1_16 = EC_READ_U16(domain3_pd + off_el1904_in+4);
s2_32 = EC_READ_U32(domain3_pd + off_el2904_in);
s2_16 = EC_READ_U16(domain3_pd + off_el2904_in+4);
in_el1004 = EC_READ_U8 (domain3_pd + off_el1004_in);
m1_32 = EC_READ_U32(domain4_pd + off_el6900_in1);
m1_16 = EC_READ_U16(domain4_pd + off_el6900_in1+4);
m2_32 = EC_READ_U32(domain4_pd + off_el6900_in2);
m2_16 = EC_READ_U16(domain4_pd + off_el6900_in2+4);
in_el6900_outvar = EC_READ_U8 (domain4_pd + off_el6900_in3);
//printf("ohoh: %x %x %x %d %d \n", s1_32, s2_32, v8, v8&4, ((v8&4)==4));
///////////////////////////////////////////////////////////////
EC_WRITE_U32(domain2_pd + off_el1904_out, m1_32);
EC_WRITE_U16(domain2_pd + off_el1904_out+4, m1_16);
EC_WRITE_U32(domain2_pd + off_el2904_out1, m2_32);
EC_WRITE_U16(domain2_pd + off_el2904_out1+4, m2_16);
EC_WRITE_U32(domain4_pd + off_el6900_out1, s1_32);
EC_WRITE_U16(domain4_pd + off_el6900_out1+4, s1_16);
EC_WRITE_U32(domain4_pd + off_el6900_out2, s2_32);
EC_WRITE_U16(domain4_pd + off_el6900_out2+4, s2_16);
if(cnt==2 && bnt<30)
{EC_WRITE_U8 (domain4_pd + off_el6900_out2+6, 0x1e); cnt=0;bnt++;}
else
{ cnt++;
if( (in_el1004 & 4) == 4)
EC_WRITE_U8 (domain4_pd + off_el6900_out2+6, 0x17);
else
EC_WRITE_U8 (domain4_pd + off_el6900_out2+6, 0x16);
}
}
else {
cnt=0; bnt=0; //errAck
start_plc = 0;
}
// queue process data
ecrt_domain_queue(domain1);
if (*int_memory[0] > 0) {
ecrt_domain_queue(domain2);
ecrt_domain_queue(domain3);
ecrt_domain_queue(domain4);
}
// sync distributed clock just before master_send to set
// most accurate master clock time
sync_distributed_clocks();
// send EtherCAT data
ecrt_master_send(master);
// update the master clock
// Note: called after ecrt_master_send() to reduce time
// jitter in the sync_distributed_clocks() call
update_master_clock();
}
}
/****************************************************************************
* Main function
***************************************************************************/
int main(int argc, char *argv[])
{
init_plc(argc, argv); *int_memory[0] = 0;
init_ecat();
/* Set the initial master time and select a slave to use as the DC
* reference clock, otherwise pass NULL to auto select the first capable
* slave. Note: This can be used whether the master or the ref slave will
* be used as the systems master DC clock.
*/
dc_start_time_ns = system_time_ns();
dc_time_ns = dc_start_time_ns;
/* Attention: The initial application time is also used for phase
* calculation for the SYNC0/1 interrupts. Please be sure to call it at
* the correct phase to the realtime cycle.
*/
ecrt_master_application_time(master, dc_start_time_ns);
printf("Activating master...\n");
if (ecrt_master_activate(master)) {
return -1;
}
if (!(domain1_pd = ecrt_domain_data(domain1)))
{ fprintf(stderr, "Failed to get domain data pointer.\n"); return -1; }
if (!(domain2_pd = ecrt_domain_data(domain2)))
{ fprintf(stderr, "Failed to get domain data pointer.\n"); return -1; }
if (!(domain3_pd = ecrt_domain_data(domain3)))
{ fprintf(stderr, "Failed to get domain data pointer.\n"); return -1; }
if (!(domain4_pd = ecrt_domain_data(domain4)))
{ fprintf(stderr, "Failed to get domain data pointer.\n"); return -1; }
/* Create cyclic RT-thread */
struct sched_param param;
param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
puts("ERROR IN SETTING THE SCHEDULER");
perror("errno");
return -1;
}
/*
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
*/
printf("Starting cyclic function.\n");
my_cyclic();
printf("End of Program\n");
ecrt_release_master(master);
return 0;
}
/****************************************************************************/