/**
* 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);
}
}
