SHARE
TWEET

square to deg v2

a guest Apr 23rd, 2019 67 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. % This function takes in a square positon on the chess board,
  2. % e.g. 'A2' or 'h8'
  3. % Returns motor angles based off calibration values
  4.  
  5. % +------------------------+
  6. % |a8                    h8|
  7. % |                        |
  8. % |                        |
  9. % |                        |
  10. % |                        |
  11. % |a1                   h1 |
  12. % +------------------------+
  13.  
  14. % TODO:
  15. % If X,Y motor axes not perpendicular and parallel to
  16. % respective sides of chess board, need to calculate
  17. % based off the board being diagonal
  18. function [bigDC, smallDC] = squareToDegrees(spot)
  19. [ndata, text, alldata] = xlsread('locations.xlsx');
  20.     % Make sure spot is 2 chars long
  21.     if length(spot) ~= 2
  22.        fprintf("Error: square spot not 2 chars.\n");
  23.        return;
  24.     end
  25.     if length(alldata) <= 4
  26.         fprintf("Error reading excel file"\n);
  27.         return;
  28.     end
  29.    
  30.     % Get ascii values for spot
  31.     ascii = double(spot);
  32.    
  33.     %% Get shit from excel
  34.     % ascii(2) = 49 for '1', 56 for '8' (-48 gives 1through8)
  35.     row = (((ascii(2) - 48)));
  36.    
  37.     % ascii(1) = 97 for a, 104 for h (-96 gives 1through8)
  38.     col = (ascii(1) - 96);
  39.    
  40.     %fprintf("row %d, col %d\n", row, col);
  41.     coord = alldata{row, col};
  42.     %fprintf("%s\n", coord);
  43.    
  44.     if length(coord) < 4
  45.        % ESTIMATE
  46.        [bigDC, smallDC] = estimateHorizontally(spot);
  47.        %[bigDC, smallDC] = estimateVertically(spot);
  48.        
  49.     else
  50.         [bigDC, smallDC] = coordToValues(coord);
  51.     end
  52.    
  53.     %% Change string coordinate to xy vals
  54.     function [x, y] = coordToValues(coord)
  55.         coord = strrep(coord, '(', '');
  56.         coord = strrep(coord, ')', '');
  57.        
  58.         coord = strrep(coord, ' ', '');
  59.         split = strsplit(coord, ',');
  60.         x = str2double(split{1});
  61.         y = str2double(split{2});
  62.     end
  63.     function [x, y] = estimateHorizontally(spot)
  64.         x = 0; y = 0;
  65.         % get left coord
  66.         lSpot = spot;
  67.         coord = nan;
  68.         while isnan(coord)
  69.             lSpot = strcat(char(lSpot(1) - 1), lSpot(2));
  70.             coord = alldata{lSpot(2) - 48, lSpot(1) - 96};
  71.         end
  72.         [leftX, leftY] = coordToValues(coord);
  73.        
  74.         % get right coord
  75.         rSpot = spot;
  76.         coord = nan;
  77.         while isnan(coord)
  78.             rSpot = strcat(char(rSpot(1) + 1), rSpot(2));
  79.             coord = alldata{rSpot(2) - 48, rSpot(1) - 96};
  80.         end
  81.         [rightX, rightY] = coordToValues(coord);
  82.        
  83.         % Line b/t 2
  84.         deltaX = rightX - leftX;
  85.         slopeX = deltaX / (rSpot(1) - lSpot(1));
  86.         x = leftX + slopeX * (spot(1) - lSpot(1));
  87.        
  88.         deltaY = rightY - leftY;
  89.         slopeY = deltaY / (rSpot(1) - lSpot(1));
  90.         y = leftY + slopeY * (spot(1) - lSpot(1));
  91.     end
  92. end
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top