-
Notifications
You must be signed in to change notification settings - Fork 28
/
cmd_einstein.scm
195 lines (164 loc) · 7.13 KB
/
cmd_einstein.scm
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
193
194
195
;; Send Commands to Professor Einstein
;;
;; File: cmd_einstein.scm
;; Author: Dagim Sisay <[email protected]>
;; Date: November, 2018
;; License: AGPL
(use-modules (ice-9 hash-table)
(ice-9 regex)
(json))
;(include-from-path "behavior/functions.scm")
; XXX there should be error checks here... just have to make sure the robot has its
; port 8080 open before starting this script. Don't know how to do the error checks
; in the time I have now.
(define sck (socket PF_INET SOCK_STREAM 0))
;; // XXX uncomment the following line and the one in the function send-to-einstein
(connect sck AF_INET (inet-pton AF_INET "192.168.1.1") 8080)
; whwn the robot remains idle for sometime, it shuts down.
; to avoid that, we're gonna have a thread that sends
; the robot random commands like turning the head and
; sticking his toungue out every few minutes just to keep
; it alive.
; Keeping the random actions to something that doesn't block
; the loop for long is preferable since stoping is only
; possible throught the do-random-actions variable,
; the loop should go around and check the value of that variable
; as frequently as possible.
(define-public do-random-actions #t)
(define-public CMD-TO-SAY #f)
(define-public TIME-TO-SAY 0)
; the command codes
(define einstein-cmds (make-hash-table))
(hash-set! einstein-cmds "head_right" "<MO=HT,1,0.1>")
(hash-set! einstein-cmds "head_center" "<MO=HT,0.5,0.5>")
(hash-set! einstein-cmds "head_left" "<MO=HT,0,0.0>")
(hash-set! einstein-cmds "head_up" "<MO=HN,1,0.0>")
(hash-set! einstein-cmds "head_down" "<MO=HN,0,0.1>")
(hash-set! einstein-cmds "mouth_open" "<MO=MO,1,0.5>")
(hash-set! einstein-cmds "mouth_close" "<MO=MO,0,0.5>")
(hash-set! einstein-cmds "eyebrow_up" "<MO=EB,1,0.5>")
(hash-set! einstein-cmds "eyebrow_down" "<MO=EB,0,0.5>")
(hash-set! einstein-cmds "eyelid_open" "<MO=EL,0,0.5>")
(hash-set! einstein-cmds "eyelid_close" "<MO=EL,1,0.5>")
(hash-set! einstein-cmds "cheek_up" "<MO=CH,0,0.5>")
(hash-set! einstein-cmds "cheek_normal" "<MO=CH,0.5,0.5>")
(hash-set! einstein-cmds "cheek_down" "<MO=CH,1,0.5>")
(hash-set! einstein-cmds "arm_up" "<MO=AR,1,0.5>")
(hash-set! einstein-cmds "arm_down" "<MO=AR,0,0.5>")
; list cmds
(define cmds (list "head_right" "head_left" "head_up" "head_down"
"mouth_open" "mouth_close" "eyebrow_up" "eyebrow_down"
"eyelid_open" "eyelid_close" "cheek_up" "cheek_down"))
; function to encode commands to the einstein robot.
(define-public (cmd-to-einstein cmd)
(define fcmd (scm->json-string `(("data".(("output",@cmd)))("cmd"."activity.recieved"))))
;(set! fcmd (regexp-substitute/global #f "[ \t]+" fcmd 'pre "" 'post))
(string-append (format #f "~5,'0d" (string-length fcmd)) fcmd))
; send einstein command through a socket to port 8080 which is the default for the robot
(define-public (send-cmd STR)
(display (cmd-to-einstein STR) sck)
(display (cmd-to-einstein STR)) (newline))
; pause the random actions thread when sending a command
; TODO also need a que for commands
(define-public (send-to-einstein CMD)
(set! do-random-actions #f)
(send-cmd CMD)
(if CMD-TO-SAY (begin (usleep TIME-TO-SAY) (set! CMD-TO-SAY #f)))
(set! do-random-actions #t))
; just an intro
(send-to-einstein "Hello, I'm Dr Robot o")
; seconds to microseconds
(define (us s) (inexact->exact (floor (* s 1000000 ))))
; send a command CMD and delay for SEC seconds
(define (send-delay CMD SEC) (send-to-einstein (hash-ref einstein-cmds CMD)) (usleep (us SEC)))
; send a command CMD and delay for a random number of seconds less than 10
(define (send-rnd-delay CMD)
(send-to-einstein (hash-ref einstein-cmds CMD))
(usleep (us (random 10))))
;; Status vars
(define eye_state 0)
;; Define Routine acts
(define-public (act-head-down)
(send-to-einstein (hash-ref einstein-cmds "head_down")))
(define-public (act-head-up)
(send-to-einstein (hash-ref einstein-cmds "head_up")))
(define-public (act-head-center)
(send-to-einstein (hash-ref einstein-cmds "head_center")))
(define-public (act-eyes-open)
(send-to-einstein (hash-ref einstein-cmds "eyelid_open")))
(define-public (act-eyes-close)
(send-to-einstein (hash-ref einstein-cmds "eyelid_close")))
(define-public (act-eyes-toggle)
(send-to-einstein (format #f "<MO=EL,~d,0.5>" (invrt eye_state))))
(define-public (act-arm-up)
(send-to-einstein (hash-ref einstein-cmds "arm_up")))
(define-public (act-arm-down)
(send-to-einstein (hash-ref einstein-cmds "arm_down")))
; Shake head horizontally - nod no
; XXX this routine is no good look into increasing the speed of the motor esti
(define-public (act-head-hshake)
(define act (list "head_left" "head_right" "head_left" "head_right"
"head_center"))
(define del (list 0.4 0.4 0.4 0.4 0.4))
(map send-delay act del))
; Shake head vertically - nod yes
(define-public (act-head-vshake)
(define act (list "head-center" "head_up" "head_down" "head_up" "head_down" "head_up"))
(define del (list 0.4 0.4 0.4 0.4 0.4 0.4))
(map send-delay act del))
(define-public (look-side-to-side)
(define act (list "head_center" "head_right" "head_left" "head_center"))
(define del (list 0.1 3 3 0.1))
(map send-delay act del))
; smile for a few seconds and then back to normal
(define-public (act-smile)
(define act (list "cheek_normal" "cheek_up" "cheek_normal"))
(define del (list (random 3) (random 5) 1))
(map send-delay act del))
; show a sad face for a little while
(define-public (act-sad)
(define act (list "cheek_normal" "cheek_down" "cheek_normal"))
(define del (list (random 3) (random 5) 1))
(map send-delay act del))
; show an afraid face for a little while
(define-public (act-silly)
(define act (list "cheek_normal" "cheek_up" "mouth_open" "mouth_close" "cheek_normal"))
(define del (list 2 1 (random 5) 1 2 ))
(map send-delay act del))
; pan the head to a given point
(define-public (act-pan-head POS)
(if (bn-zero-one? POS)
(begin
(send-to-einstein (format #f "<MO=HT,~f,0.5>" (format #f "~4f" POS)))
(stv 1 1))
(stv 0 1)))
; tilt the head to a given point
(define-public (act-tilt-head POS)
(if (bn-zero-one? POS)
(begin
(send-to-einstein (format #f "<MO=HN,~f,0.5>" (format #f "~4f" POS)))
(stv 1 1))
(stv 0 1)))
;; XXX TODO this is just to avoid an unimplemented function error
;; do it right!
(define-public (act-face-a-point XY)
(act-pan-head (image-width->head-pan (car XY)))
(act-tilt-head (image-height->head-tilt (cadr XY))))
(define-public (random-cmds)
(while #t
(if do-random-actions
(act-eyes-toggle) ; change this to something more relevant
(continue))
(usleep (us (random 30))))) ; delay random no of seconds less than 30s
; XXX check if the exact value should be 30s
; reset the prof to normal posture after
; internal startup script made him abnormal
(act-arm-down) (usleep (us 1))
(act-head-up) (usleep (us 1))
(act-head-center) (usleep (us 1))
(act-eyes-open) (usleep (us 1))
; send random commands to einstein on a new thread so that it doesn't
; go to sleep. When a speech or intentional command is to be sent,
; do-random-actions should be set to false so that no interfering cmnds
; are sent during that time and then reenabled again.
;(call-with-new-thread random-cmds)