Function executeTurn

frontend/src/hooks/useBlockProgramExecution.tsx:370–569

(degrees: number, direction: 'left' | 'right')

  // Execute turn command with odometry feedback
  const executeTurn = async (degrees: number, direction: 'left' | 'right'): Promise<void> => {
    // Validate odometry before starting
    if (!isValidOdometry()) {
      throw new Error('Invalid odometry data - cannot execute turn');
    }
    
    const radians = (degrees * Math.PI) / 180;
    const startOrientation = { ...odometry.pose.pose.orientation };
    const startYaw = getYawFromQuaternion(startOrientation);
    const angularVel = direction === 'left' ? ANGULAR_VELOCITY : -ANGULAR_VELOCITY;
    const slowAngularVel = angularVel * 0.3; // 30% speed for final approach
    
    // Add minimum execution time based on expected rotation speed
    const minExecutionTime = Math.max(500, (radians / Math.abs(ANGULAR_VELOCITY)) * 1000 * 0.8); // 80% of theoretical time
    const maxExecutionTime = (radians / Math.abs(ANGULAR_VELOCITY)) * 1000 * 3; // 3x theoretical time as absolute max
    const startTime = Date.now();
    
    // Debug logging
    console.log('Starting turn:', {
      degrees,
      radians,
      direction,
      startYaw,
      angularVel,
      minExecutionTime,
      maxExecutionTime
    });
    
    return new Promise((resolve, reject) => {
      let lastYaw = startYaw;
      let totalAngleTurned = 0;
      let stuckCounter = 0;
      
      const intervalId = setInterval(() => {
        if (executionRef.current.abortController?.signal.aborted) {
          clearInterval(intervalId);
          publishVelocity({
            linear: new Vector3({ x: 0, y: 0, z: 0 }),
            angular: new Vector3({ x: 0, y: 0, z: 0 })
          });
          reject(new Error('Execution aborted'));
          return;
        }

        const currentOrientation = odometry.pose.pose.orientation;
        const currentYaw = getYawFromQuaternion(currentOrientation);
        const elapsedTime = Date.now() - startTime;
        
        // Calculate angle turned in this iteration
        const deltaAngle = calculateAngleDifference(lastYaw, currentYaw);
        
        // Accumulate total angle turned (handling wrap-around)
        if (direction === 'left' && deltaAngle > 0) {
          totalAngleTurned += deltaAngle;
        } else if (direction === 'right' && deltaAngle < 0) {
          totalAngleTurned += Math.abs(deltaAngle);
        }