#include <AccelStepper.h>
#include <SoftwareSerial.h>
#include <FlexiTimer2.h>
#include <ModbusMaster.h>
#include "TeensyThreads.h"
#include <Arduino.h>
#include <TimerOne.h>
#include <Bounce.h>
#define RS485Serial Serial1
ModbusMaster node;
#define z_Step 2
#define z_Dir 3
#define z_AXIS_Speed 50000 // number per 1mm
#define z_AXIS_Accelration 50000
#define z_PostionMultiplier 10000 // number per 1mm
#define zstepPermm 1 //divide by 1000
#define SSerialRX 0 //Serial Receive pin rx
#define SSerialTX 1 //Serial Transmit pin tx
//Z axis parameter
bool bool_zReach = 0;
int z_CurrentPos = 0;
bool autoLoop = false;
bool autoLoop1 = false;
bool en = false;
bool bool_Emergency = 0;
unsigned long currentTime_Auto;
unsigned long previousMillis_Auto0;
unsigned long modTime;
unsigned long modMicros;
unsigned long modMicros0;
SoftwareSerial dis_Serial(17, 18);
//SoftwareSerial RS485Serial(SSerialRX, SSerialTX); // RX, TX
AccelStepper z_Axis(1, z_Step, z_Dir);
//volatile int blinkcode = 1;
volatile int data[1];
volatile static uint32_t i;
volatile uint8_t j, result;
//
elapsedMillis sincePrint;
elapsedMicros sincePrint1;
void blinkthread() {
// static uint32_t i;
// uint8_t j, result;
// uint16_t data[1];
if (sincePrint > 1000) { // "sincePrint" auto-increases
sincePrint = 0;
result = node.readHoldingRegisters(0x1019, 1);
if (result == node.ku8MBSuccess)
{
data[0] = node.getResponseBuffer(0);
Serial.println(data[0]);
}
}
}
void setup() {
dis_Serial.begin(9600);
Serial.begin(9600);
z_Axis.setMaxSpeed(z_AXIS_Speed);
z_Axis.setSpeed(z_AXIS_Speed);
z_Axis.setAcceleration(z_AXIS_Accelration);
z_Axis.move(60000);
RS485Serial.begin(9600);
node.begin(1, RS485Serial);
}
void loop() {
float value = 400;
en = true;
if (Serial.available() > 0) {
char cmd = Serial.read();
if (cmd == 'M' and en == true) {
z_Axis.move(1000000);
autoLoop = true;
en = false;
}
}
while (autoLoop == true) {
blinkthread();
z_Axis.run();
if (z_Axis.currentPosition() == 1000000)
{
a = true;
autoLoop = false;
Serial.println("X Completed");
}
}
}