Avatar for user98151

user98151

Member since Feb 2019 • Last active Feb 2019
  • 0 conversations
  • 2 comments

Most recent activity

  • in Projects
    Avatar for user98151

    enter code here strong text

  • in Projects
    Avatar for user98151
    1. // Servo 0: base rotation
    2. // Servo 1: direct in/out movement
    3. // Servo 2: in/out movement via linkage
    4. // Servo 3: gripper
    5. // Servo pins
    6. var SERVOS = [C4,B15,B14,B13];
    7. // Servo home pulse widths - tweak these for different arms (depending on assembly)
    8. var SERVOHOME = [1.51,1.5,1.5,1.5];
    9. // gripper positions
    10. var JOPEN = 0;
    11. var JCLOSE = 0.45;
    12. // Keyframes for movement of arm. Each line
    13. // is a position, and we move between them
    14. // at 2fps
    15. // Pick up token, move it to the sensor
    16. var KF_PICKUP = [
    17. [ 0, 0, 0, JOPEN ], // default position
    18. // [ 0, 0.3, -0.1, JOPEN ],
    19. [ 0, 0.25, -0.1, JOPEN ],
    20. [ 0, 0.45, -0.3, JOPEN ], // over token here
    21. [ 0, 0.45, -0.3, JCLOSE ], // over token here
    22. [ 0, 0.25, -0.1, JCLOSE ],
    23. [ 0, 0, 0, JCLOSE ],
    24. [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
    25. [ 0.75, -0.3, 0, JCLOSE ],
    26. ];
    27. // Hold steady at the sensor
    28. var KF_TEST_RGB = [ // just hold steady for 2 secs
    29. [ 0.75, -0.3, 0, JCLOSE ],
    30. [ 0.75, -0.3, 0, JCLOSE ],
    31. [ 0.75, -0.3, 0, JCLOSE ],
    32. [ 0.75, -0.3, 0, JCLOSE ],
    33. [ 0.75, -0.3, 0, JCLOSE ],
    34. ];
    35. // move from the sensor to a location, release gripper, go home
    36. var KF_DOWN_LEFT = [
    37. [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
    38. [ 0.3, 0.45, -0.3, JCLOSE ], // down left
    39. [ 0.3, 0.45, -0.3, JOPEN ],
    40. [ 0, 0, 0, JOPEN ], // home
    41. [ 0, 0, 0, JOPEN ],
    42. ];
    43. var KF_DOWN_RIGHT = [
    44. [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
    45. [ -0.3, -0.3, 0, JCLOSE ],
    46. [ -0.3, 0.7, 0.0, JCLOSE ], // down left
    47. [ -0.3, 0.7, 0.0, JOPEN ],
    48. [ 0, 0, 0, JOPEN ], // home
    49. [ 0, 0, 0, JOPEN ],
    50. ];
    51. var KF_DOWN_RIGHT_NEAR = [
    52. [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
    53. [ -0.4, -0.3, 0, JCLOSE ],
    54. [ -0.4, -0.2, -0.4, JCLOSE ],
    55. [ -0.4, -0.2, -0.4, JOPEN ], // down left
    56. [ 0, 0, 0, JOPEN ], // home
    57. [ 0, 0, 0, JOPEN ],
    58. ];
    59. // variables for keyframe movement
    60. var keyInterval;
    61. var keyPos = 0;
    62. var keyEnd = 0;
    63. var keyFrames = [];
    64. var keyCallback;
    65. var rgbAvr = [0,0,0]; // average RGB value
    66. var tcs; // rgb sensor
    67. var isRunning = false; // for on/off button
    68. var ledRGBs = new Uint8Array(25*3); // values for LED strip
    69. // Called to animate between keyframes
    70. function step() {
    71. keyPos += 0.04; // 20ms * 2fps
    72. var n = 0|keyPos;
    73. var a = keyPos-n;
    74. var n2 = n+1;
    75. //print(keyPos,n,n2,a);
    76. for (var i in SERVOS) {
    77. // interpolate between keyframes
    78. var p = keyFrames[n][i]*(1-a) + keyFrames[n2][i]*a;
    79. // move servo
    80. digitalPulse(SERVOS[i], 1, SERVOHOME[i]+p);
    81. }
    82. // are we finished?
    83. if (keyPos >= keyEnd) {
    84. clearInterval(keyInterval);
    85. keyInterval = undefined;
    86. if (keyCallback) keyCallback();
    87. }
    88. }
    89. // Play back an array of keyframes, call callback when done
    90. function playback(frames, callback) {
    91. keyPos = 0 - 0.02;
    92. keyEnd = frames.length-2;
    93. keyFrames = frames;
    94. keyCallback = callback;
    95. keyInterval = setInterval(step, 20);
    96. }
    97. // Test a single token
    98. function testToken() {
    99. // pick it up
    100. playback(KF_PICKUP, function() {
    101. // then wait with it by the sensor for 2 secs
    102. playback(KF_TEST_RGB, function() {
    103. // print the colour values and work out if it's red, green or blue
    104. // finally put it down in the right place and start again
    105. print(rgbAvr);
    106. if (rgbAvr[0] > rgbAvr[1] && rgbAvr[0] > rgbAvr[2]) { // red
    107. print("Red!");
    108. playback(KF_DOWN_LEFT, testToken);
    109. } else if (rgbAvr[1] > rgbAvr[0] && rgbAvr[1] > rgbAvr[2]) { // green
    110. print("Green!");
    111. playback(KF_DOWN_RIGHT, testToken);
    112. } else {
    113. print("Blue!");
    114. playback(KF_DOWN_RIGHT_NEAR, testToken); // blue
    115. }
    116. });
    117. });
    118. }
    119. // Get RGB value from the sensor
    120. function getRGB() {
    121. // get the value
    122. var v = tcs.getValue();
    123. // re-scale it, to get full saturation and lightness (leaving just hue)
    124. var min = Math.min(v.red, v.green, v.blue);
    125. var range = Math.max(v.red, v.green, v.blue) - min;
    126. if (range<1) range=1;
    127. var rgb = [ // values between 0 and 255
    128. (v.red - min)*200/range, // tweak red - so we don't accidentally classify green sometimes
    129. (v.green - min)*255/range,
    130. (v.blue - min)*255/range,
    131. ];
    132. // handle the average
    133. rgbAvr[0] = rgbAvr[0]*0.9 + rgb[0]*0.1;
    134. rgbAvr[1] = rgbAvr[1]*0.9 + rgb[1]*0.1;
    135. rgbAvr[2] = rgbAvr[2]*0.9 + rgb[2]*0.1;
    136. // set the LED strip to the right colour
    137. for (var i=0;i<ledRGBs.length;i+=3)
    138. ledRGBs.set(rgbAvr,i);
    139. SPI1.send4bit(ledRGBs, 0b0001, 0b0011);
    140. }
    141. // Start the arm working - initialise RGB sensor and start movement
    142. function startArm() {
    143. SPI1.setup({baud:3200000, mosi:B5}); // LEDs
    144. I2C1.setup({scl:B8, sda:B9}); // RGB sensor
    145. tcs = require("TCS3472x").connect(I2C1,
    146. 1 /*integration cycles*/,
    147. 1 /*gain*/);
    148. testToken(); // start arm movement
    149. setTimeout(function() {
    150. // we seem to need a delay after initialisation before we use the RGB sensor
    151. setInterval(getRGB,20);
    152. },1000);
    153. }
    154. // On/off button
    155. setWatch(function() {
    156. if (!isRunning) {
    157. LED1.set();
    158. isRunning = true;
    159. startArm();
    160. } else {
    161. LED1.reset();
    162. isRunning = false;
    163. clearInterval();
    164. }
    165. }, BTN, {edge:"rising", repeat:true, debounce:50});
    166. // For testing - call startTest() and then change the
    167. // values in the test array to work out the positions
    168. // needed for the keyframes array
    169. var test = [0,0,0,0];
    170. function startTest() {
    171. setInterval(function() {
    172. for (var i in SERVOS) {
    173. digitalPulse(SERVOS[i], 1, SERVOHOME[i]+test[i]);
    174. }
    175. }, 20);
    176. }
    177. //startTest()
Actions