|
|
#include <SPI.h>
#include <Ethernet.h>
#include <Servo.h>
// 采用 MyArduinoProjects Modbus TCP 程序库
// http://myarduinoprojects.com/modbus.html
#include "MgsModbus.h"
MgsModbus Mb;
// 设置网络IP位址 (网络扩充卡 MAC 可自行修改 +1 避免冲突)
byte mac[] = {0x90, 0xA2, 0xDA, 0x0E, 0x94, 0xB5 };
IPAddress ip(192, 168, 3, 161);
IPAddress gateway(192, 168, 3, 1);
IPAddress subnet(255, 255, 255, 0);
int pos = 0; // 设置 Servo 位置的变量
Servo myservo; // 定义舵机变量
unsigned char i=0;
#define EN 8 // X,Y,Z轴步进电机使能端，低电平有效
#define X_DIR 5 // X轴 步进电机方向控制
#define Y_DIR 6 // y轴 步进电机方向控制
#define Z_DIR 7 // z轴 步进电机方向控制
#define X_STP 2 // x轴 步进控制
#define Y_STP 3 // y轴 步进控制
#define Z_STP 4 // z轴 步进控制
#define AEN 28 // A轴步进电机使能端，低电平有效
#define A_DIR 26 // A轴 步进电机方向控制
#define A_STP 24 // A轴 步进控制
int armx=0;
int army=0;
int armz=450;
int arma=0;
int armc=60;
int larmx=0;
int larmy=0;
int larmz=450;
int larma=0;
int larmc=60;
// #include <Stepper.h>
#define STEPS 200 //定义步进马达每圈的步数
//steps:代表马达转完一圈需要多少步数。如果马达上有标示每步的度数，
//将360除以这个角度，就可以得到所需要的步数(例如：360/1.8=200)。
//螺杆行程是 2mm 代表转一圈 移动的距离
/*
//函数：step 功能：控制步进电机方向，步数。
//参数：dir 方向控制, dirPin对应步进电机的DIR引脚，stepperPin 对应步进电机的step引脚， steps 步进的步数
//无返回值
*/
void step(boolean dir, byte dirPin, byte stepperPin, int steps)
{
digitalWrite(dirPin, dir);
delay(50);
for (int i = 0; i < steps; i++) {
digitalWrite(stepperPin, HIGH);
delayMicroseconds(1500);
digitalWrite(stepperPin, LOW);
delayMicroseconds(1350);
}
}
void stepZ(boolean dir, byte dirPin, byte stepperPin, int steps) // 转盘转动速度太快会抖动 所以放慢速度 用单独的函数
{
digitalWrite(dirPin, dir);
delay(50);
for (int i = 0; i < steps; i++) {
digitalWrite(stepperPin, HIGH);
delayMicroseconds(12000);
digitalWrite(stepperPin, LOW);
delayMicroseconds(10800);
}
}
void setup(){
Serial.begin(9600);
Ethernet.begin(mac, ip, gateway, subnet); //启动网络
Serial.println("网络已经开通");
//设置要使用的寄存器位址
//0 1 2 3 4 是 Holding 寄存器的顺序,其位址分别是10000,10001,10002,10003,10004
// 添加寄存器 mb.MbData(i);
Mb.MbData[0] = 0; // X轴行程 300mm 水平前进初始位置 转 200步移动 2mm,所以最大可以走 6000步 ,初始值取原点,也就是水平平台最接近马达处
Mb.MbData[1] = 0; // Y轴行程 210mm 往上初始位置 转 200步移动 2mm,所以最大可以走 4200步 ,初始值取原点,也就是垂直平台最接近马达处
Mb.MbData[2] = 450; // Z轴转盘初始位置 转360步 相当于转动 90度 Z轴可转动 180 度相当于 720步,初始值取中间值,90度,也就是爪子轴在中间
Mb.MbData[3] = 0; // A轴 爪子延伸轴初始位置
Mb.MbData[4] = 60; // 爪子初始位置 爪子可开启最大转动步数为 180 因为开机会自动开到最大 所以设 0 舵机最大旋转角度也只有 180度
myservo.attach(22); //定义舵机接口
pinMode(X_DIR, OUTPUT); pinMode(X_STP, OUTPUT); //将步进电机用到的 IO 脚位设置成输出
pinMode(Y_DIR, OUTPUT); pinMode(Y_STP, OUTPUT);
pinMode(Z_DIR, OUTPUT); pinMode(Z_STP, OUTPUT);
pinMode(A_DIR, OUTPUT); pinMode(A_STP, OUTPUT);
pinMode(EN, OUTPUT);
digitalWrite(EN, LOW); // 开启 CNC 开发板的马达启动功能
pinMode(AEN, OUTPUT);
digitalWrite(AEN, LOW);
}
void loop(){
// 读取数位寄存器的数值,
armx=Mb.MbData[0]; // X轴 数值变小 因为 Modbus 无法存负数 所以中心点设为 45 以 X轴而言 数值变小往前
army=Mb.MbData[1]; // Y轴 数值变小 Y轴 往上
armz=Mb.MbData[2]; // Z轴 数值变小 转盘 往左
arma=Mb.MbData[3]; // A轴
armc=Mb.MbData[4]; // 0 爪子关 1 爪子开 开机时爪子会自动打开
if (armx!=larmx){
if (armx<larmx){
step(true, X_DIR, X_STP, (larmx-armx)); // X轴电机后退 顺时旋转转1圈，200步为一圈
}
else {
step(false, X_DIR, X_STP, (armx-larmx)); // X轴电机後前
}
larmx=armx;
}
if (army!=larmy){
if (army<larmy){
step(false, Y_DIR, Y_STP, (larmy-army)); // Y轴电机往下
}
else {
step(true, Y_DIR, Y_STP, (army-larmy)); // Y轴电机往上
}
larmy=army;
}
if (armz!=larmz){
if (armz<larmz){
stepZ(false, Z_DIR, Z_STP, (larmz-armz)); //Z轴电机 往左旋转
}
else {
stepZ(true, Z_DIR, Z_STP, (armz-larmz)); //Z轴电机 往右旋转
}
larmz=armz;
}
if (arma!=larma){
if (arma<larma){
stepz(true, A_DIR, A_STP, (larma-arma)); // A轴电机往后
}
else {
stepz(false, A_DIR, A_STP, (arma-larma)); // A轴电机往前
}
larma=arma;
}
if (armc!=larmc){
if (armc<larmc){ //数值变大 舵机顺时旋转 关闭爪子
for (pos = larmc; pos >= armc; pos -= 1)
{
myservo.write(pos); // 告诉 servo 走到 pos 的位置 舵机正旋转 关闭爪子 打开爪子
delay(5); // 等待 15ms 让 servo 走到指定位置
}
}
else
{ // 舵机逆时转 180度 打开爪子
for (pos = larmc; pos <= armc; pos += 1)
{
myservo.write(pos); // 告诉 servo 走到 'pos' 的位置
delay(5); // 等待 15ms 让 servo 走到指定位置
}
}
larmc=armc;
}
/**
if (armc!=larmc){
if (armc==1){ //舵机顺时旋转 打开爪子
for (pos = 180; pos >= 0; pos -= 1) // 从 180 度旋转到 0 度，每次 1 度
{
myservo.write(pos); // 告诉 servo 走到 pos 的位置 舵机正旋转 打开爪子
delay(15); // 等待 15ms 让 servo 走到指定位置
}
}
if (armc==0){ // 舵机逆时转 180度 爪子关
for (pos = 0; pos <= 180; pos += 1) // 从 0 度旋转到 180 度，每次 1 度
{
myservo.write(pos); // 告诉 servo 走到 'pos' 的位置
delay(15); // 等待 15ms 让 servo 走到指定位置
}
}
larmc=armc;
}
**/
Mb.MbsRun(); //调用 Modbus
}
void autorun()
{
step(false, X_DIR, X_STP, 240); // X轴电机 反转1圈，200步为一圈 方向往前
delay(1000);
step(false, Y_DIR, Y_STP, 240); // y轴电机 反转1圈，200步步为一圈 方向往上
delay(1000);
step(false, Z_DIR, Z_STP, 240); // z轴电机 反转1圈，30步为一圈 方向往左旋转
delay(1000);
for (pos = 180; pos >= 0; pos -= 1) //舵机顺时旋转 打开爪子
{ // 从 180 度旋转到 0 度，每次 1 度
myservo.write(pos); // 告诉 servo 走到 'pos' 的位置
delay(15); // 等待 15ms 让 servo 走到指定位置
}
delay(2000);
step(true, X_DIR, X_STP, 240); // X轴电机 正转1圈，200步为一圈 方向往後
delay(1000);
step(true, Y_DIR, Y_STP, 240); // y轴电机 正转1圈，200步为一圈 方向往下
delay(1000);
step(true, Z_DIR, Z_STP, 240); // z轴电机 正转1圈，200步为一圈 方向往右
delay(1000);
for (pos = 0; pos <= 180; pos += 1) // 舵机逆时转 180度 爪子关
{ // 从 0 度旋转到 180 度，每次 1 度
myservo.write(pos); // 告诉 servo 走到 'pos' 的位置
delay(15); // 等待 15ms 让 servo 走到指定位置
}
delay(2000);
} |
|
发表于 2022-8-19 13:57
楼主