SHARE
TWEET

6.5.2020 almost done

Zaganu Jun 5th, 2020 1,219 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // definirea pinului de pas, respectiv de directie pentru motorul pas cu pas miscare verticala
  2. #define stepPin 48
  3. #define dirPin 46
  4. #define ls3in 32
  5. #define ls4in 22
  6.  
  7. // definirea pinului de pas, respectiv de directie pentru motorul pas cu pas miscare orizontala lopata
  8. #define stepPin2 44
  9. #define dirPin2 42
  10. #define ls2in 34
  11.  
  12. // definirea pinului de pas, respectiv de directie pentru motorul pas cu pas miscare orizontala masa
  13. #define stepPin3 40
  14. #define dirPin3 38
  15. #define ls1in 36
  16.  
  17. //////////////// senzori///////////////
  18. #define ls0in 52
  19. #define so1in 50 //intrarea primului senzor de obstacole
  20.  
  21. //////////////relee/////////////////////////
  22. #define relay1 30
  23. #define relay2 28
  24. #define relay3 26
  25. #define relay4 24
  26.  
  27. ////// definim butoanele + switch
  28. #define swin 23  //switch
  29. #define start_button 20
  30. #define stop_button 21
  31.  
  32.  
  33.  
  34. byte flag_motor_done = 0;
  35. int so1 = 0;
  36. extern volatile unsigned long timer0_millis;
  37. int sw_val = 0;
  38. int time = 0;
  39. const byte ROWS = 4; // Four rows
  40. const byte COLS = 4; // Three columns
  41.  
  42.  
  43. char keys[ROWS][COLS] = { // Define the Keymap
  44.   {'1', '2', '3', 'A'},
  45.   {'4', '5', '6', 'B'},
  46.   {'7', '8', '9', 'C'},
  47.   {'*', '0', '#', 'D'}
  48. };
  49. char opposite_keys[ROWS][COLS] = { // Define the Keymap
  50.   {NULL, NULL, NULL, NULL},
  51.   {NULL, NULL, NULL, NULL},
  52.   {'^', '%', '$', NULL},
  53.   {NULL, '@', NULL, NULL}
  54. };
  55.  
  56. byte rowPins[ROWS] = { 2, 3, 4, 5 };// Connect keypad ROW0, ROW1, ROW2 and ROW3 to these Arduino pins.
  57.  
  58. byte colPins[COLS] = { 6, 7, 8 , 9}; // Connect keypad COL0, COL1 and COL2 to these Arduino pins.
  59.  
  60. int opposite_key = NULL;
  61. bool flag_alarm = true;
  62. int current_state = -1;
  63. int counter = 0;
  64.  
  65. //////////////////// encoder///////////////////
  66. byte cha = 18;
  67. byte chb = 19;
  68. long encoder_pos = 0;
  69. byte last_value = B00000000;
  70. byte cumulator = B00000000;
  71. int go_to_value = 0;
  72.  
  73.  
  74. void setup()
  75. {
  76.   pinMode(stepPin, OUTPUT);
  77.   pinMode(dirPin, OUTPUT);
  78.   pinMode(ls3in, INPUT);
  79.   pinMode(ls4in, INPUT);
  80.  
  81.   pinMode(stepPin2, OUTPUT);
  82.   pinMode(dirPin2, OUTPUT);
  83.   pinMode(ls2in, INPUT);
  84.  
  85.   pinMode(stepPin3, OUTPUT);
  86.   pinMode(dirPin3, OUTPUT);
  87.   pinMode(ls1in, INPUT);
  88.  
  89.   pinMode(so1in, INPUT);
  90.   pinMode(ls0in, INPUT);
  91.  
  92.   pinMode(relay1, OUTPUT);
  93.   pinMode(relay2, OUTPUT);
  94.   pinMode(relay3, OUTPUT);
  95.   pinMode(relay4, OUTPUT);
  96.  
  97.   // pinmode pentru cele doua butoane si switch
  98.   pinMode(swin, INPUT_PULLUP);
  99.   pinMode(start_button, INPUT);
  100.   pinMode(stop_button, INPUT);
  101.  
  102.  
  103.  
  104.   // declararea pinilor de input respectiv output pentru tastatura 4x4
  105.   pinMode(6, INPUT);
  106.   pinMode(7, INPUT);
  107.   pinMode(8, INPUT);
  108.   pinMode(9, INPUT);
  109.   pinMode(2, OUTPUT);
  110.   pinMode(3, OUTPUT);
  111.   pinMode(4, OUTPUT);
  112.   pinMode(5, OUTPUT);
  113.   ///////////////////////////////
  114.   // setarea starii high pentru pinii asociati coloanelor
  115.  
  116.   digitalWrite(6, HIGH);
  117.   digitalWrite(7, HIGH);
  118.   digitalWrite(8, HIGH);
  119.   digitalWrite(9, HIGH);
  120.  
  121.   digitalWrite(relay1, HIGH);
  122.   digitalWrite(relay2, HIGH);
  123.   digitalWrite(relay3, HIGH);
  124.   digitalWrite(relay4, HIGH);
  125.  
  126.   ///// releul e COM_NC neenergizat, pe semnal LOW se conecteaza COM_NO iar pe semnal HIGH se conecteaza COM_NC.
  127.   Serial.begin(9600);
  128.  
  129.   attachInterrupt(digitalPinToInterrupt(stop_button), alarm, RISING);
  130.   attachInterrupt(digitalPinToInterrupt(start_button), reset_alarm, RISING);
  131.  
  132.   ////////// encoder////////
  133.   pinMode(cha, INPUT_PULLUP);
  134.   pinMode(chb, INPUT_PULLUP);
  135. }
  136.  
  137. void loop() {
  138.   // Serial.println(encoder_pos);
  139.   sw_val = digitalRead(swin);
  140.   if (sw_val == 0 && flag_alarm == false) {     // sw_val == 0 coincide cu modul AUTOMAT
  141.     automat();
  142.   }
  143.   else if (sw_val == 1) { //sw_val == 1 conincide cu modul MANUAL
  144.     current_state = -1;
  145.     manual();
  146.     flag_alarm = true;
  147.  
  148.   }
  149. }
  150.  
  151.  
  152. char read_keyboard()
  153. {
  154.   for ( uint8_t i = 2; i < 6; ++i) {
  155.     digitalWrite(i, LOW);
  156.     for ( uint8_t j = 6; j < 10; ++j) {
  157.       int key = digitalRead(j);
  158.       if (!key)
  159.       {
  160.         digitalWrite(i, HIGH);
  161.         opposite_key = (opposite_keys[i - 2][j - 6]);
  162.         return (keys[i - 2][j - 6]);
  163.       }
  164.     }
  165.     digitalWrite(i, HIGH);
  166.   }
  167.   return opposite_key;
  168. }
  169.  
  170.  
  171. void manual() {
  172.   char a = read_keyboard();
  173.   //Serial.println(a);
  174.   switch (a)
  175.   {
  176.  
  177.     case '0': //pompa pornita
  178.       {
  179.         if (digitalRead(so1in) == 0) {
  180.           pump_on();
  181.         }
  182.         break;
  183.       }
  184.     case '@': //pompa oprita
  185.       {
  186.         pump_off();
  187.         reset_opposite_key();     //opposite_character = NULL ;
  188.         break;
  189.       }
  190.     case '1': //stepper miscare sus
  191.       {
  192.  
  193.         if (digitalRead(ls3in) == 0) {
  194.           break;
  195.         }
  196.         else {
  197.           move_motor(stepPin, dirPin, HIGH, 130);
  198.           break;
  199.         }
  200.       }
  201.     case '4': //stepper miscare jos
  202.       {
  203.         if (digitalRead(ls4in) == 0) {
  204.           break;
  205.         }
  206.         else {
  207.           move_motor(stepPin, dirPin, LOW, 130);
  208.           break;
  209.         }
  210.       }
  211.     case '3': //stepper lopata miscare stanga
  212.       {
  213.  
  214.         if (digitalRead(ls2in) == 0) {
  215.           break;
  216.         }
  217.         else {
  218.           move_motor(stepPin2, dirPin2, HIGH, 1000);
  219.           break;
  220.         }
  221.       }
  222.     case '2': //stepper lopata miscare dreapta
  223.       {
  224.         move_motor(stepPin2, dirPin2, LOW, 1000);
  225.         break;
  226.  
  227.       }
  228.     case '6': //stepper masa miscare stanga
  229.       {
  230.  
  231.         if (digitalRead(ls1in) == 0) {
  232.           break;
  233.         }
  234.         else {
  235.           move_motor(stepPin3, dirPin3, HIGH, 100);
  236.           break;
  237.         }
  238.       }
  239.     case '5': //stepper masa miscare dreapta
  240.       {
  241.         move_motor(stepPin3, dirPin3, LOW, 100);
  242.         break;
  243.       }
  244.     case '9': // miscare banda
  245.       {
  246.         if (digitalRead(ls0in) == 0) { // trebuie modificat
  247.           conveyor_off();
  248.           break;
  249.         }
  250.         else
  251.         {
  252.           conveyor_on();
  253.           break;
  254.         }
  255.       }
  256.  
  257.     case '$' : // opposite 9
  258.       {
  259.         conveyor_off();
  260.         reset_opposite_key();   //opposite_character = NULL ;
  261.         break;
  262.       }
  263.     case '8': //invartire sens orar(privit de sus)
  264.       {
  265.         motor_cw();
  266.         break;
  267.       }
  268.     case '%':
  269.       {
  270.         motor_stop();
  271.         reset_opposite_key();    //opposite_character = NULL ;
  272.         break;
  273.       }
  274.     case '7': //invartire sens trigonometric(privit de sus)
  275.       {
  276.         motor_ccw();
  277.         break;
  278.       }
  279.     case '^':
  280.       {
  281.         motor_stop();
  282.         reset_opposite_key();   //opposite_character = NULL ;
  283.         break;
  284.       }
  285.  
  286.   }
  287. }
  288.  
  289. /////////////////////////////// automat /////////////////////////
  290. void automat() {
  291.  
  292.   switch (current_state)
  293.   {
  294.     case 0 :
  295.       {
  296.         initialize();
  297.         break;
  298.       }
  299.  
  300.     case 1 :
  301.       {
  302.         //Serial.println("case 1");
  303.         conveyor_on();
  304.         current_state = 2;
  305.         break;
  306.       }
  307.     case 2:
  308.       {
  309.         // Serial.println("case 2");
  310.         if (digitalRead(so1in) == 0) {
  311.           conveyor_off();
  312.           current_state = 3;
  313.           reset_millis();
  314.           time = millis();
  315.         }
  316.         break;
  317.       }
  318.     // waint 1 second
  319.     case 3:
  320.       {
  321.         // Serial.println("case 3");
  322.         if (millis() - time > 1000)
  323.         {
  324.           pump_on();
  325.           current_state = 4;
  326.           reset_millis();
  327.           time = millis();
  328.         }
  329.         break;
  330.       }
  331.     case 4:
  332.       {
  333.         //Serial.println("case 4");
  334.         if (millis() - time > 2000)
  335.         {
  336.           pump_off();
  337.           current_state = 5;
  338.         }
  339.         break;
  340.       }
  341.     case 5:
  342.       {
  343.         //Serial.println("case 5");
  344.         if (digitalRead(ls0in) == 1) {
  345.           conveyor_on();
  346.         }
  347.         else {
  348.           conveyor_off();
  349.           current_state = 6;
  350.         }
  351.         break;
  352.       }
  353.     case 6:
  354.       {
  355.         if (counter >= 4000) {
  356.           current_state = 7;
  357.           counter = 0;
  358.         }
  359.         else {
  360.           move_motor(stepPin2, dirPin2, LOW, 1000);
  361.           counter++;
  362.         }
  363.         break;
  364.       }
  365.     case 7:
  366.       {
  367.         if (counter >= 17850) {
  368.           current_state = 8;
  369.           counter = 0;
  370.         }
  371.         else {
  372.           move_motor(stepPin3, dirPin3, LOW, 100);
  373.           counter++;
  374.         }
  375.         break;
  376.       }
  377.     case 8:
  378.       {
  379.         if (counter >= 2800) {
  380.           current_state = 9;
  381.           counter = 0;
  382.           go_to_value = 50;
  383.           motor_cw();
  384.           attachInterrupt(4, encoder, CHANGE);
  385.         }
  386.         else {
  387.           move_motor(stepPin, dirPin, LOW, 130);
  388.           counter++;
  389.         }
  390.         break;
  391.       }
  392.  
  393.     case 9 :
  394.       {
  395.         //attachInterrupt(4, encoder, CHANGE);
  396.         if (flag_motor_done == 1)
  397.         {
  398.           current_state = 10;
  399.           flag_motor_done = 0;
  400.           detachInterrupt(digitalPinToInterrupt(19));
  401.           encoder_pos = 50; //corectezi pozitia
  402.         }
  403.         break;
  404.       }
  405.     case 10 :
  406.       {
  407.         if (digitalRead(ls1in) == 1) {
  408.           move_motor(stepPin3, dirPin3, HIGH, 100);
  409.         }
  410.         else
  411.         {
  412.           if (digitalRead(ls2in) == 1) {
  413.             move_motor(stepPin2, dirPin2, HIGH, 1000);
  414.           }
  415.         }
  416.         if (digitalRead(ls1in) == 0 && digitalRead(ls2in) == 0)
  417.         {
  418.           current_state = 11;
  419.         }
  420.         break;
  421.       }
  422.     case 11:
  423.       {
  424.         if (digitalRead(ls4in) == 1) {
  425.           move_motor(stepPin, dirPin, LOW, 130);
  426.         }
  427.         else
  428.         {
  429.           go_to_value = 500;
  430.           motor_cw();
  431.           attachInterrupt(4, encoder, CHANGE);
  432.           flag_motor_done = 0;
  433.           current_state = 12;
  434.         }
  435.         break;
  436.       }
  437.  
  438.     case 12:
  439.       {
  440.         //Serial.println(encoder_pos);
  441.         if (flag_motor_done == 1)
  442.         {
  443.           detachInterrupt(digitalPinToInterrupt(19));
  444.           encoder_pos = 500; // corecteaza pozitia
  445.           flag_motor_done = 0;
  446.           current_state = 13;
  447.         }
  448.         break;
  449.       }
  450.     case 13:
  451.       {
  452.         if (counter <= 1500) {
  453.           move_motor(stepPin, dirPin, HIGH, 130);
  454.           counter++;
  455.         }
  456.         else
  457.         {
  458.            current_state = 14;
  459.         }
  460.       }
  461.   }
  462. }
  463. ////////////////////////////// states///////////////////////////
  464.  
  465. void initialize()
  466. {
  467.   pump_off();
  468.   conveyor_off();
  469.   motor_stop();
  470.   //  attachInterrupt(4, encoder, CHANGE);
  471.  
  472.   if (digitalRead(ls1in) == 1) {
  473.     move_motor(stepPin3, dirPin3, HIGH, 100);
  474.   }
  475.   else
  476.   {
  477.     if (digitalRead(ls2in) == 1) {
  478.       move_motor(stepPin2, dirPin2, HIGH, 1000);
  479.     }
  480.     else
  481.     {
  482.       if (digitalRead(ls3in) == 1) {
  483.         move_motor(stepPin, dirPin, HIGH, 130);
  484.       }
  485.     }
  486.   }
  487.   if (digitalRead(ls1in) == 0 && digitalRead(ls2in) == 0 && digitalRead(ls3in) == 0)
  488.   {
  489.     current_state = 1;
  490.   }
  491.  
  492. }
  493.  
  494.  
  495.  
  496. //////////////////////////////// helper functions/////////////////
  497.  
  498. void conveyor_on() //miscare banda pornita
  499. {
  500.   digitalWrite(relay1, LOW);
  501. }
  502.  
  503. void conveyor_off() //miscare banda oprita
  504. {
  505.   digitalWrite(relay1, HIGH);
  506. }
  507.  
  508. void motor_cw() //motor infiletare sens orar
  509. {
  510.   digitalWrite(relay4, LOW);
  511. }
  512.  
  513. void motor_ccw() // motor infiletare sens trigonometric
  514. {
  515.   digitalWrite(relay3, LOW);
  516. }
  517.  
  518. void motor_stop() //motor infiletare este oprit
  519. {
  520.   digitalWrite(relay3, HIGH);
  521.   digitalWrite(relay4, HIGH);
  522. }
  523.  
  524. void pump_on()
  525. {
  526.   digitalWrite(relay2, LOW);
  527. }
  528.  
  529. void pump_off()
  530. {
  531.   digitalWrite(relay2, HIGH);
  532. }
  533.  
  534. void reset_opposite_key()
  535. {
  536.   opposite_key = NULL;
  537. }
  538.  
  539.  
  540.  
  541. // functie pentru steppere
  542. void move_motor(int motor_pin, int dir_pin, int direction, int delay) {
  543.   digitalWrite(dir_pin, direction);
  544.   digitalWrite(motor_pin, HIGH);
  545.   digitalWrite(motor_pin, LOW);
  546.   delayMicroseconds(delay);
  547.   //ST1-SW123-011, comutator 111100;  delay micro 130
  548.   //ST2-SW123-010, comutator 111011;  delay micro 1000
  549.   //ST2-SW123--010, comutator 001100; delay micro 100
  550. }
  551.  
  552. void alarm() {
  553.   motor_stop();
  554.   pump_off();
  555.   conveyor_off();
  556.   flag_alarm = true;
  557.  
  558. }
  559.  
  560. void reset_alarm() {
  561.   if (flag_alarm)
  562.   {
  563.     flag_alarm = false;
  564.     current_state = 0;
  565.     // encoder_pos = 0;
  566.  
  567.   }
  568. }
  569.  
  570. void reset_millis() {
  571.   noInterrupts ();
  572.   timer0_millis = 0;
  573.   interrupts ();
  574. }
  575.  
  576. void encoder()
  577. {
  578.   // pp B11001001
  579.   byte current_value = PIND & B00001100;  // excludem din portu D biti nefolositi => B00001000
  580.   if (last_value != current_value )  // B00001100 -> B00001000
  581.   {
  582.     cumulator <<= 2;   // B00001100 => B00110000
  583.     cumulator |= current_value >> 2; // 1. B00001000 => B00000010 , 2.B00110010
  584.     if (cumulator == 75)
  585.     {
  586.       encoder_pos --;
  587.     }
  588.  
  589.     if (cumulator == 30)
  590.     {
  591.       encoder_pos ++;
  592.     }
  593.     if (abs(encoder_pos - go_to_value) < 1 )
  594.     {
  595.       motor_stop();
  596.       // Serial.println("done");
  597.       flag_motor_done = 1;
  598.     }
  599.   }
  600.   last_value = current_value;
  601. }
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