мега 2 шаговых двигателя и 4 датчика

svorog
Offline
Зарегистрирован: 27.04.2017

Здравствуйте ищу исполнителя по доработке кода, код в приложении обеспечивает управление движением двумя шаговыми двигателями по четырем датчикам (первый датчик начало движения первого двигателя с максимальной скоростью на заданное число шагов, потом проверяем второй датчик если ещё не сработал то продолжаем движение с уменьшенной скоростью пока не сработает второй датчик, датчик сработал останавливаемся) Второй двигатель управляется аналогично по третьему четвертому датчику. Есть ещё один пятый датчик который при получении сообщения брак подает высокое напряжение на один из пинов. Код работает однако имеются наводки, т.к. плата расположена в силовом шкафу вместе с драйверами двигателей, при включении двигателей появляются ложные срабатывания датчиков, аппаратно решить проблему наводок нет возможности, поэтому хочу решить проблему програмно.

Задача: прописать в имеющемся коде минимальный интервал срабатывания датчика, который мы будем считать реальной сменой сигнала, все короткие импульсы от наводок мы считаем помехами и игнорируем,  если сигнал датчика  сменился и держится 100..200мс то да мы считаем что датчик реально сработал и начинаем движение. Продолжительность интервала нужно будет настраивать вручную при пусконаладке (по умолчанию 100мс на все датчики).

sistema164@mail.ru

#define DEBUG //enable debug messages

#include "AccelStepper.h" // #include <AccelStepper.h>

AccelStepper stepper1(AccelStepper::FULL2WIRE, 2,3);
AccelStepper stepper2(AccelStepper::FULL2WIRE, 4,5);

//***************************
//***************************
// ANALOG INPUTS
#define R1_DELAY_PIN A2
#define R1_SPEED_PIN A1
#define R2_DELAY_PIN A4
#define R2_SPEED_PIN A3

//***************************
// output pin to relay
#define OUT_BRAK_PIN 46

//***************************
// SENSORS PINS AND HIGH/LOW TRIGGER LEVEL
uint8_t sensor_pin[6] = {0, 14,43,32,33,42};	// pin numbers, sensor_pin[0] not used !!!
uint8_t sensor_level[6] = {0, 0,0,0,0,0};		// working level: 0 - LOW TRIGGER LEVEL, 1 - HIGH
// sensor #1 - ШД_1
// sensor #2 - ШД_1
// sensor #3 - ШД_2
// sensor #4 - ШД_2
// sensor #5 - "+1"

uint8_t sensor_old[6];		// previous value, see setup
uint8_t sensor_new[6];		// current value

// SENSORS ANTI-BOUNCE TIME IN MILLISEC !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
uint32_t sensor_bounce[6] = {0, 90,90,90,90,90};
uint32_t sensor_start_time[6] = {0};

//***************************
float SM1_speed;
float SM2_speed;

uint8_t moving1 = 0;
uint8_t moving2 = 0;

//***************************
char inp_buf[8]; // serial input buffer for "brak"
uint8_t indx = 0;

//***************************
uint8_t timer_flag[6] = {0,0,0,0,0,0};
uint32_t timer_start[6];
uint32_t timer_delay[6];
// timer #1 - пауза для первого ШД (регулируется потенциометром)
// timer #2 - пауза для второго ШД (регулируется потенциометром)
// timer #3 - при получении сообщения «brak» выжидаем паузу 20мс
// timer #4 - 500мс на пин который будет управлять реле

//***************************
#ifdef DEBUG
 char data[80];
#endif

//**************************************************
//**************************************************
void test_serial_for_BRAK() // read serial one byte
{
    char inp_byte = Serial.read();
        if (inp_byte=='\n') { // it's the end of line
        inp_buf[indx] = '\0'; // for C-string
        indx = 0;
        if (strncmp(inp_buf, "brak", 4)==0) timer_begin( 3, 20 );
        return;
    };

    if (inp_byte>=' ') { // buffer consists only letters
        inp_buf[indx] = inp_byte;
        indx++;
        indx &= 0x07; // 8 bytes buffer
        return;
    };
};

//**************************************************
bool sensor(uint8_t i)
{
    if ( millis() - sensor_start_time[i] < sensor_bounce[i] ) return false; // sensor bounce
	
    sensor_new[i] = digitalRead(sensor_pin[i]);
    if ( sensor_new[i] == sensor_old[i] ) return false;
	
	sensor_old[i] = sensor_new[i];	//input level changed
	sensor_start_time[i] = millis();

#ifdef DEBUG
 sprintf(data, "sensor_%d %s", i, (sensor_new[i]==sensor_level[i])?("ON"):("OFF"));
 Serial.println(data);
#endif

	return ( sensor_new[i] == sensor_level[i] );
};

//**************************************************
void timer_begin(uint8_t i, uint32_t tm)
{

#ifdef DEBUG
 sprintf(data, "timer_%u BEGIN %lums", i, tm);
 Serial.println(data);
#endif

    timer_flag[i] = 1; // timer is active now
    timer_start[i] = millis();
    timer_delay[i] = tm;
};

//**************************************************
bool check_timer(uint8_t i) // test timer for the end
{
    if ( timer_flag[i] == 0 ) return false;
    if ( (millis() - timer_start[i]) < timer_delay[i] ) return false;
    timer_flag[i] = 0;

#ifdef DEBUG
 sprintf(data, "timer_%u END", i);
 Serial.println(data);
#endif

    return true; // end of timer[i]
};

//****************************************************************************
//****************************************************************************
void setup()
{
    for (uint8_t i = 1; i <= 5; i++) {
        pinMode(sensor_pin[i], INPUT_PULLUP); // !!! PULLUP !!!
        sensor_old[i] = (1-sensor_level[i]);
    };

    pinMode(OUT_BRAK_PIN, OUTPUT);

    Serial.begin(9600);

    stepper1.setMaxSpeed(3000.0);
    stepper1.setAcceleration(10000.0);

    stepper2.setMaxSpeed(5000.0);
    stepper2.setAcceleration(10000.0);

#ifdef DEBUG
 Serial.println("PROGRAM STARTING...\n");
#endif

}

//****************************************************************************
void loop()
{
//******************************* stepper1
    if ( sensor(1) ) {
        timer_begin( 1, analogRead(R1_DELAY_PIN) );
    };

    if ( check_timer(1) ) {
		stepper1.setCurrentPosition(0);
        SM1_speed = (float)map(analogRead(R1_SPEED_PIN), 0, 1023, 3000, 150);
        stepper1.setMaxSpeed(SM1_speed);
        stepper1.move(1000);
		moving1 = 1;
    };

    if ( moving1 == 1 && stepper1.distanceToGo() == 0 ) {
        stepper1.setMaxSpeed(SM1_speed/2.0);
        stepper1.move(1000000);

#ifdef DEBUG
 Serial.println("stepper_1 half speed");
#endif

    };

    if ( sensor(2) ) {
        stepper1.stop();
		moving1 = 0;
    };

//******************************* stepper2
    if ( sensor(3) ) {
        timer_begin( 2, analogRead(R2_DELAY_PIN) );
    };

    if ( check_timer(2) ) {
		stepper2.setCurrentPosition(0);
        SM2_speed = (float)map(analogRead(R2_SPEED_PIN), 0, 1023, 5000, 150);
        stepper2.setMaxSpeed(SM2_speed);
        stepper2.move(1000);
		moving2 = 1;
    };

    if ( moving2 == 1 && stepper2.distanceToGo() == 0 ) {
        stepper2.setMaxSpeed(SM2_speed/2.0);
        stepper2.move(1000000);

#ifdef DEBUG
 Serial.println("stepper_2 half speed");
#endif

    };

    if ( sensor(4) ) {
        stepper2.stop();
		moving2 = 0;
    };

//******************************* send "+1" 
    if ( sensor(5) ) {
        Serial.println("+1");
    };

//******************************* catch "brak" with timer_begin( 3, 20 );
    if ( Serial.available() ) test_serial_for_BRAK();

//******************************* brak 500ms pulse
    if ( check_timer(3) ) {
        digitalWrite(OUT_BRAK_PIN, HIGH);
        timer_begin( 4, 500 );
    };
    if ( check_timer(4) ) {
		digitalWrite(OUT_BRAK_PIN, LOW);
	};

//******************************* RUN-RUN-RUN... STEPPERS
    stepper1.run();
    stepper2.run();
}

 

b707
Offline
Зарегистрирован: 26.05.2017

svorog пишет:

Задача: прописать в имеющемся коде минимальный интервал срабатывания датчика, который мы будем считать реальной сменой сигнала, все короткие импульсы от наводок мы считаем помехами и игнорируем,  если сигнал датчика  сменился и держится 100..200мс то да мы считаем что датчик реально сработал и начинаем движение. Продолжительность интервала нужно будет настраивать вручную при пусконаладке (по умолчанию 100мс на все датчики).

если я правильно понял, ваши датчики представляют из себя обычные цифровые пины, то есть фактически кнопки. А то, что вы описываете - называется антидребезг. Пример кода антидребезга можно найти в любой приличной библиотеке работы с кнопками.

Более того, в вашем коде он уже прописан - смотрите процедуру sensor() в районе 80-х строк. Время реакции пина. как вы его называете - в вашем коде 90мс для всех сенсоров.

Так что я не очень понимаю, что именно вы хотите от исполнителя. Или задача - в подборе конкретного дебонсинг-интервала? - вряд ли это можно сделать удаленно.