close all; clc; clear; %% -------------------- Lane & Gear Parameters -------------------- %% % Lane geometry laneWidth_in = 6; % total lane width in inches halfLane_in = laneWidth_in/2; % 3 in each side % Gear / rack parameters (Mod 1, 16-tooth spur gear) module_mm = 1; % metric module (mm per tooth) gearTeeth = 16; mm_per_in = 25.4; pitchDiameter_mm = module_mm * gearTeeth; % d = m*z travelPerRev_mm = pi * pitchDiameter_mm; % linear travel per revolution travelPerRev_in = travelPerRev_mm / mm_per_in; % in/rev maxRotations = halfLane_in / travelPerRev_in; % max rev from center to side fprintf('Travel per rev: %.3f in\n', travelPerRev_in); fprintf('Max rotations from center: %.3f rev\n', maxRotations); %% -------------------- Fixed lane pixel measurements -------------------- %% % These are from your calibration and do NOT change with setup laneLeft_px_meas = 900.0; % one side of lane laneRight_px_meas = 900.0; % other side of lane % Ensure left < right logically laneLeft_px = min(laneLeft_px_meas, laneRight_px_meas); laneRight_px = max(laneLeft_px_meas, laneRight_px_meas); laneCenter_px = (laneLeft_px + laneRight_px) / 2; pixelSpan = abs(laneRight_px - laneLeft_px); inchesPerPixel = laneWidth_in / pixelSpan; fprintf('Lane left pixel (stored): %.1f\n', laneLeft_px); fprintf('Lane right pixel (stored): %.1f\n', laneRight_px); fprintf('Lane center pixel (stored): %.1f\n', laneCenter_px); fprintf('Pixel span: %.3f px\n', pixelSpan); fprintf('Inches per pixel (fixed): %.5f in/px\n', inchesPerPixel); %% expose to base for functions using evalin assignin('base', 'laneLeft_px', laneLeft_px); assignin('base', 'laneRight_px', laneRight_px); assignin('base', 'laneCenter_px', laneCenter_px); assignin('base', 'inchesPerPixel', inchesPerPixel); %% -------------------- Initialize Camera & Simulink Vars -------------------- %% cam = webcam('Brio 100'); % Optional: tame exposure a bit (adjust as needed) try cam.ExposureMode = 'manual'; cam.Exposure = -5; catch disp('Could not set manual exposure on this camera (OK to ignore).'); end % Variables used by Simulink (in base workspace) evalin('base', 'targetRot = 0;'); evalin('base', 'newTargetRotAvailable = 0;'); evalin('base', 'currentRot = 0;'); assignin('base', 'travelPerRev_in', travelPerRev_in); assignin('base', 'maxRotations', maxRotations); assignin('base', 'halfLane_in', halfLane_in); fprintf('Initialized workspace variables for Simulink.\n'); %% -------------------- Capture Game Image -------------------- %% fprintf("Waiting to capture game state image...\n"); dialogBox("game board"); rawImg = snapshot(cam); % OPTIONAL HORIZONTAL FLIP: % Leave this ON if the image looks mirrored vs real life. fore = fliplr(rawImg); gameState.foreGnd = fore; [height,width,~] = size(gameState.foreGnd); figure; imshow(gameState.foreGnd); title('Raw Game Image (possibly flipped)'); %% -------------------- Pin Detection via HSV (separate + max 6) ----------- %% hsvImg = rgb2hsv(gameState.foreGnd); h = hsvImg(:,:,1); s = hsvImg(:,:,2); v = hsvImg(:,:,3); % 1) Color + saturation thresholds for orange pins maskHue = (h > 0.03 & h < 0.14); maskSat = (s > 0.45); % 2) Brightness threshold to kill shadows maskVal = (v > 0.35); % tweak 0.3–0.45 if needed pinMask = maskHue & maskSat & maskVal; % Remove tiny specks pinMask = bwareaopen(pinMask, 100); % Gentle cleanup (keep shapes, don't over-fuse) SE = strel('disk', 5); pinMask = imopen(pinMask, SE); pinMask = imclose(pinMask, SE); pinMask = imfill(pinMask, 'holes'); figure; imshow(pinMask); title('Cleaned Binary Foreground (Pins Only)'); % -------- Erode to break thin connections between touching pins -------- desiredMaxPins = 6; pinCore = []; statsCore = []; for r = 2:5 % try disk radius 2,3,4,5 coreSE = strel('disk', r); candidateCore = imerode(pinMask, coreSE); candidateCore = bwareaopen(candidateCore, 50); % remove tiny bits candidateStats = regionprops(candidateCore, 'Centroid','Area'); fprintf(' r=%d -> %d regions\n', r, numel(candidateStats)); % Keep this candidate; we'll pick the best later pinCore = candidateCore; statsCore = candidateStats; % If we have at least as many blobs as there are pins, we can stop if numel(candidateStats) >= desiredMaxPins break; end end figure; imshow(pinCore); title('Eroded core mask used for centroids'); if isempty(statsCore) warning('No regions found in eroded core mask – adjust HSV/value thresholds.'); pinPos = []; else % -------- HERE: enforce "6 buttons max, largest spots" ---------------- areas = [statsCore.Area]; [~, sortIdx] = sort(areas, 'descend'); % largest first keepN = min(desiredMaxPins, numel(sortIdx)); % up to 6, or fewer if fewer blobs topIdx = sortIdx(1:keepN); pinPos = zeros(keepN, 2); for k = 1:keepN pinPos(k,:) = statsCore(topIdx(k)).Centroid; end end fprintf('Using %d pin centroid(s) for buttons.\n', size(pinPos,1)); % Debug overlay to check centroids actually used figure; imshow(gameState.foreGnd); title('Debug: centroids overlay'); hold on; for i = 1:size(pinPos,1) plot(pinPos(i,1), pinPos(i,2), 'r+', 'MarkerSize', 15, 'LineWidth', 2); end hold off; %% -------------------- GUI Configuration (smaller window) -------------------- %% scale = 0.5; % 0.5 = 50% of image size figWidth = width * scale; figHeight = height * scale; screenSize = get(0, 'ScreenSize'); screenWidth = screenSize(3); screenHeight = screenSize(4); figX = (screenWidth - figWidth ) / 2; figY = (screenHeight - figHeight) / 2; fig = uifigure('Name', 'Robot Bowling Control', ... 'Position', [figX, figY, figWidth, figHeight]); ax = uiaxes(fig, 'Position', [0, 0, figWidth, figHeight]); imshow(gameState.foreGnd, 'Parent', ax); title(ax, 'Select a Pin to Target'); % Motor (bowler) position in IMAGE coordinates (right side middle) motorPosition.x = width * 0.92; motorPosition.y = height * 0.50; hold(ax, 'on'); plot(ax, motorPosition.x, motorPosition.y, 'ro', 'MarkerSize', 10, 'LineWidth', 2); hold(ax, 'off'); ax.DataAspectRatio = [1 1 1]; ax.XLim = [1 width]; ax.YLim = [1 height]; % Scale factors for image → GUI button coordinates xScale = figWidth / width; yScale = figHeight / height; % Overlay buttons at pin positions for i = 1:size(pinPos, 1) uix = pinPos(i,1) * xScale; uiy = figHeight - (pinPos(i,2) * yScale); % flip Y btn = uibutton(fig, 'push', ... 'Text', 'Select', ... 'Position', [uix-20, uiy-15, 40, 30], ... 'ButtonPushedFcn', @(btn, event) moveMotorToPin([pinPos(i,1), pinPos(i,2)], ... fig, motorPosition), ... 'FontSize', 8); btn.BackgroundColor = [0.2 0.8 0.2]; end %% ========================== LOCAL FUNCTIONS ========================== %% function dialogBox(currentImage) d = dialog('Position',[300 300 260 150],'Name','Image Acquisition'); textString = "Click Advance when " + currentImage + " is ready"; uicontrol('Parent',d,... 'Style','text',... 'Position',[20 80 220 40],... 'String',textString); uicontrol('Parent',d,... 'Position',[95 20 70 25],... 'String','Advance',... 'Callback','delete(gcf)'); uiwait(d); end function sendRotationsToMotor(targetRot) assignin('base', 'targetRot', targetRot); assignin('base', 'newTargetRotAvailable', 1); set_param('working', 'SimulationCommand', 'update'); fprintf('targetRot = %.3f rev sent to Simulink workspace\n', targetRot); startSimulinkModel(); end function slowMoveToRot(targetRot) try currentRot = evalin('base','currentRot'); catch currentRot = 0; end maxStep = 0.05; % rev per step (smaller = slower) dt = 0.1; % seconds between steps fprintf('Slow move: currentRot = %.3f, targetRot = %.3f\n', currentRot, targetRot); while abs(targetRot - currentRot) > 1e-3 step = maxStep * sign(targetRot - currentRot); if abs(targetRot - currentRot) < abs(step) currentRot = targetRot; else currentRot = currentRot + step; end assignin('base','currentRot', currentRot); assignin('base','targetRot', currentRot); assignin('base','newTargetRotAvailable', 1); set_param('working', 'SimulationCommand', 'update'); fprintf(' -> commanding %.3f rev\n', currentRot); pause(dt); end fprintf('Reached targetRot = %.3f rev (slow move).\n', targetRot); end function startSimulinkModel() modelName = 'working'; if ~bdIsLoaded(modelName) try load_system(modelName); fprintf('Simulink model "%s" loaded\n', modelName); catch fprintf('Could not load Simulink model "%s". Check model name.\n', modelName); return; end end % If you want auto-start: % set_param(modelName, 'SimulationCommand', 'start'); % fprintf('Simulation started\n'); end function targetRot = moveMotorToPin(objectLoc, fig, motorPosition) %#ok fprintf("Calculating motor rotations for selected pin...\n"); laneCenter_px = evalin('base', 'laneCenter_px'); inchesPerPixel = evalin('base', 'inchesPerPixel'); travelPerRev_in = evalin('base', 'travelPerRev_in'); maxRotations = evalin('base', 'maxRotations'); halfLane_in = evalin('base', 'halfLane_in'); pinX_px = objectLoc(1); pinY_px = objectLoc(2); %#ok fprintf("Pin centroid (pixels): (%.1f, %.1f)\n", pinX_px, pinY_px); fprintf("Lane center (pixels): %.1f\n", laneCenter_px); % Pixels -> inches from lane center offset_in = (pinX_px - laneCenter_px) * inchesPerPixel; % + right, - left fprintf("Desired offset from lane center: %.3f in\n", offset_in); % Clamp to physical lane edges offset_in_clamped = max(min(offset_in, halfLane_in), -halfLane_in); if abs(offset_in - offset_in_clamped) > 1e-3 fprintf("Offset clamped from %.3f in to %.3f in (lane edge)\n", ... offset_in, offset_in_clamped); end % Inches -> rotations targetRot = offset_in_clamped / travelPerRev_in; % Clamp rotations to mechanical limit targetRot_clamped = max(min(targetRot, maxRotations), -maxRotations); if abs(targetRot - targetRot_clamped) > 1e-3 fprintf("Rotations clamped from %.3f rev to %.3f rev (rack limit)\n", ... targetRot, targetRot_clamped); end targetRot = targetRot_clamped; fprintf("Commanded target rotations from center: %.3f rev\n", targetRot); % Move gently (slowed) to the new rotation slowMoveToRot(targetRot); end