daily pastebin goal
53%
SHARE
TWEET

Untitled

a guest Nov 18th, 2017 53 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <AFMotor.h>
  2. int StepSensor1 = 20;
  3. int count1 = 0;
  4. int turn = 0;
  5. int new_val;
  6. int old_val;
  7. int iss = 1;
  8. int iss2 = 1;
  9.  
  10.  
  11. AF_DCMotor motorHor(2, MOTOR12_64KHZ);
  12. AF_DCMotor motorVer(4, MOTOR34_64KHZ);
  13.  
  14. void setup() {
  15.   // put your setup code here, to run once:
  16.   pinMode(StepSensor1, INPUT);
  17.   Serial.begin(9600);
  18.  
  19.   motorHor.setSpeed(255);
  20.   motorHor.run(RELEASE);
  21.   motorVer.setSpeed(255);
  22.   motorVer.run(RELEASE);
  23.  
  24.   Up();
  25.   delay(1000);
  26.   backward();
  27.   delay(1000);
  28.  
  29.  
  30. }
  31.  
  32. void loop() {
  33.   Box10();
  34.   delay(1000);
  35.   defaultPos();
  36.   delay(1000);
  37.   Box20();
  38.   delay(1000);
  39.   defaultPos();
  40.   delay(1000);
  41.   Box30();
  42.   delay(1000);
  43.   defaultPos();
  44.   delay(1000);
  45.   Box11();
  46.   delay(1000);
  47.   defaultPos();
  48.   delay(1000);
  49.   Box21();
  50.   delay(1000);
  51.   defaultPos();
  52.   delay(1000);
  53.   Box31();    
  54.   delay(1000);
  55.   defaultPos();
  56.   delay(1000);
  57. }
  58.  
  59. void Box10()
  60. {
  61.   StepSense(2); //goes 2 step from default position
  62. }
  63.  
  64. void Box20()
  65. {
  66.   StepSense(6); //goes 6 step from default position
  67. }
  68.  
  69. void Box30()
  70. {
  71.   StepSense(12); //goes 12 step from default position
  72. }
  73.  
  74. void Box11()
  75. {
  76.   NextLevel();  // Goes Up
  77.   StepSense(2); //goes 2 step from default position
  78. }
  79.  
  80. void Box21()
  81. {
  82.   NextLevel();   // Goes Up
  83.   StepSense(6); //goes 6 step from default position
  84. }
  85.  
  86. void Box31()
  87. {
  88.   NextLevel();    // Goes Up
  89.   StepSense(12); //goes 12 step from default position
  90. }
  91.  
  92. void defaultPos()
  93. {
  94.   Down();
  95.   delay(1000);
  96.   backward();
  97.   delay(1000);
  98. }
  99.  
  100. void forward()
  101. {
  102.   motorHor.run(FORWARD);
  103.   delay(1000);
  104.   motorHor.run(RELEASE);
  105. }
  106.  
  107. void backward()
  108. {
  109.   motorHor.run(BACKWARD);
  110.   delay(2000);
  111.   motorHor.run(RELEASE);
  112. }    
  113.  
  114. void Up()
  115. {
  116.   motorVer.run(FORWARD);
  117.   delay(700);
  118.   motorVer.run(RELEASE);
  119. }
  120.  
  121. void Down()
  122. {
  123.   motorVer.run(BACKWARD);
  124.   delay(650);
  125.   motorVer.run(RELEASE);
  126. }
  127.  
  128. void NextLevel()
  129. {
  130.   if (iss2 < 2) {
  131.     Down();
  132.     iss2++;
  133.   }
  134. }
  135.  
  136.  
  137.  
  138. void StepSense(int step)
  139. {
  140.   if (turn != step) {
  141.     motorHor.run(FORWARD);
  142.     new_val = digitalRead(StepSensor1);
  143.  
  144.  
  145.     if (new_val != old_val)
  146.     {
  147.       count1++;
  148.       if (count1 == 10)
  149.       {
  150.         motorHor.run(RELEASE);
  151.         turn++;
  152.         count1 = 0;
  153.         Serial.print("TURN ");
  154.         Serial.print(turn);
  155.         Serial.print(" Complete ");
  156.  
  157.       }
  158.       else
  159.  
  160.         Serial.println(count1);
  161.       old_val = new_val;
  162.  
  163.     }
  164.   }
  165.  
  166. }
RAW Paste Data
Top