HRESULT CSnowDropper::Setup ( float in_areaSize,
vec3_t in_velocity,
int in_ratio,
D3DCOLOR in_color )
{
ChangeAreaSize ( in_areaSize );
ChangeDropColor ( in_color );
ChangeDropRatio ( in_ratio );
ChangeVelocity ( in_velocity );
Reset ();
return S_OK;
}
HRESULT CRainDropper::Setup ( float in_areaSize,
vec3_t in_velocity,
float in_lengthMin,
float in_lengthMax,
int in_ratio,
D3DCOLOR in_color )
{
ChangeAreaSize ( in_areaSize );
ChangeDropColor ( in_color );
ChangeDropRatio ( in_ratio );
ChangeVelocity ( in_velocity );
ChangeLength ( in_lengthMin, in_lengthMax );
Reset ();
return S_OK;
}
/* YAPOCClient main function */
void main(){
Initialize();
//wait until at least one client is connected to output port
while (outCommandPort.IsReceiving() <= 0){
YARPTime::DelayInSeconds(1);
}
//change velocity
//note that this can also be done by changing default velocities in the
//configuration file
short axes[] = {NECK_TILT,LEFTEYE_PAN,RIGHTEYE_PAN,EYE_TILT,NECK_PAN, HEAD_ROLL, HEAD_TILT, NECK_CRANE};
float newvel[8] = {0.08, 0.5, 0.5, 0.3, 0.3, 0.2, 0.2, 0.5};
ChangeVelocity(8,axes,newvel);
//time variables for regulating movement frequency
time_t curtime;
time(&curtime);
time_t lastmove_neckpan = curtime;
time_t lastmove_headtilt = curtime;
time_t lastmove_headroll = curtime;
time_t lastmove_expression = curtime;
time_t lastmove_drive = curtime;
time_t lastmove_neckcrane = curtime;
time_t lastmove_basepan = curtime;
time_t lastmove_lowerlid = curtime;
//convert pixel distance to pan/tilt radians
float pan_radians_per_pixel = -0.5f * radians_per_degree;
float tilt_radians_per_pixel = -0.5f * radians_per_degree;
float posbuffer[N_AXIS];
PositionCommand eye_com, neck_pan_com, base_pan_com, head_tilt_com, head_roll_com, neck_crane_com;
int first = 1;
int current_emotion = CALM;
int modify_emotion = 0;
int current_drive;
//initially randomly choose current drive, 50% SOCIAL 50% PLAY
if (rand() > 16383)
current_drive = SOCIAL;
else
current_drive = PLAY;
printf("current drive is %d\n",current_drive);
FiveBoxesInARow target_leftboxes, target_rightboxes;
int total_pixels, total_sal, last_total_sal = 0, sum_delta_sal = 0, delta_iter = 0;
//continuous loop
while (1){
//read position from encoder
ReadPosition(posbuffer);
if(DEBUG) printf("Read position:");
for (int j = 0; j < N_MEIAXES; j++)
if(DEBUG) printf(" %f ",posbuffer[j]);
if(DEBUG) printf("\n");
//read target pos, produced by vision modules
in_pos.Read();
StereoPosData& target_pos = in_pos.Content();
if(DEBUG) printf("The position as of this time is %4g,%4g %4g,%4g, %d :",
target_pos.xl, target_pos.yl,
target_pos.xr, target_pos.yr, target_pos.valid);
modify_emotion = 0;
//if current drive is SOCIAL, read boxes from skin detectors
if (current_drive == SOCIAL){
in_leftboxes.Read();
in_rightboxes.Read();
}
else {
//if current drive is PLAY, read boxes from color detectors
in_attnboxes.Read();
}
if (current_drive == SOCIAL){
target_leftboxes = in_leftboxes.Content();
target_rightboxes = in_rightboxes.Content();
//calculate total pixels from all detected boxes
total_pixels = target_leftboxes.box1.total_pixels+target_leftboxes.box2.total_pixels+
target_leftboxes.box3.total_pixels+target_leftboxes.box4.total_pixels+target_leftboxes.box5.total_pixels
+ target_rightboxes.box1.total_pixels+target_rightboxes.box2.total_pixels+
target_rightboxes.box3.total_pixels+target_rightboxes.box4.total_pixels+target_rightboxes.box5.total_pixels;
total_sal = target_leftboxes.box1.total_sal+target_leftboxes.box2.total_sal+
target_leftboxes.box3.total_sal+target_leftboxes.box4.total_sal+target_leftboxes.box5.total_sal
+ target_rightboxes.box1.total_sal+target_rightboxes.box2.total_sal+
target_rightboxes.box3.total_sal+target_rightboxes.box4.total_sal+target_rightboxes.box5.total_sal;
printf("Total pixels = %d saliency = %d \n",total_pixels, total_sal);
}
else {
target_leftboxes = in_attnboxes.Content();
//calculate total pixels from all detected boxes
total_pixels = target_leftboxes.box1.total_pixels+target_leftboxes.box2.total_pixels+
target_leftboxes.box3.total_pixels+target_leftboxes.box4.total_pixels+target_leftboxes.box5.total_pixels;
total_sal = target_leftboxes.box1.total_sal+target_leftboxes.box2.total_sal+
target_leftboxes.box3.total_sal+target_leftboxes.box4.total_sal+target_leftboxes.box5.total_sal;
printf("Total pixels = %d saliency = %d \n",total_pixels, total_sal);
}
double delta_pan_pixels;
double delta_tilt_pixels;
//translate target pos to pan and tilt delta pixels
delta_pan_pixels = target_pos.xl - 63;
delta_tilt_pixels = target_pos.yl - 63;
if(DEBUG)
printf("%4g,%4g\n", delta_pan_pixels, delta_tilt_pixels);
//at startup, generate CALM expression
if (first == 1){
FaceExpression(CALM_pos,posbuffer);
first = 0;
}
//move eye pan and tilt motors according to target pos
eye_com.ClearAll();
float new_eye_pos_pan;
new_eye_pos_pan = posbuffer[LEFTEYE_PAN] +
(pan_radians_per_pixel * delta_pan_pixels);
float new_eye_pos_tilt;
new_eye_pos_tilt = posbuffer[EYE_TILT] +
(tilt_radians_per_pixel * delta_tilt_pixels);
eye_com.Set(LEFTEYE_PAN, new_eye_pos_pan);
eye_com.Set(RIGHTEYE_PAN, -new_eye_pos_pan);
eye_com.Set(EYE_TILT, new_eye_pos_tilt);
eye_com.Execute();
//get current time
time(&curtime);
/*if total saliency (total pixels of all boxes) haven't changed much in
the last 20 iterations and total saliency jumps by more than 10000,
generate surprised expression */
if (delta_iter > 20 && abs (last_total_sal - total_sal) > 10000){
printf("--> Surprised\n");
FaceExpression(SURPRISED_pos, posbuffer);
modify_emotion = 1;
current_emotion = SURPRISED;
lastmove_expression = curtime;
short axis[1];
axis[0] = NECK_TILT;
float newvelo[1];
newvelo[0] = 0.16f;
ChangeVelocity(1,axis,newvelo);
head_tilt_com.ClearAll();
head_tilt_com.Set(NECK_TILT, posbuffer[NECK_TILT] + 0.15f);
head_tilt_com.Execute();
newvelo[0]= 0.08f;
ChangeVelocity(1,axis,newvelo);
}
else {
//if total saliency > threshold and current emotion is not FEAR, generate
//fear expression
if (total_sal > 550000 && current_emotion != FEAR){
printf("--> Fear\n");
FaceExpression(FEAR_pos, posbuffer);
modify_emotion = 1;
current_emotion = FEAR;
lastmove_expression = curtime;
short axis[1];
axis[0] = NECK_TILT;
float newvelo[1];
newvelo[0] = 0.25f;
ChangeVelocity(1,axis,newvelo);
head_tilt_com.ClearAll();
head_tilt_com.Set(NECK_TILT, posbuffer[NECK_TILT] + 0.3f);
head_tilt_com.Execute();
newvelo[0]= 0.08f;
ChangeVelocity(1,axis,newvelo);
}
}
//if change in total saliency is less than threshold, increment delta_iter
if (abs(total_sal - last_total_sal) < 5000){
delta_iter++;
}
//otherwise, reset to 0
else
delta_iter = 0;
//if the last expression change occurs more than 3 secs ago
if (curtime - lastmove_expression > 3){
//if current emotion is surprised, become happy
if (current_emotion == SURPRISED){
printf("--> Happy\n");
FaceExpression(HAPPY_pos, posbuffer);
modify_emotion = 1;
current_emotion = HAPPY;
//tilt neck forward
head_tilt_com.ClearAll();
head_tilt_com.Set(NECK_TILT, posbuffer[NECK_TILT] - 0.08f);
head_tilt_com.Execute();
}
//if current emotion is fear, become sad
else if (current_emotion == FEAR){
printf("--> Sad\n");
FaceExpression(SAD_pos,posbuffer);
modify_emotion = 1;
current_emotion = SAD;
//tilt neck forward
head_tilt_com.ClearAll();
head_tilt_com.Set(NECK_TILT, posbuffer[NECK_TILT] - 0.08f);
head_tilt_com.Execute();
}
else
{
//if total saliency is low and not currently sad, about 50% chance of
//becoming sad
if (total_sal < 25000 && current_emotion != SAD){
if (rand() > 16000){
printf("--> Sad\n");
FaceExpression(SAD_pos,posbuffer);
modify_emotion = 1;
current_emotion = SAD;
head_tilt_com.ClearAll();
head_tilt_com.Set(NECK_TILT, posbuffer[NECK_TILT] - 0.08f);
head_tilt_com.Execute();
}
// else do nothing
}
//if total saliency is high, either become calm or happy
if (total_sal > 60000){
if (rand() > 16000){
printf("--> Calm");
FaceExpression(CALM_pos,posbuffer);
modify_emotion = 1;
current_emotion = CALM;
}
else {
FaceExpression(HAPPY_pos,posbuffer);
modify_emotion = 1; current_emotion = HAPPY;
head_tilt_com.ClearAll();
head_tilt_com.Set(NECK_TILT, posbuffer[NECK_TILT] + 0.08f);
head_tilt_com.Execute();
}
}
}
lastmove_expression = curtime;
}
last_total_sal = total_sal;
//Upper eyelids are adjusted during face expression, so if there's no
//change in face expression, we need to adjust eyelids here.
if (modify_emotion == 0){
//move eyelids
float uleftlid_diff, urightlid_diff;
if (current_emotion == HAPPY){
uleftlid_diff = posbuffer[EYE_TILT]/0.28 * 400 + HAPPY_pos[LEFT_UPPER_LID - 8];
urightlid_diff = - (posbuffer[EYE_TILT]/0.28 * 400) + HAPPY_pos[RIGHT_UPPER_LID - 8];
}
if (current_emotion == SAD){
uleftlid_diff = posbuffer[EYE_TILT]/0.28 * 400 + SAD_pos[LEFT_UPPER_LID - 8];
urightlid_diff = - (posbuffer[EYE_TILT]/0.28 * 400) + SAD_pos[RIGHT_UPPER_LID - 8];
}
if (current_emotion == SURPRISED){
uleftlid_diff = posbuffer[EYE_TILT]/0.28 * 400 + SURPRISED_pos[LEFT_UPPER_LID - 8];
urightlid_diff = - (posbuffer[EYE_TILT]/0.28 * 400) + SURPRISED_pos[RIGHT_UPPER_LID - 8];
}
if (current_emotion == FEAR){
uleftlid_diff = posbuffer[EYE_TILT]/0.28 * 400 + FEAR_pos[LEFT_UPPER_LID - 8];
urightlid_diff = - (posbuffer[EYE_TILT]/0.28 * 400) + FEAR_pos[RIGHT_UPPER_LID - 8];
}
if (current_emotion == CALM){
uleftlid_diff = posbuffer[EYE_TILT]/0.28 * 400 + CALM_pos[LEFT_UPPER_LID - 8];
urightlid_diff = - (posbuffer[EYE_TILT]/0.28 * 400) + CALM_pos[RIGHT_UPPER_LID - 8];
}
MoveOneAxis(LEFT_UPPER_LID, -uleftlid_diff);
MoveOneAxis(RIGHT_UPPER_LID, -urightlid_diff);
}
//if left eye pan is beyond threshold & the last neckpan move is more than
//1 sec ago, move neckpan accordingly
float neck_eye_pan_diff;
neck_eye_pan_diff = posbuffer[LEFTEYE_PAN];
if (curtime - lastmove_neckpan > 1 && (neck_eye_pan_diff > 0.16f || neck_eye_pan_diff < -0.16f))
{
neck_pan_com.ClearAll();
neck_pan_com.Set(NECK_PAN,
posbuffer[NECK_PAN] + (-1.25f *neck_eye_pan_diff));
neck_pan_com.Execute();
lastmove_neckpan = curtime;
}
//if neck pan is beyond threshold & the last base pan move is more than
//2 secs ago, move base_pan accordingly
float neck_base_pan_diff = posbuffer[NECK_PAN];
if (curtime - lastmove_basepan > 2 && (neck_base_pan_diff > 0.6f || neck_base_pan_diff < -0.6f)){
//printf("neck_base_pan_diff = %g, ",neck_base_pan_diff);
base_pan_com.ClearAll();
base_pan_com.Set(BASE_PAN, posbuffer[BASE_PAN] + (-0.4f * neck_base_pan_diff));
//printf("currently at %g , send base pan to %g \n",posbuffer[BASE_PAN], posbuffer[BASE_PAN] + (-1.0f * neck_base_pan_diff));
base_pan_com.Execute();
lastmove_basepan = curtime;
}
//if eye tilt is beyond threshold and the last head tilt movement happens //in more than 2 sec ago, move head and neck tilt accordingly
float head_eye_tilt_diff, head_roll_tilt_diff;
head_eye_tilt_diff = posbuffer[EYE_TILT];
if (curtime - lastmove_headtilt > 1 && (head_eye_tilt_diff > 0.16f || head_eye_tilt_diff < -0.16f))
{
head_tilt_com.ClearAll();
head_tilt_com.Set(HEAD_TILT,
posbuffer[HEAD_TILT] +
head_eye_tilt_diff);
//we know posbuffer[HEAD_TILT] is always >= 0
if (head_eye_tilt_diff < 0 && posbuffer[HEAD_TILT] < -head_eye_tilt_diff){
head_roll_tilt_diff = -posbuffer[HEAD_TILT];
}
else
head_roll_tilt_diff = head_eye_tilt_diff;
head_tilt_com.Set(HEAD_ROLL, posbuffer[HEAD_ROLL] + (-0.7f * head_roll_tilt_diff));
head_tilt_com.Set(NECK_TILT, posbuffer[NECK_TILT] + (0.4f * head_eye_tilt_diff));
head_tilt_com.Execute();
lastmove_headtilt = curtime;
}
//if head tilt is beyond threshold and the last neck crane move is more than
//2 secs ago, move neck crane accordingly.
float neck_tilt_crane_diff = posbuffer[NECK_TILT];
float head_tilt_crane_diff = posbuffer[HEAD_TILT];
if (curtime - lastmove_neckcrane > 2 && (head_tilt_crane_diff > 0.2f || head_tilt_crane_diff < 0.05f))//neck_tilt_crane_diff < -0.08f))
{
neck_crane_com.ClearAll();
if (head_tilt_crane_diff > 0.2f)
neck_crane_com.Set(NECK_CRANE, posbuffer[NECK_CRANE] + (-2.0f * head_tilt_crane_diff));
if (neck_tilt_crane_diff < -0.08f)
neck_crane_com.Set(NECK_CRANE, posbuffer[NECK_CRANE] + (-3.0f * neck_tilt_crane_diff));
neck_crane_com.Execute();
lastmove_neckcrane = curtime;
}
//randomly switch drive
if ( lastmove_drive - curtime > 10 && rand() > 5000){
if (current_drive == SOCIAL){
current_drive = PLAY;
printf("current drive is PLAY\n");
}
else{
current_drive = SOCIAL;
printf("current drive is SOCIAL\n");
}
lastmove_drive = curtime;
}
}
}
