#define MainStEnb 10 //Основной ШД
#define MainStDir 9
#define MainStStep 8
#define MoveStEnb 13 //Укладчик ШД
#define MoveStDir 12
#define MoveStStep 11
#define ledPin 3
#define ledRXTXPin 2
#define AnalogSignal A1// датчик провисания(угол)
#define ButtonPin A0 //кнопка "Домой"
#define LeftStop A2 // концевой выключатель для калибровки ноля
#include <ModbusRtu.h>
#include <PID_v1.h>
#include <GyverTimers.h>
const int KP = 26; // коэффициент пропорциональности ПИД, иногда меняется от глубины намотки текущей катушки
double Setpoint, Input, Output;
PID regulator(&Input, &Output, &Setpoint, KP, 1, 0, REVERSE);
unsigned int StepsForMM = 200, RollWidth = 50; // ширина наматываемой части
unsigned int ShiftWidthLeft = 2; //отступ слева от концевого выключателя
const float RollSide = 12.5; //суммарная толщина бортиков катушек
const float PeredatochnoeChislo = 3.1; //передаточное число между шестернями привода
unsigned int FilamentWidth = 3; // ширина занимаемая одним витком 3 на первом ряду затем 2
const unsigned int MainSpeedMax = 4200; // макс скорость
const unsigned int MainSpeedMin = 0; //минимальная скокрость работы - 0 - стоп
const int PIDInterval = 100; //интервал обработки датчика провисания в мс
bool s1 = 0;
bool s2 = 0;
volatile unsigned long Steps = 0; //шаги онсновного привода
const int AngleConst = 245; //угол в аналогрид(0-1023) нечто среднее между 210(верх) и 300 (нижнее положение ролика)
unsigned int RollNum = 1; //номер катушки
unsigned int Speed=2000; //частота таймера осн двиг
unsigned int MoveStSpeed = 600; //частота(скорость) частота вращения мотора укладчика
unsigned int SpinsNow = 0; //обороты
unsigned int Spins = 0; //обороты
int Row =0; // ряд
volatile long MoveStCurrentPosition = 0; //текущая координата
volatile long MoveStTarget = 0; // цель для ползуна
long LeftSide[11];
long RightSide[11];
bool isEnabe = 0; // выключатель
bool Direction = 1; // направление хода укладчика один это двигаться направо
bool NowGoingHome = 0; //сейчас еду домой
bool OutOfRoll = 0; // укладчик находится вне диапазона намотки, нужно для увеличения скокрости укладчика относительно намотки
int AvgAnalogRead = 0; //средние значения с датчика
unsigned long LEDTimer, AnalogReadTimer, MainStSpeedUpdateTimer, isEnableTimer=0; //таймеры
bool ledState = 0;
unsigned int modbus_array[2] = {0, 1}; //Готовим массив данных "разрешно работать 0/1" и "номер катушки"
Modbus bus(4, Serial, 4); // this is slave 4, Serial 0, and RS-485 (4 pin for enable)
void setup() {
pinMode(MainStEnb, OUTPUT); // Инициализация осноного мотора
pinMode(MainStDir, OUTPUT);
pinMode(MainStStep, OUTPUT);
pinMode(MoveStEnb, OUTPUT); // Инициализация мотора укладчика
pinMode(MoveStDir, OUTPUT);
pinMode(MoveStStep, OUTPUT);
pinMode(ledPin, OUTPUT);
pinMode(ledRXTXPin, OUTPUT);
pinMode(LeftStop, INPUT); //концевик слева
digitalWrite(LeftStop, HIGH);
pinMode(ButtonPin, INPUT); //кнопка Домой
digitalWrite(ButtonPin, HIGH);
// Инициализация и параметры ПИД
Setpoint = AngleConst;
regulator.SetMode(AUTOMATIC);
regulator.SetOutputLimits(MainSpeedMin, MainSpeedMax);
regulator.SetSampleTime(PIDInterval);
Serial.begin(19200);
bus.start();
Serial.println("Start");
for (int i=1; i <= 10; i++){ //расчитываем параметры катушек
LeftSide[i] = ((i-1)*StepsForMM*(RollWidth+RollSide) + StepsForMM*ShiftWidthLeft);
RightSide[i] = LeftSide[i]+StepsForMM*RollWidth;
Serial.print("i: "); Serial.print(i); Serial.print(" ");
Serial.print(LeftSide[i]); Serial.print("<< >>"); Serial.println(RightSide[i]);
}
Timer1.setFrequency(Speed); //частота таймера
Timer1.enableISR(CHANNEL_B); // Подключить прерывание таймера 1, канал B
Timer2.setFrequency(MoveStSpeed); //частота таймера
Timer2.enableISR(CHANNEL_B); // Подключить прерывание таймера 2, канал B
//при генерации меандра реальная частота будет в два раза меньше заданной из-за особенности работы самого таймера.
}
ISR(TIMER1_B) { //прерывание по таймеру для осн двигателя
s1 = not(s1);
digitalWrite(MainStStep, s1);
Steps++;
}
ISR(TIMER2_B) { //прерывание по таймеру для двигателя укладчика
if (MoveStCurrentPosition<MoveStTarget) {digitalWrite(MoveStDir, 0); MoveStCurrentPosition++;}
if (MoveStCurrentPosition>MoveStTarget) {digitalWrite(MoveStDir, 1); MoveStCurrentPosition--;}
if (MoveStCurrentPosition!=MoveStTarget) {
s2 = not(s2);
digitalWrite(MoveStStep, s2);
}
}
void loop() {
bus.poll(modbus_array, 2); //Принимаем данные о номере катушки и рарешении на работу по Modbus
regulator.Compute(); // Вычисляем ПИД
if (!NowGoingHome && OutOfRoll && (abs(MoveStCurrentPosition - MoveStTarget)<10) ) { // достгли левого края новой катушки
OutOfRoll=0;
Direction = 1; //направо
Row = 0;
FilamentWidth = 3;
regulator.SetTunings(KP, 1, 0);
//Serial.print(" I am in roll! spd 600 "); Serial.print(" CurrentPosition: "); Serial.print(MoveStCurrentPosition);
Timer2.setFrequency(600);
//Serial.print(" Target after: "); Serial.println(MoveStTarget);
}
if ((isEnabe) && (millis() - MainStSpeedUpdateTimer > PIDInterval) && (AvgAnalogRead > 160) ) { // отправка скорости на мотор
MainStSpeedUpdateTimer = millis();
Input = AvgAnalogRead; //ПИД вход с датчика
Speed = int(Output); //ПИД результат
Timer1.setFrequency(Speed); //обновляем скорость
}
noInterrupts();
if (!NowGoingHome && (modbus_array[1] != RollNum)&&(modbus_array[1]<11)) { // поступил сигнал "следующая катушка"
RollNum=modbus_array[1];
OutOfRoll=1; //вне катушки, нужно быстренько ехать к новой
Timer2.setFrequency(5000); // увеличиваем скорость укладчика
MoveStTarget = ((RollNum-1)*StepsForMM*(RollWidth+RollSide) + StepsForMM*ShiftWidthLeft); //едь сюда к левой стенке следующей катушки
Serial.print("Next roll spd 5000 MoveStTarget: ");Serial.println(MoveStTarget);
}
if ((AvgAnalogRead > 160) && (isEnabe) && (!NowGoingHome)) { //включена работа
if (!OutOfRoll) { //таскаем туда сюда, если не переход на след катушку
SpinsNow = Steps/1600/PeredatochnoeChislo;
if (SpinsNow>Spins) { // каждый оборот сдвинуться на FilamentWidth
Spins = SpinsNow;
if (Direction) {MoveStTarget = MoveStTarget + StepsForMM*FilamentWidth;} else
{MoveStTarget = MoveStTarget - StepsForMM*FilamentWidth;}
if (MoveStTarget < LeftSide[RollNum]) {
MoveStTarget = MoveStTarget + 2*StepsForMM*FilamentWidth;
Direction = !Direction; //меняем напрвление
Row++;
}
if (MoveStTarget > RightSide[RollNum]) {
MoveStTarget = MoveStTarget - 2*StepsForMM*FilamentWidth;
Direction = !Direction; //меняем напрвление
Row++;
}
}
if (Row>2) {FilamentWidth = 2;}
if (Row>5) {regulator.SetTunings(10, 1, 0); } //После первых пяти слое намотки уменьшаем KP ПИД для более плавной работы
}
}else {Timer1.pause();}
interrupts();
if (millis() - AnalogReadTimer > 50) { // контролируем скорость с датчика угла провисания
AnalogReadTimer = millis();
AvgAnalogRead = 0.8*AvgAnalogRead + 0.2*analogRead(AnalogSignal); //бегущее среднее для датчика
}
if (!digitalRead(ButtonPin)){ //нажали кнопку Домой
delay(50);
if (!digitalRead(ButtonPin)) {
MoveStTarget = MoveStCurrentPosition; //остановка
Timer2.setFrequency(5000);
MoveStTarget = -1000000;
Serial.println("Go home");
NowGoingHome = 1;
}
}
if(!digitalRead(LeftStop) && (NowGoingHome)) { // сработал левый концевик
NowGoingHome = 0;
// Serial.println("Iam home, go to start point");
MoveStCurrentPosition = 0;
MoveStTarget = StepsForMM*ShiftWidthLeft; // После калибровки отправляем ползун чуть правее в начало первой катушки
RollNum=1; Direction = 1; OutOfRoll=0;
Row = 0; FilamentWidth = 3;
regulator.SetTunings(KP, 1, 0);
Timer2.setFrequency(600);
}
if (modbus_array[0]==1) { isEnableTimer = millis(); isEnabe = 1;}
//if (modbus_array[0]==0) {Serial.println("Modbus 0 == 0 !!!!!!");}
if ((modbus_array[0]==0) && ((millis() - isEnableTimer)>5000)) {
Timer1.pause();
isEnabe = 0;
//Serial.println("disableISR !!!!!!!!!!!!");
digitalWrite(MainStEnb, 1);
} // Разрешение на работу от мастера
if(millis() - LEDTimer > 500) { //мигаем
LEDTimer = millis();
ledState = !ledState;
digitalWrite(ledRXTXPin, ledState);
//Serial.println(" LED ");
Serial.println();
Serial.print(" [0]: "); Serial.print(modbus_array[0]);
Serial.print(" [1]: "); Serial.print(modbus_array[1]);
Serial.print(" OutOfRoll: "); Serial.print(OutOfRoll);
Serial.print(" RollNum: "); Serial.print(RollNum);
Serial.print(" FilamentWidth: "); Serial.print(FilamentWidth);
Serial.print(" Steps: "); Serial.print(Steps);
Serial.print(" SpinsNow: "); Serial.print(SpinsNow);
Serial.print(" Spins: "); Serial.print(Spins);
Serial.print(" CurrentPosition: "); Serial.print(MoveStCurrentPosition);
Serial.print(" Target>> "); Serial.print(MoveStTarget);
Serial.print(" Left: "); Serial.print(LeftSide[RollNum]); Serial.print(" Right: "); Serial.print(RightSide[RollNum]);
}
interrupts();
}