示例#1
0
文件: main.c 项目: applsdev/LeapC
void on_frame(leap_controller_ref controller, void *user_info)
{
    leap_frame_ref frame = leap_controller_copy_frame(controller, 0);
    printf("frame %llu\n", leap_frame_timestamp(frame));

    for (int i = 0; i < leap_frame_hands_count(frame); i++) {
        leap_hand_ref hand = leap_frame_hand_at_index(frame, i);
        printf("\thand %i: fingers=%i\n", leap_hand_id(hand), leap_hand_fingers_count(hand));

        for (int p = 0; p < leap_hand_fingers_count(hand); p++) {
            leap_pointable_ref pointable = leap_hand_finger_at_index(hand, p);
            leap_vector tip_position = leap_pointable_tip_position(pointable);
            printf("\t\tfinger %i: tip-pos={%0.2f, %0.2f, %0.2f}\n",
                   leap_pointable_id(pointable), tip_position.x, tip_position.y, tip_position.z);
        }

        for (int p = 0; p < leap_hand_tools_count(hand); p++) {
            leap_pointable_ref pointable = leap_hand_tool_at_index(hand, p);
            leap_vector tip_position = leap_pointable_tip_position(pointable);
            printf("\t\ttool   %i: tip-pos={%0.2f, %0.2f, %0.2f}\n",
                   leap_pointable_id(pointable), tip_position.x, tip_position.y, tip_position.z);
        }
    }
    leap_frame_delete(frame);
}
void jamieOnFrame(leap_hand_ref hand,leap_frame_ref frame, CCrazyflie* cflie){
	//printf("JAMIEonframe");
	leap_vector current_position;
	leap_vector current_direction;
	leap_vector current_velocity;
	leap_vector current_normal; // perpendicular line to direction

	// Gets number of fingers on screen
	int num_fingers = leap_hand_fingers_count(hand); 
	int num_hands = leap_frame_hands_count(frame);
	
	// Gets current position, direction, velocity of hand from Leap
	leap_hand_palm_position(hand, &current_position);
	leap_hand_direction(hand, &current_direction);
	leap_hand_palm_velocity(hand, &current_velocity);
	leap_hand_palm_normal(hand, &current_normal);
	
	// Print out values. Easier to determine thresholds for gestures.
	//printf("\n");
	//printf("Position: \n	X: %f \n	Y: %f \n	Z:%f", current_position.x, current_position.y, current_position.z);
	//printf("\nDirection: \n	X: %f \n	Y: %f \n	Z:%f", current_direction.x, current_direction.y, current_direction.z);
	//printf("\nVelocity: \n	X: %f \n	Y: %f \n	Z:%f", current_velocity.x, current_velocity.y, current_velocity.z);
	//printf("\nNormal: \n	X: %f \n	Y: %f \n	Z:%f", current_normal.x, current_normal.y, current_normal.z);
	//printf("\nNumber of Fingers: %d", num_fingers);
	


	//printf("\ncurrent_signal == %d\n", current_signal);

	//lock
    //pthread_mutex_lock(&lockz);


	// Only send new signal if last signal has been processed
	if (current_signal == NO_SIG){
	
		//Pitch
			// Palm is stationary. 
			// looks at Y direction of palm vector (vector from middle of palm to finger)
		if ((current_velocity.x < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.y < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.z < MINIMUM_VELOCITY_THRESHOLD)){
			if (current_direction.y < -PITCH_THRESHOLD){ // hand pointed down. increase pitch
				current_pitch = PITCH_CONSTANT;
				//printf("\n-------Decreased pitch  ");
			}else if (current_direction.y > PITCH_THRESHOLD){ // hand pointed up. decrease pitch
				current_pitch = -PITCH_CONSTANT;
				//printf("\n-------Increased pitch  ");
			}
			else {
				current_pitch = 0;
			}
		}
	
		//Roll
			// Palm is stationary. 
			// looks at X direction of palm normal vector (prependicular to vector from middle of palm to finger)
		if ((current_velocity.x < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.y < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.z < MINIMUM_VELOCITY_THRESHOLD)){
			if (current_normal.x < -ROLL_THRESHOLD){ // hand pointed left. decrease roll
				current_roll = ROLL_CONSTANT;
				//printf("\n-------Decreased roll  ");
			}else if (current_normal.x > ROLL_THRESHOLD){ // hand pointed right. increase roll
				current_roll = -ROLL_CONSTANT;
				//printf("\n-------Increased roll  ");
			}
			else {
				current_roll = 0;
			}
		}	

		//Thrust
			// Palm is stationary.
			// looks at Y position. 2-3ft above leap = increase thrust. a few inches above leap = decrease thrust
		if ((current_velocity.x < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.y < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.z < MINIMUM_VELOCITY_THRESHOLD)){
			current_thrust = current_position.y;
		}
	
	
		//FLY_SIG = Two hands. 5 fingers on each (open palm). Stationary hands
			// number of hands == 2. and each hand has 5 fingers
			// hand velocity is less than minimum threshold
		if (num_hands == 2){
				leap_hand_ref hand_0 = leap_frame_hand_at_index(frame, 0); // gets hand objects
				leap_hand_ref hand_1 = leap_frame_hand_at_index(frame, 1);
			if ((leap_hand_fingers_count(hand_0) == 5) && (leap_hand_fingers_count(hand_1) == 5)){
				if ((current_velocity.x < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.y < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.z < MINIMUM_VELOCITY_THRESHOLD)){
					
					current_signal = FLY_SIG;
					printf("----------Two hands, 5 fingers each gesture: FLY_SIG\n");
				}	
			}	
		}
	
		//LAND_SIG = Two hands. 0 fingers on each (fist). fists stationary.
			// number of hands == 2. number of fingers == 0
			// hand velocity less than minimum threshold
		if (num_hands == 2){
			leap_hand_ref hand_0 = leap_frame_hand_at_index(frame, 0);
			leap_hand_ref hand_1 = leap_frame_hand_at_index(frame, 1);
			if ((leap_hand_fingers_count(hand_0) == 2) && (leap_hand_fingers_count(hand_1) == 2)){
				if ((current_velocity.x < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.y < MINIMUM_VELOCITY_THRESHOLD) && (current_velocity.z < MINIMUM_VELOCITY_THRESHOLD)){
					
					current_signal = LAND_SIG;
					printf("----------Two hands, two fingers: LAND_SIG\n");
				}
			}
		}
	
		// CHANGE_HOVER_SIG = Swipe gesture. From right to left.
			// X velocitiy less than SWIPE_VELOCITY_THRESHOLD
			// Recognizes swipes in both directions: CHANGE_HOVER_SIG
		if ( (num_hands == 2) && (current_velocity.x < -SWIPE_VELOCITY_THRESHOLD || current_velocity.x > SWIPE_VELOCITY_THRESHOLD)){
			current_signal = CHANGE_HOVER_SIG;
			printf("----------Swipe gesture: CHANGE_HOVER_SIG\n");
		} 
	
		// POINTABLE OBJECT: FLY_PATTERN
			// Detects pointable.
			// Gets x,y,z position
			// calls record_tool_movement
		
		int num_tools = leap_hand_tools_count(hand);

//		printf("\n Number of tools: %d ", num_tools);
		if ((FLY_PATTERN == 1) && (num_tools == 1) && (num_hands == 2)){ // pointable taken off screen, done drawing pattern
			printf("\n----FLYING PATTERN----\n");
			FLY_PATTERN = 2;
			totalFrames = frame_count; 
			frame_count = 0;
			current_node = DLL_HEAD;
			
		}else if((num_tools > 0) && (FLY_PATTERN != 2)){
			//printf("\nPointable tool detected\n");

			//reset everything while recording
			current_pitch = 0;
			current_roll = 0;
			current_yaw = 0;
			current_thrust = 0;

			//hover mode while recording
			flightModeHover = 1;
			cflie->m_nThrust = HOVERTHRUST;
			cycle(cflie);

			leap_pointable_ref tool1;
			tool1 = leap_hand_tool_at_index(hand,0);
			leap_vector tool_position;

			//printf("FRAME COUNT: %d\n",frame_count);
			leap_pointable_tip_position(tool1, &tool_position);
			record_tool_movement(tool_position);
			FLY_PATTERN = 1;
		}


	}//end of if current_signal == NO_SIG
	
	//leap_pointable_ref leap_hand_finger_at_index(leap_hand_ref hand, int index);

	//unlock
    //pthread_mutex_unlock(&lockz);
}