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
| physicalclass Wheel {
knownrebecs {WCtlr ctlr;}
statevars {float trq; real spd; real t;}
msgsrv initial(float spd_) {
spd = spd_;
setmode(Rolling);
}
msgsrv setTrq(float trq_) {
trq = trq_;
}
mode Rolling {
inv(t <= 0.05) {
t' = 1;
spd' = -0.1-trq;
}
guard(t == 0.05) {
t = 0;
ctlr.setWspd(spd);
if(spd > 0)
setmode(Rolling);
}
}
}
softwareclass WCtlr {
knownrebecs {Wheel w; BrakeCtlr bctlr;}
statevars {int id; float wspd; float slprt;}
msgsrv initial(int id_) {
id = id_;
}
msgsrv setWspd(float wspd_) {
wspd = wspd_;
bctlr.setWspd(id,wspd);
}
msgsrv applyTrq(float reqTrq, float vspd) {
if(vspd == 0)
slprt = 0;
else
slprt = (vspd - wspd * WRAD)/vspd;
if(slprt > 0.2)
wheel.setTrq(0);
else
wheel.setTrq(reqTrq);
}
}
physicalclass Brake {
knownrebecs {BrakeCtlr bctlr;}
statevars {real bprcnt; real t; float mxprcnt; float r}
msgsrv initial(float bprcnt_, float mxprcnt_) {
bprcnt = bprcnt_;
mxprcnt = mxprcnt_;
r = 1;
setmode(Braking);
}
mode Braking {
inv(t <= 0.05) {
t' = 1;
bprcnt' = r;
}
guard(t == 0.05) {
t = 0;
bctrl.setBprcnt(bprcnt);
if(bprcnt>=mxprcnt)
r = 0;
setmode(Braking);
}
}
}
softwareclass BrakeCtlr {
knownrebecs {WCtlr wctlrR;WCtlr wctlrL;}
statevars {float wspdR;float wspdL;float bprcnt;float gtrq;float espd;}
msgsrv control() {
espd = (wspdR + wspdL)/2;
gtrq = bprcnt;
wctlrR.applyTrq(gtrq, espd);
wctlrL.applyTrq(gtrq, espd);
}
// Setters for wspdR, wspdL and bprcnt
...
}
physicalclass Clock {
knownrebecs {BrakeCtlr bctlr;}
statevars {real t;}
msgsrv initial() {
setmode(Running)
}
mode Running() {
inv(t <= 0.05) {
t' = 1;
}
guard(t == 0.05) {
t = 0;
bctlr.control();
setmode(Running);
}
}
}
main {
Wheel wR (@Wire wctlrR):(1);
Wheel wL (@Wire wctlrL):(1);
WCtlr wctlrR (@Wire wR, @CAN bctlr):(0);
WCtlr wctlrL (@Wire wL, @CAN bctlr):(1);
BrakeCtlr bctlr (@CAN wctlrR, @CAN wctlrL):();
Brake brake(@Wire bctlr):(60,65);
Clock clock(@Wire bctlr):();
CAN {
priorities {
bctlr wctlrR.applyTrq 1;
bctlr wctlrL.applyTrq 2;
wctlrR bctlr.setWspd 3;
wctlrL bctlr.setWspd 4;
}
delays {
bctlr wctlrR.applyTrq 0.01;
bctlr wctlrL.applyTrq 0.01;
wctlrR bctlr.setWspd 0.01;
wctlrL bctlr.setWspd 0.01;
}
}
}
|