A tele-operated robot is a tracked device with vehicle schematic as shown in the following figure.
Motor control: If the sensor detects an object then the control system takes over control otherwise if the operator requests a new movement then action this.
Let Lic and Ric denote respectively the left and right motor commands issued by the infra-red control. Let Iractive denote the presence/absence of an object. Let Loc and Roc denote the left and right motor command issued by the operator and let Opactive denote an active operator request.
Let move(l,r) denote the sending of left l and right r motor commands to the two motors.
Motor control:
MCS ≜ |
( (Iractive ∧move(Lic,Ric)) ∨ |
(Opactive ∧move(Loc,Roc)) ∨ |
(¬Iractive ∧¬Opactive) )∗ |
What happens when both Iractive and Opactive are true? Both move (Lic,Ric)) and move(Loc,Roc) commands are issued.
Need to resolve which command has priority. If Infra-red has priority we have:
MCS ≜ |
( (Iractive ∧move(Lic,Ric)) ∨ |
(Opactive ∧¬Iractive ∧move(Loc,Roc)) ∨ |
(¬Iractive ∧¬Opactive) )∗ |
We have strengthened the guard of the Operator with the negation of the Infra-red guard.
Infra-red control: read the sensors and for each sensor that is greater than the threshold of 100 adjust the motor commands accordingly. For example if the north sensor detects an object we should move in the south direction as an avoidance strategy.
Let Ircounts(i) denote the sensor i (N: i=0, NE: i=1, E: i=2, SE: i=3, S: i=4, SW: i=5, W: i=6, NW: i=7). Let mvl(i) and mvr(i) denote respectively the left and right steering commands corresponding to sensor i.
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 | |
mvl |
b |
s |
b |
s |
f |
f |
f |
b |
mvr | b | b | f | f | f | s | b | s |
↓ | ↙ | ← | ↖ | ↑ | ↗ | → | ↘ |
|
ICS ≜ |
( Iractive = (∨
iIrcounts(i) > 100) ∧ |
Lic = (∑
i : Ircounts(i) > 100 : mvl(i)) ∧ |
Ric = (∑
i : Ircounts(i) > 100 : mvr(i)) |
)∗ |
Operator control: if the operator requests some changes then process them.
Let Loc and Roc denote respectively the left and right steering commands received from the operator. Let Lloc and Lroc denote respectively the last left and last right steering commands received from the operator.
OCS ≜ |
∃Lloc,Lroc ( |
Lloc = 0 ∧Lroc = 0 ∧ |
(Opactive = (Loc≠Lloc ∨Roc≠Lroc) ∧ |
Lloc = Loc ∧Lroc = Roc |
)∗ |
) |
Operator control: The operator is active if at least one of the issued steering commands is not zero.
OCS ≜ (Opactive = (Loc≠0 ∨Roc≠0) )∗ |
/∗ ∗ tempura code for robot control system ∗ OCS : operator control system ∗ ICS : infrared control system ∗ MCS : motor control system ∗/ define move(X,Y) = { format("sending left %t right %t \n", X, Y) }. define s = 0. define f = 20. define b = -20. define mvl = [b,s,b,s,f,f,f,b]. define mvr = [b,b,f,f,f,s,b,s]. /∗ run ∗/ define robot() = { exists Loc, Roc, Lic, Ric, Opactive, Iractive, Ircounts : { list(Ircounts,8) and stable(struct(Ircounts)) and
define OCS() = { while true do { len(1) and input Loc and input Roc and Opactive = {if (Loc ~= 0) or (Roc ~= 0) then 1 else 0} } } and define sum(X) = { {if Ircounts[0]>100 then X[0] else 0} + {if Ircounts[1]>100 then X[1] else 0} + {if Ircounts[2]>100 then X[2] else 0} + {if Ircounts[3]>100 then X[3] else 0} + {if Ircounts[4]>100 then X[4] else 0} + {if Ircounts[5]>100 then X[5] else 0} + {if Ircounts[6]>100 then X[6] else 0} + {if Ircounts[7]>100 then X[7] else 0} } and
define ICS() = { while true do { len(1) and input Ircounts and Iractive = {if (exists i < 8 : Ircounts[i]>100) then 1 else 0} and Lic = sum(mvl) and Ric = sum(mvr) } } and define MCS() = { while true do { len(1) and if Opactive=1 and Iractive=0 then { format("operator move \n") and move(Loc,Roc) } else { if Iractive=1 then { format("infrared move \n") and move(Lic,Ric) } } } } and
MCS() and ICS() and OCS() } }.