SHARE
TWEET

Everything Combined 5-21-19

a guest May 21st, 2019 62 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #pragma config(Sensor, in2,    ArmLF,          sensorLineFollower)
  2. #pragma config(Sensor, in3,    CDLF,           sensorLineFollower)
  3. #pragma config(Sensor, dgtl1,  distance,       sensorSONAR_cm)
  4. #pragma config(Sensor, dgtl3,  eStop,          sensorTouch)
  5. #pragma config(Sensor, dgtl4,  en,             sensorQuadEncoder)
  6. #pragma config(Motor,  port2,           mGrip,         tmotorVex393_MC29, openLoop)
  7. #pragma config(Motor,  port3,           mSlider,       tmotorVex393_MC29, openLoop)
  8. #pragma config(Motor,  port4,           mRotate,       tmotorServoContinuousRotation, openLoop)
  9. #pragma config(Motor,  port5,           mm,            tmotorVex393_MC29, openLoop)
  10. #pragma config(Motor,  port6,           lidStamper,    tmotorVex393_MC29, openLoop)
  11. #pragma config(Motor,  port7,           pmCyl,         tmotorVex393_MC29, openLoop)
  12. #pragma config(Motor,  port8,           chestClose,    tmotorVex393_MC29, openLoop)
  13. #pragma config(Motor,  port9,           coinStamper,   tmotorVex393_MC29, openLoop)
  14. //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  15.  
  16. //mm is the cd racks motor
  17. //cm is the counters motor
  18. //km is the lid closer motor
  19.  
  20. int counter = 0;
  21. int coinsPassed = 0;
  22. int cdRotations = 0;
  23.  
  24. task e_stop()
  25. {
  26.   while(true)
  27.   {
  28.     if(SensorValue(eStop) == 1)
  29.     {
  30.         stopAllTasks();      // ends the program and all tasks including task main.
  31.     }
  32.         wait1Msec(10);      // prevents the current task from using majority of available CPU capacity
  33.   }
  34. }
  35. task cdCounter()
  36. {
  37.     while(true) {
  38.         if(SensorValue(CDLF) < 1800 && SensorValue(CDLF) > 1400) {
  39.             coinsPassed = coinsPassed + 1;
  40.             waitUntil(SensorValue(CDLF) > 1800); //Wait until coin passes
  41.         }
  42.  
  43.         if(coinsPassed == 4) {
  44.             coinsPassed = 0;
  45.         }
  46.  
  47.         wait1Msec(5);
  48.     }
  49. }
  50. bool goTo(tMotor m, float point, int speed)
  51. {
  52.     while(SensorValue(en) != point)
  53.     {
  54.         if(SensorValue(en) > point)
  55.         {
  56.             startMotor(m, -1 * speed);
  57.         }
  58.         else
  59.         {
  60.             startMotor(m, speed);
  61.         }
  62.     }
  63.     stopMotor(m);
  64.     return true;
  65. }
  66.  
  67. /* Will attempt to go to an exact point 6 times
  68. If after 6 attempts it can't get the exact value requested it will
  69. accept a value that is +- 1 of the requested value */
  70. bool goToExactValue(tMotor m, float point)
  71. {
  72.     int completed = false;
  73.     int fixAttempts = 0; //Here so robot doesn't get stuck and try to correct itself forever
  74.  
  75.     while(!completed) {
  76.         waitUntil(goTo(m, point, 19));
  77.         wait(.25);  //Allow the motor to fully stop before correction
  78.         if(SensorValue(en) != point  && fixAttempts <= 5) {
  79.             waitUntil(goTo(m, point, 19));
  80.             fixAttempts = fixAttempts + 1;
  81.         } else {
  82.             completed = (SensorValue(en) == point || SensorValue(en) == point + 1 || SensorValue(en) == point - 1);
  83.         }
  84.     }
  85.  
  86.     return true;
  87. }
  88.  
  89. bool manipulateGrip(bool open) {
  90.     if(open) {
  91.         startMotor(mGrip, 40);
  92.         } else {
  93.         startMotor(mGrip, -40);
  94.     }
  95.  
  96.     wait(.5);
  97.     stopMotor(mGrip);
  98.     return true;
  99. }
  100.  
  101. bool moveSlider(float point)
  102. {
  103.     if(SensorValue(distance) > point) {
  104.         startMotor(mSlider, -25);
  105.         waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
  106.         startMotor(mSlider, 25);
  107.         waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
  108.     } else {
  109.         startMotor(mSlider, 25);
  110.         waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
  111.         startMotor(mSlider, -25);
  112.         waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
  113.     }
  114.     stopMotor(mSlider);
  115.  
  116.     return true;
  117. }
  118.  
  119. bool rotateArm(bool home) {
  120.     if (home) {
  121.         setServo(mRotate, -15);
  122.         } else {
  123.         setServo(mRotate, -120);
  124.     }
  125.  
  126.     wait(1);
  127.  
  128.     return true;
  129. }
  130.  
  131. task cdRack()
  132. {
  133.     SensorValue(en) = 0;
  134.     //startMotor(cm, 20);
  135.     startTask(cdCounter);
  136.  
  137.     while(true) {
  138.         if(coinsPassed == 4) {
  139.             //Wiggle before moving
  140.             waitUntil(goTo(mm, (90 * cdRotations) + 10, 100));
  141.             waitUntil(goTo(mm, (90 * cdRotations) - 10, 100));
  142.             cdRotations = cdRotations + 1;
  143.             waitUntil(goToExactValue(mm, 90 * cdRotations));// No longer using the gear makes corrections not needed
  144.             //waitUntil(goTo(mm, 90 * cdRotations, 30));
  145.         }
  146.     }
  147. }
  148.  
  149. task main()
  150. {
  151.     ///startTask(e_stop);
  152.     startTask(cdRack);
  153.  
  154.     waitUntil(manipulateGrip(true)); //open gripper
  155.     waitUntil(rotateArm(true)); //Move the arm to the home position
  156.  
  157.     while(true) {
  158.         if(SensorValue(ArmLF) < 2800 && counter <= 3) {
  159.             waitUntil(manipulateGrip(false)); //close gripper
  160.             int position = (counter * 3) + 6;
  161.             waitUntil(rotateArm(false));//Rotate the arm
  162.             waitUntil(moveSlider(position)); //Move slider to position
  163.             counter = counter + 1;
  164.  
  165.             waitUntil(manipulateGrip(true)); //open the gripper
  166.             wait(2);
  167.             waitUntil(moveSlider(6)); //Move slider back home
  168.             waitUntil(rotateArm(true)); //Move arm back to home
  169.  
  170.             if(counter == 3) {
  171.                 counter = 0;
  172.             }
  173.         }
  174.         waitInMilliseconds(50);
  175.     }
  176.  
  177. }
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