Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // definirea pinului de pas, respectiv de directie pentru motorul pas cu pas miscare verticala
- #define stepPin 48
- #define dirPin 46
- #define ls3in 32
- #define ls4in 22
- // definirea pinului de pas, respectiv de directie pentru motorul pas cu pas miscare orizontala lopata
- #define stepPin2 44
- #define dirPin2 42
- #define ls2in 34
- // definirea pinului de pas, respectiv de directie pentru motorul pas cu pas miscare orizontala masa
- #define stepPin3 40
- #define dirPin3 38
- #define ls1in 36
- //////////////// senzori///////////////
- #define ls0in 52
- #define so1in 50 //intrarea primului senzor de obstacole
- //////////////relee/////////////////////////
- #define relay1 30
- #define relay2 28
- #define relay3 26
- #define relay4 24
- ////// definim butoanele + switch
- #define swin 23 //switch
- #define start_button 20
- #define stop_button 21
- byte flag_motor_done = 0;
- int so1 = 0;
- extern volatile unsigned long timer0_millis;
- int sw_val = 0;
- int time = 0;
- const byte ROWS = 4; // Four rows
- const byte COLS = 4; // Three columns
- char keys[ROWS][COLS] = { // Define the Keymap
- {'1', '2', '3', 'A'},
- {'4', '5', '6', 'B'},
- {'7', '8', '9', 'C'},
- {'*', '0', '#', 'D'}
- };
- char opposite_keys[ROWS][COLS] = { // Define the Keymap
- {NULL, NULL, NULL, NULL},
- {NULL, NULL, NULL, NULL},
- {'^', '%', '$', NULL},
- {NULL, '@', NULL, NULL}
- };
- byte rowPins[ROWS] = { 2, 3, 4, 5 };// Connect keypad ROW0, ROW1, ROW2 and ROW3 to these Arduino pins.
- byte colPins[COLS] = { 6, 7, 8 , 9}; // Connect keypad COL0, COL1 and COL2 to these Arduino pins.
- int opposite_key = NULL;
- bool flag_alarm = true;
- int current_state = -1;
- int counter = 0;
- //////////////////// encoder///////////////////
- byte cha = 18;
- byte chb = 19;
- long encoder_pos = 0;
- byte last_value = B00000000;
- byte cumulator = B00000000;
- int go_to_value = 0;
- void setup()
- {
- pinMode(stepPin, OUTPUT);
- pinMode(dirPin, OUTPUT);
- pinMode(ls3in, INPUT);
- pinMode(ls4in, INPUT);
- pinMode(stepPin2, OUTPUT);
- pinMode(dirPin2, OUTPUT);
- pinMode(ls2in, INPUT);
- pinMode(stepPin3, OUTPUT);
- pinMode(dirPin3, OUTPUT);
- pinMode(ls1in, INPUT);
- pinMode(so1in, INPUT);
- pinMode(ls0in, INPUT);
- pinMode(relay1, OUTPUT);
- pinMode(relay2, OUTPUT);
- pinMode(relay3, OUTPUT);
- pinMode(relay4, OUTPUT);
- // pinmode pentru cele doua butoane si switch
- pinMode(swin, INPUT_PULLUP);
- pinMode(start_button, INPUT);
- pinMode(stop_button, INPUT);
- // declararea pinilor de input respectiv output pentru tastatura 4x4
- pinMode(6, INPUT);
- pinMode(7, INPUT);
- pinMode(8, INPUT);
- pinMode(9, INPUT);
- pinMode(2, OUTPUT);
- pinMode(3, OUTPUT);
- pinMode(4, OUTPUT);
- pinMode(5, OUTPUT);
- ///////////////////////////////
- // setarea starii high pentru pinii asociati coloanelor
- digitalWrite(6, HIGH);
- digitalWrite(7, HIGH);
- digitalWrite(8, HIGH);
- digitalWrite(9, HIGH);
- digitalWrite(relay1, HIGH);
- digitalWrite(relay2, HIGH);
- digitalWrite(relay3, HIGH);
- digitalWrite(relay4, HIGH);
- ///// releul e COM_NC neenergizat, pe semnal LOW se conecteaza COM_NO iar pe semnal HIGH se conecteaza COM_NC.
- Serial.begin(9600);
- attachInterrupt(digitalPinToInterrupt(stop_button), alarm, RISING);
- attachInterrupt(digitalPinToInterrupt(start_button), reset_alarm, RISING);
- ////////// encoder////////
- pinMode(cha, INPUT_PULLUP);
- pinMode(chb, INPUT_PULLUP);
- }
- void loop() {
- // Serial.println(encoder_pos);
- sw_val = digitalRead(swin);
- if (sw_val == 0 && flag_alarm == false) { // sw_val == 0 coincide cu modul AUTOMAT
- automat();
- }
- else if (sw_val == 1) { //sw_val == 1 conincide cu modul MANUAL
- current_state = -1;
- manual();
- flag_alarm = true;
- }
- }
- char read_keyboard()
- {
- for ( uint8_t i = 2; i < 6; ++i) {
- digitalWrite(i, LOW);
- for ( uint8_t j = 6; j < 10; ++j) {
- int key = digitalRead(j);
- if (!key)
- {
- digitalWrite(i, HIGH);
- opposite_key = (opposite_keys[i - 2][j - 6]);
- return (keys[i - 2][j - 6]);
- }
- }
- digitalWrite(i, HIGH);
- }
- return opposite_key;
- }
- void manual() {
- char a = read_keyboard();
- //Serial.println(a);
- switch (a)
- {
- case '0': //pompa pornita
- {
- if (digitalRead(so1in) == 0) {
- pump_on();
- }
- break;
- }
- case '@': //pompa oprita
- {
- pump_off();
- reset_opposite_key(); //opposite_character = NULL ;
- break;
- }
- case '1': //stepper miscare sus
- {
- if (digitalRead(ls3in) == 0) {
- break;
- }
- else {
- move_motor(stepPin, dirPin, HIGH, 130);
- break;
- }
- }
- case '4': //stepper miscare jos
- {
- if (digitalRead(ls4in) == 0) {
- break;
- }
- else {
- move_motor(stepPin, dirPin, LOW, 130);
- break;
- }
- }
- case '3': //stepper lopata miscare stanga
- {
- if (digitalRead(ls2in) == 0) {
- break;
- }
- else {
- move_motor(stepPin2, dirPin2, HIGH, 1000);
- break;
- }
- }
- case '2': //stepper lopata miscare dreapta
- {
- move_motor(stepPin2, dirPin2, LOW, 1000);
- break;
- }
- case '6': //stepper masa miscare stanga
- {
- if (digitalRead(ls1in) == 0) {
- break;
- }
- else {
- move_motor(stepPin3, dirPin3, HIGH, 100);
- break;
- }
- }
- case '5': //stepper masa miscare dreapta
- {
- move_motor(stepPin3, dirPin3, LOW, 100);
- break;
- }
- case '9': // miscare banda
- {
- if (digitalRead(ls0in) == 0) { // trebuie modificat
- conveyor_off();
- break;
- }
- else
- {
- conveyor_on();
- break;
- }
- }
- case '$' : // opposite 9
- {
- conveyor_off();
- reset_opposite_key(); //opposite_character = NULL ;
- break;
- }
- case '8': //invartire sens orar(privit de sus)
- {
- motor_cw();
- break;
- }
- case '%':
- {
- motor_stop();
- reset_opposite_key(); //opposite_character = NULL ;
- break;
- }
- case '7': //invartire sens trigonometric(privit de sus)
- {
- motor_ccw();
- break;
- }
- case '^':
- {
- motor_stop();
- reset_opposite_key(); //opposite_character = NULL ;
- break;
- }
- }
- }
- /////////////////////////////// automat /////////////////////////
- void automat() {
- switch (current_state)
- {
- case 0 :
- {
- initialize();
- break;
- }
- case 1 :
- {
- //Serial.println("case 1");
- conveyor_on();
- current_state = 2;
- break;
- }
- case 2:
- {
- // Serial.println("case 2");
- if (digitalRead(so1in) == 0) {
- conveyor_off();
- current_state = 3;
- reset_millis();
- time = millis();
- }
- break;
- }
- // waint 1 second
- case 3:
- {
- // Serial.println("case 3");
- if (millis() - time > 1000)
- {
- pump_on();
- current_state = 4;
- reset_millis();
- time = millis();
- }
- break;
- }
- case 4:
- {
- //Serial.println("case 4");
- if (millis() - time > 2000)
- {
- pump_off();
- current_state = 5;
- }
- break;
- }
- case 5:
- {
- //Serial.println("case 5");
- if (digitalRead(ls0in) == 1) {
- conveyor_on();
- }
- else {
- conveyor_off();
- current_state = 6;
- }
- break;
- }
- case 6:
- {
- if (counter >= 4000) {
- current_state = 7;
- counter = 0;
- }
- else {
- move_motor(stepPin2, dirPin2, LOW, 1000);
- counter++;
- }
- break;
- }
- case 7:
- {
- if (counter >= 17850) {
- current_state = 8;
- counter = 0;
- }
- else {
- move_motor(stepPin3, dirPin3, LOW, 100);
- counter++;
- }
- break;
- }
- case 8:
- {
- if (counter >= 2800) {
- current_state = 9;
- counter = 0;
- go_to_value = 50;
- motor_cw();
- attachInterrupt(4, encoder, CHANGE);
- }
- else {
- move_motor(stepPin, dirPin, LOW, 130);
- counter++;
- }
- break;
- }
- case 9 :
- {
- //attachInterrupt(4, encoder, CHANGE);
- if (flag_motor_done == 1)
- {
- current_state = 10;
- flag_motor_done = 0;
- detachInterrupt(digitalPinToInterrupt(19));
- encoder_pos = 50; //corectezi pozitia
- }
- break;
- }
- case 10 :
- {
- if (digitalRead(ls1in) == 1) {
- move_motor(stepPin3, dirPin3, HIGH, 100);
- }
- else
- {
- if (digitalRead(ls2in) == 1) {
- move_motor(stepPin2, dirPin2, HIGH, 1000);
- }
- }
- if (digitalRead(ls1in) == 0 && digitalRead(ls2in) == 0)
- {
- current_state = 11;
- }
- break;
- }
- case 11:
- {
- if (digitalRead(ls4in) == 1) {
- move_motor(stepPin, dirPin, LOW, 130);
- }
- else
- {
- go_to_value = 500;
- motor_cw();
- attachInterrupt(4, encoder, CHANGE);
- flag_motor_done = 0;
- current_state = 12;
- }
- break;
- }
- case 12:
- {
- //Serial.println(encoder_pos);
- if (flag_motor_done == 1)
- {
- detachInterrupt(digitalPinToInterrupt(19));
- encoder_pos = 500; // corecteaza pozitia
- flag_motor_done = 0;
- current_state = 13;
- }
- break;
- }
- case 13:
- {
- if (counter <= 1500) {
- move_motor(stepPin, dirPin, HIGH, 130);
- counter++;
- }
- else
- {
- current_state = 14;
- }
- }
- }
- }
- ////////////////////////////// states///////////////////////////
- void initialize()
- {
- pump_off();
- conveyor_off();
- motor_stop();
- // attachInterrupt(4, encoder, CHANGE);
- if (digitalRead(ls1in) == 1) {
- move_motor(stepPin3, dirPin3, HIGH, 100);
- }
- else
- {
- if (digitalRead(ls2in) == 1) {
- move_motor(stepPin2, dirPin2, HIGH, 1000);
- }
- else
- {
- if (digitalRead(ls3in) == 1) {
- move_motor(stepPin, dirPin, HIGH, 130);
- }
- }
- }
- if (digitalRead(ls1in) == 0 && digitalRead(ls2in) == 0 && digitalRead(ls3in) == 0)
- {
- current_state = 1;
- }
- }
- //////////////////////////////// helper functions/////////////////
- void conveyor_on() //miscare banda pornita
- {
- digitalWrite(relay1, LOW);
- }
- void conveyor_off() //miscare banda oprita
- {
- digitalWrite(relay1, HIGH);
- }
- void motor_cw() //motor infiletare sens orar
- {
- digitalWrite(relay4, LOW);
- }
- void motor_ccw() // motor infiletare sens trigonometric
- {
- digitalWrite(relay3, LOW);
- }
- void motor_stop() //motor infiletare este oprit
- {
- digitalWrite(relay3, HIGH);
- digitalWrite(relay4, HIGH);
- }
- void pump_on()
- {
- digitalWrite(relay2, LOW);
- }
- void pump_off()
- {
- digitalWrite(relay2, HIGH);
- }
- void reset_opposite_key()
- {
- opposite_key = NULL;
- }
- // functie pentru steppere
- void move_motor(int motor_pin, int dir_pin, int direction, int delay) {
- digitalWrite(dir_pin, direction);
- digitalWrite(motor_pin, HIGH);
- digitalWrite(motor_pin, LOW);
- delayMicroseconds(delay);
- //ST1-SW123-011, comutator 111100; delay micro 130
- //ST2-SW123-010, comutator 111011; delay micro 1000
- //ST2-SW123--010, comutator 001100; delay micro 100
- }
- void alarm() {
- motor_stop();
- pump_off();
- conveyor_off();
- flag_alarm = true;
- }
- void reset_alarm() {
- if (flag_alarm)
- {
- flag_alarm = false;
- current_state = 0;
- // encoder_pos = 0;
- }
- }
- void reset_millis() {
- noInterrupts ();
- timer0_millis = 0;
- interrupts ();
- }
- void encoder()
- {
- // pp B11001001
- byte current_value = PIND & B00001100; // excludem din portu D biti nefolositi => B00001000
- if (last_value != current_value ) // B00001100 -> B00001000
- {
- cumulator <<= 2; // B00001100 => B00110000
- cumulator |= current_value >> 2; // 1. B00001000 => B00000010 , 2.B00110010
- if (cumulator == 75)
- {
- encoder_pos --;
- }
- if (cumulator == 30)
- {
- encoder_pos ++;
- }
- if (abs(encoder_pos - go_to_value) < 1 )
- {
- motor_stop();
- // Serial.println("done");
- flag_motor_done = 1;
- }
- }
- last_value = current_value;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement