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; } } }