/** * Tank drive using the Kinect. */ void Autonomous(void) { double leftAxis, rightAxis; double leftAngle, rightAngle, headAngle, rightLegAngle, leftLegAngle, rightLegYZ, leftLegYZ=0; bool dataWithinExpectedRange; bool buttons[8]; while (IsAutonomous()) { /* Only process data if skeleton is tracked */ if (kinect->GetTrackingState() == Kinect::kTracked) { /* Determine angle of each arm and map to range -1,1 */ leftAngle = AngleXY(kinect->GetSkeleton().GetShoulderLeft(), kinect->GetSkeleton().GetWristLeft(), true); rightAngle = AngleXY(kinect->GetSkeleton().GetShoulderRight(), kinect->GetSkeleton().GetWristRight(), false); leftAxis = CoerceToRange(leftAngle, -70, 70, -1, 1); rightAxis = CoerceToRange(rightAngle, -70, 70, -1, 1); /* Check if arms are within valid range and at approximately the same z-value */ dataWithinExpectedRange = (leftAngle < ARM_MAX_ANGLE) && (leftAngle > ARM_MIN_ANGLE) && (rightAngle < ARM_MAX_ANGLE) && (rightAngle > ARM_MIN_ANGLE); dataWithinExpectedRange = dataWithinExpectedRange && InSameZPlane(kinect->GetSkeleton().GetShoulderLeft(), kinect->GetSkeleton().GetWristLeft(), Z_PLANE_TOLERANCE) && InSameZPlane(kinect->GetSkeleton().GetShoulderRight(), kinect->GetSkeleton().GetWristRight(), Z_PLANE_TOLERANCE); /* Determine the head angle and use it to set the Head buttons */ headAngle = AngleXY(kinect->GetSkeleton().GetShoulderCenter(), kinect->GetSkeleton().GetHead(), false); buttons[0] = headAngle > HEAD_LEFT; buttons[1] = headAngle < HEAD_RIGHT; /* Calculate the leg angles in the XY plane and use them to set the Leg Out buttons */ leftLegAngle = AngleXY(kinect->GetSkeleton().GetHipLeft(), kinect->GetSkeleton().GetAnkleLeft(), true); rightLegAngle = AngleXY(kinect->GetSkeleton().GetHipRight(), kinect->GetSkeleton().GetAnkleRight(), false); buttons[2] = leftLegAngle > LEG_OUT; buttons[3] = rightLegAngle > LEG_OUT; /* Calculate the leg angle in the YZ plane and use them to set the Leg Forward and Leg Back buttons */ leftLegYZ = AngleYZ(kinect->GetSkeleton().GetHipLeft(), kinect->GetSkeleton().GetAnkleLeft(), false); rightLegYZ = AngleYZ(kinect->GetSkeleton().GetHipRight(), kinect->GetSkeleton().GetAnkleRight(), false); buttons[4] = rightLegYZ < LEG_FORWARD; buttons[5] = rightLegYZ > LEG_BACKWARD; buttons[6] = rightLegYZ < LEG_FORWARD; buttons[7] = rightLegYZ > LEG_BACKWARD; if (dataWithinExpectedRange){ /** * Drives using the Kinect axes scaled to 1/3 power * Axes are inverted so arms up == joystick pushed away from you */ myRobot.TankDrive(-leftAxis*.33, -rightAxis*.33); /** * Do something with boolean "buttons" here */ /* Optional SmartDashboard display of Kinect values */ //dash->PutDouble("Left Arm", -leftAxis); //dash->PutDouble("Right Arm", -rightAxis); //dash->PutBoolean("Head Left", buttons[0]); //dash->PutBoolean("Head Right", buttons[1]); //...etc... } else{ /* Arms are outside valid range */ myRobot.TankDrive(0.0, 0.0); /** * Do default behavior with boolean "buttons" here */ /* Optional SmartDashboard display of Kinect values */ //dash->PutDouble("Left Arm", 0); //dash->PutDouble("Right Arm", 0); //dash->PutBoolean("Head Left", false); //dash->PutBoolean("Head Right", false); //...etc... } } else{ /* Skeleton not tracked */ myRobot.TankDrive(0.0, 0.0); /** * Do default behavior with boolean "buttons" here */ /* Optional SmartDashboard display of Kinect values */ //dash->PutDouble("Left Arm", 0); //dash->PutDouble("Right Arm", 0); //dash->PutBoolean("Head Left", false); //dash->PutBoolean("Head Right", false); //...etc... } Wait(0.01); } }