SHARE
TWEET

Untitled

a guest Apr 23rd, 2019 63 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 METHOD 1
  46.        %[bigDC, smallDC] = estimateHorizontally(spot);
  47.        
  48.        %ESTIMATE METHOD 2
  49.        %[bigDC, smallDC] = estimateVertically(spot);
  50.        
  51.        %ESTIMATE METHOD 3
  52.        [a, b] = estimateHorizontally(spot);
  53.        [c, d] = estimateVertically(spot);
  54.        bigDC = mean([a c]);
  55.        smallDC = mean([b d]);
  56.     else
  57.         [bigDC, smallDC] = coordToValues(coord);
  58.     end
  59.    
  60.     %% Change string coordinate to xy vals
  61.     function [x, y] = coordToValues(coord)
  62.         coord = strrep(coord, '(', '');
  63.         coord = strrep(coord, ')', '');
  64.        
  65.         coord = strrep(coord, ' ', '');
  66.         split = strsplit(coord, ',');
  67.         x = str2double(split{1});
  68.         y = str2double(split{2});
  69.     end
  70.     function [x, y] = estimateHorizontally(spot)
  71.         x = 0; y = 0;
  72.         % get left coord
  73.         lSpot = spot;
  74.         coord = nan;
  75.         while isnan(coord)
  76.             lSpot = strcat(char(lSpot(1) - 1), lSpot(2));
  77.             coord = alldata{lSpot(2) - 48, lSpot(1) - 96};
  78.         end
  79.         [leftX, leftY] = coordToValues(coord);
  80.        
  81.         % get right coord
  82.         rSpot = spot;
  83.         coord = nan;
  84.         while isnan(coord)
  85.             rSpot = strcat(char(rSpot(1) + 1), rSpot(2));
  86.             coord = alldata{rSpot(2) - 48, rSpot(1) - 96};
  87.         end
  88.         [rightX, rightY] = coordToValues(coord);
  89.        
  90.         % Line b/t 2
  91.         deltaX = rightX - leftX;
  92.         slopeX = deltaX / (rSpot(1) - lSpot(1));
  93.         x = leftX + slopeX * (spot(1) - lSpot(1));
  94.        
  95.         deltaY = rightY - leftY;
  96.         slopeY = deltaY / (rSpot(1) - lSpot(1));
  97.         y = leftY + slopeY * (spot(1) - lSpot(1));
  98.     end
  99.  
  100.     function [x, y] = estimateVertically(spot)
  101.         % lspot is coord above, rspot is coord below
  102.         x = 0; y = 0;
  103.         % get up coord
  104.         lSpot = spot;
  105.         coord = nan;
  106.         while isnan(coord)
  107.             lSpot = strcat(lSpot(1), char(lSpot(2) - 1));
  108.             coord = alldata{lSpot(2) - 48, lSpot(1) - 96};
  109.         end
  110.         [leftX, leftY] = coordToValues(coord);
  111.        
  112.         % get down coord
  113.         rSpot = spot;
  114.         coord = nan;
  115.         while isnan(coord)
  116.             rSpot = strcat(rSpot(1), char(rSpot(2) + 1));
  117.             coord = alldata{rSpot(2) - 48, rSpot(1) - 96};
  118.         end
  119.         [rightX, rightY] = coordToValues(coord);
  120.        
  121.         % Line b/t 2
  122.         deltaX = rightX - leftX;
  123.         slopeX = deltaX / (rSpot(2) - lSpot(2));
  124.         x = leftX + slopeX * (spot(2) - lSpot(2));
  125.        
  126.         deltaY = rightY - leftY;
  127.         slopeY = deltaY / (rSpot(2) - lSpot(2));
  128.         y = leftY + slopeY * (spot(2) - lSpot(2));
  129.     end
  130. 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