-
Notifications
You must be signed in to change notification settings - Fork 1
/
moveToSound.js
208 lines (173 loc) · 7.73 KB
/
moveToSound.js
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
196
197
198
199
200
201
202
203
204
205
206
/**********************************************************************
Copyright 2020 Misty Robotics
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
**WARRANTY DISCLAIMER.**
* General. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW, MISTY
ROBOTICS PROVIDES THIS SAMPLE SOFTWARE "AS-IS" AND DISCLAIMS ALL
WARRANTIES AND CONDITIONS, WHETHER EXPRESS, IMPLIED, OR STATUTORY,
INCLUDING THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE, TITLE, QUIET ENJOYMENT, ACCURACY, AND NON-INFRINGEMENT OF
THIRD-PARTY RIGHTS. MISTY ROBOTICS DOES NOT GUARANTEE ANY SPECIFIC
RESULTS FROM THE USE OF THIS SAMPLE SOFTWARE. MISTY ROBOTICS MAKES NO
WARRANTY THAT THIS SAMPLE SOFTWARE WILL BE UNINTERRUPTED, FREE OF VIRUSES
OR OTHER HARMFUL CODE, TIMELY, SECURE, OR ERROR-FREE.
* Use at Your Own Risk. YOU USE THIS SAMPLE SOFTWARE AND THE PRODUCT AT
YOUR OWN DISCRETION AND RISK. YOU WILL BE SOLELY RESPONSIBLE FOR (AND MISTY
ROBOTICS DISCLAIMS) ANY AND ALL LOSS, LIABILITY, OR DAMAGES, INCLUDING TO
ANY HOME, PERSONAL ITEMS, PRODUCT, OTHER PERIPHERALS CONNECTED TO THE PRODUCT,
COMPUTER, AND MOBILE DEVICE, RESULTING FROM YOUR USE OF THIS SAMPLE SOFTWARE
OR PRODUCT.
Please refer to the Misty Robotics End User License Agreement for further
information and full details:
https://www.mistyrobotics.com/legal/end-user-license-agreement/
**********************************************************************/
// Misty subtly looks at people who are speaking around her !!
// It is intended to be slow and not super responsive - Feel free to play with latencey and make it faster
misty.ChangeLED(0, 0, 255);
misty.Set("lookedAt",(new Date()).toUTCString());
misty.MoveHeadDegrees(-15.0, 0.0, 0.0, 70);
misty.StartRecordingAudio("deleteThis.wav");
misty.Pause(4000);
misty.ChangeLED(0, 255, 0);
// Setting some global variables as normal var <variable_name> cannot be accessed in event callbacks
misty.Set("headYaw", 0.0);
misty.Set("robotYaw", 0.0);
// Used for smoothening data - barely helps :D - Gotta do more layers for better softening
misty.Set("1b", 0.0);
misty.Set("2b",0.0);
misty.Set("inProgress", false);
registerAudioLocalisation();
registerIMU();
registerActuatorPosition();
// -------------------------------------- Audio Localization --------------------------------------------------
function registerAudioLocalisation(){
misty.AddReturnProperty("sound", "DegreeOfArrivalSpeech");
misty.RegisterEvent("sound", "SourceTrackDataMessage", 100 ,true);
}
function _sound(data)
{
// We get the location of the Audio Speech in 0-360 degrees wrt Local Frame of Head
// Using the function toRobotFrame() it is mapped to Robots local frame
var vector = 0.4*toRobotFrame(data.AdditionalResults[0]) + 0.35*misty.Get("1b") + 0.25*misty.Get("2b") ;
// misty.Debug(data.AdditionalResults[0]);
if (!misty.Get("inProgress")) misty.Debug(vector.toString() + " <-- soundIn");
if (secondsPast(misty.Get("lookedAt"))>5.0 && !misty.Get("inProgress"))
{
misty.UnregisterEvent("sound");
misty.Set("inProgress", true);
misty.Debug(vector.toString()+" <-- Look At Input Global");
// misty.Pause(300);
lookAt(vector,misty.Get("robotYaw"),misty.Get("headYaw"));
registerAudioLocalisation();
}
misty.Set("2b",misty.Get("1b"));
misty.Set("1b",vector);
}
// -------------------------------------- Robot Control - MAIN -----------------------------------------------
function lookAt(heading,robotYawAtStart,headYawAtStart)
{
misty.Set("inProgress", true);
// Head Motion
var rawFinalHeadPose = misty.Get("headYaw")+(heading/2.0);
var actuateTo = rawFinalHeadPose;
actuateTo = actuateTo <-45.0 ? -45.0 : actuateTo;
actuateTo = actuateTo >45 ? 45.0 : actuateTo;
misty.Debug(actuateTo.toString()+" <- Head to move to");
misty.MoveHeadDegrees(-15.0, 0.5, actuateTo, 500);
// Body Motion
if (Math.abs(rawFinalHeadPose)>=45)
{
misty.Pause(1000);
var globalHeading = offset_heading(heading+(headYawAtStart*2.0));
if (globalHeading>180) globalHeading-=360;
AngleDifference(robotYawAtStart,globalHeading)>=0 ? misty.Drive(0, 30) : misty.Drive(0, -30);// use speed 30
// misty.MoveHeadDegrees(-14.0, 0.0, 0.0,5);
misty.Debug(globalHeading.toString()+" <- Body to move to");
var initialError = Math.abs(AngleDifference(robotYawAtStart,globalHeading));
var currentAbsError = initialError;
var headResetDone = false;
while (Math.abs(misty.Get("robotYaw")-globalHeading)>=10)
{
// misty.Debug(currentAbsError/initialError);
currentAbsError = Math.abs(AngleDifference(misty.Get("robotYaw"),globalHeading));
if (currentAbsError/initialError <0.70 && !headResetDone)
{
headResetDone = true;
misty.MoveHeadDegrees(-15.0, 0.0, 0.0,500);
}
else {}
misty.Pause(10);
// misty.Debug(misty.Get("robotYaw").toString()+" , " + globalHeading.toString());
}
// misty.Drive(0, 0);
misty.Stop();
} else {
//misty.Pause(3000);
}
misty.Debug("DONE");
misty.Set("lookedAt",(new Date()).toUTCString());
misty.Set("inProgress", false);
}
// ------------------------------------ Head Current Position Updater --------------------------------------
function registerActuatorPosition()
{
misty.AddReturnProperty("Positions", "SensorId");
misty.AddReturnProperty("Positions", "Value");
misty.AddPropertyTest("Positions", "SensorId", "==", "ahy", "string"); //////////// ahy //////////
misty.RegisterEvent("Positions", "ActuatorPosition", 100 ,true);
}
function _Positions(data)
{
if (data.AdditionalResults[0] == "ahy" ) // should be ahy ////////////////////////////////////////
{
// misty.Debug(data.AdditionalResults[0]+" "+(data.AdditionalResults[1]).toString())
var headYaw = data.AdditionalResults[1];//*180.0/Math.PI;
headYaw = headYaw <-45.0 ? -45.0 : headYaw;
headYaw = headYaw >45.0 ? 45.0 : headYaw;
misty.Set("headYaw", headYaw);
}
}
// ------------------------------------ Robot Heading Updater --------------------------------------------
function registerIMU()
{
misty.RegisterEvent("Heading", "IMU",100 ,true);
}
function _Heading(data)
{
var yaw = data.PropertyTestResults[0].PropertyParent.Yaw;//*180.0/Math.PI;
// yaw *= -1; // CW to CCW
if (yaw > 180) yaw -= 360;
misty.Set("robotYaw", yaw);
// misty.Debug(yaw.toString()+" <-- Yaw");
}
// ----------------------------------- Support Functions ------------------------------------------
function AngleDifference(now, to)
{
var diff = ( to - now + 180 ) % 360 - 180;
return diff < -180 ? diff + 360 : diff;
}
function secondsPast(value)
{
var timeElapsed = new Date() - new Date(value);
timeElapsed /= 1000;
return timeElapsed; // seconds
}
function toRobotFrame(data)
{
var soundIn = 360-data;
if (soundIn>180) soundIn -= 360;
return (soundIn);//-1*(data-180)
// return data;
}
function offset_heading(to_offset){
var heading = misty.Get("robotYaw")+to_offset;
return (360.0+(heading%360))%360.0;
}