303 lines
7.9 KiB
C
303 lines
7.9 KiB
C
/*
|
|
* Core.c
|
|
*
|
|
* Created on: 2023年5月17日
|
|
* Author: User
|
|
*/
|
|
|
|
#include "Core.h"
|
|
|
|
#include "BLTouch.h"
|
|
#include "EndStop.h"
|
|
#include "GPIO.h"
|
|
#include "Key.h"
|
|
#include "Stepper.h"
|
|
#include "TM1650.h"
|
|
#include "delay.h"
|
|
#include "main.h"
|
|
|
|
static void runIdle(); // 空闲状态
|
|
static void runHome(); // 执行回零
|
|
static void runTest(); // 运行测试
|
|
static void error(u8); // 报警提示
|
|
|
|
u8 runMode = MODE_IDLE;
|
|
u8 runState = STATE_INIT;
|
|
u8 errorCode = ERROR_NONE;
|
|
|
|
/**
|
|
* P30 准双向 RXD
|
|
* P31 准双向 TXD
|
|
* P32 准双向 BLTouch零位输入
|
|
* P33 准双向 机械零位输入
|
|
* P36 高阻输入 按键
|
|
* P37 高阻输入 按键
|
|
* P10 准双向 悬空
|
|
* P11 推挽 BLTouch信号输出 PWM
|
|
* P12 推挽 电机方向信号 DIR
|
|
* P13 推挽 电机步进信号 STEP
|
|
* P14 推挽 电机使能信号 EN
|
|
* P15 准双向 SCL
|
|
* P54 准双向 SDA
|
|
* P55 准双向 悬空
|
|
*/
|
|
static void GPIO_config(void) {
|
|
GPIO_InitTypeDef GPIO_InitStructure;
|
|
|
|
// 准双向
|
|
GPIO_InitStructure.Mode = GPIO_PullUp;
|
|
GPIO_InitStructure.Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
|
|
GPIO_Inilize(GPIO_P3, &GPIO_InitStructure);
|
|
GPIO_InitStructure.Pin = GPIO_Pin_5;
|
|
GPIO_Inilize(GPIO_P1, &GPIO_InitStructure);
|
|
GPIO_InitStructure.Pin = GPIO_Pin_4;
|
|
GPIO_Inilize(GPIO_P5, &GPIO_InitStructure);
|
|
|
|
// 高阻输入
|
|
GPIO_InitStructure.Mode = GPIO_HighZ;
|
|
GPIO_InitStructure.Pin = GPIO_Pin_6 | GPIO_Pin_7;
|
|
GPIO_Inilize(GPIO_P3, &GPIO_InitStructure);
|
|
|
|
// 推挽输出
|
|
GPIO_InitStructure.Mode = GPIO_OUT_PP;
|
|
GPIO_InitStructure.Pin = GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4;
|
|
GPIO_Inilize(GPIO_P1, &GPIO_InitStructure);
|
|
}
|
|
|
|
void init() {
|
|
GPIO_config(); // 初始化IO口
|
|
EndStop_Init(); // 初始化零位中断
|
|
Stepper_Init(); // 初始化电机定时器
|
|
BLTouch_Init(); // 初始化BLTouch的PWM发生器
|
|
KEY_Init(); // 初始化按键
|
|
EA = 1; // 开启全局中断
|
|
|
|
// 复位BLTouch
|
|
BLTouch_Reset();
|
|
|
|
// 复位数码管
|
|
TM1650.Init();
|
|
TM1650.Display_IDLE(&IIC);
|
|
|
|
// 上电自动回零
|
|
changeMode(MODE_HOME);
|
|
}
|
|
|
|
void core() {
|
|
switch (runMode) {
|
|
case MODE_IDLE: runIdle(); break;
|
|
case MODE_HOME: runHome(); break;
|
|
case MODE_TEST: runTest(); break;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 禁用电机
|
|
*/
|
|
static void runIdle() {
|
|
switch (runState) {
|
|
case STATE_INIT:
|
|
STOP_STEPPER; // 禁用电机中断
|
|
BLTouch_Reset(); // BLTouch复位
|
|
BLTouch_Set(UP); // BLTouch收回探针
|
|
TM1650.Display_IDLE(&IIC); // 显示----
|
|
runState = STATE_DONE;
|
|
break;
|
|
|
|
case STATE_IDLE_SHOW:
|
|
TM1650.Display_Num(&IIC, Stepper_GetPos()); // 显示数字
|
|
runState = STATE_DONE;
|
|
break;
|
|
case STATE_ERROR:
|
|
STOP_STEPPER; // 禁用电机中断
|
|
Stepper_SetEnable(DISABLE); // 失能电机
|
|
TM1650.Display_Err(&IIC, errorCode); // 显示错误
|
|
runState = STATE_DONE;
|
|
break;
|
|
|
|
case STATE_DONE:
|
|
if (BLTOUCH_TRIGGER && (errorCode != ERROR_BLTOUCH)) {
|
|
error(ERROR_BLTOUCH);
|
|
return;
|
|
}
|
|
|
|
if (KEY_isTrigger(KEY_CODE_HOME)) {
|
|
changeMode(MODE_HOME);
|
|
TM1650.Display_Fun(&IIC, FUN_HOME);
|
|
} else if (KEY_isTrigger(KEY_CODE_TEST)) {
|
|
changeMode(MODE_TEST);
|
|
TM1650.Display_Fun(&IIC, FUN_TEST);
|
|
} else if (KEY_isTrigger(KEY_CODE_IDLE)) {
|
|
Stepper_SetEnable(DISABLE); // 失能电机
|
|
TM1650.Display_OFF(&IIC);
|
|
}
|
|
break;
|
|
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 1. BLTouch置位
|
|
* 2. 电机回零
|
|
* 3. 数码管显示
|
|
*/
|
|
static void runHome() {
|
|
switch (runState) {
|
|
case STATE_INIT:
|
|
STOP_STEPPER;
|
|
BLTouch_Reset(); // BLTouch复位
|
|
BLTouch_Set(UP); // BLTouch收回探针
|
|
Stepper_SetEnable(ENABLE); // 使能电机
|
|
changeState(STATE_HOME_FIRST_INIT); // 切换状态
|
|
break;
|
|
|
|
case STATE_HOME_FIRST_INIT: // 首次回零
|
|
Stepper_SetHz(STEPPER_FAST_HZ);
|
|
if (ENDSTOP_NORMAL) // 正常状态
|
|
{
|
|
Stepper_SetDir(DOWN);
|
|
Stepper_SetPos(HOME_PRESET_POS);
|
|
changeState(STATE_HOME_FIRST_WAIT);
|
|
} else // 回零时零位已经触发
|
|
{
|
|
Stepper_SetDir(UP);
|
|
Stepper_SetPos(0);
|
|
changeState(STATE_HOME_LEAVE_WAIT);
|
|
}
|
|
RUN_STEPPER;
|
|
break;
|
|
|
|
case STATE_HOME_LEAVE_INIT: // 离开一点
|
|
Stepper_SetHz(STEPPER_FAST_HZ);
|
|
Stepper_SetDir(UP);
|
|
Stepper_SetPos(0);
|
|
changeState(STATE_HOME_LEAVE_WAIT);
|
|
RUN_STEPPER;
|
|
break;
|
|
|
|
case STATE_HOME_AGAIN_INIT: // 再次回零
|
|
Stepper_SetHz(STEPPER_SLOW_HZ);
|
|
Stepper_SetDir(DOWN);
|
|
Stepper_SetPos(HOME_LEAVE_POS * 2);
|
|
changeState(STATE_HOME_AGAIN_WAIT);
|
|
RUN_STEPPER;
|
|
break;
|
|
|
|
case STATE_HOME_FIRST_WAIT: // 等待首次回零
|
|
// 当前坐标已经过零
|
|
if (Stepper_ComparePos(HOME_PRESET_POS)) {
|
|
error(ERROR_HOME_FIRST);
|
|
}
|
|
break;
|
|
|
|
case STATE_HOME_LEAVE_WAIT: // 等待离开
|
|
// 当前坐标已经完成
|
|
if (Stepper_ComparePos(HOME_LEAVE_POS)) {
|
|
if (ENDSTOP_TRIGGER) {
|
|
error(ERROR_HOME_LEAVE);
|
|
} else {
|
|
STOP_STEPPER;
|
|
changeState(STATE_HOME_AGAIN_INIT);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case STATE_HOME_AGAIN_WAIT: // 等待再次回零
|
|
// 当前坐标已经过零
|
|
if (Stepper_ComparePos(HOME_LEAVE_POS * 2)) {
|
|
error(ERROR_HOME_AGAIN);
|
|
}
|
|
break;
|
|
|
|
case STATE_DONE:
|
|
Stepper_SetPos(0);
|
|
changeMode(MODE_IDLE);
|
|
break;
|
|
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 1. 电机上升
|
|
* 2. BLTouch触发检测
|
|
* 3. 数码管显示
|
|
*/
|
|
static void runTest() {
|
|
switch (runState) {
|
|
case STATE_INIT:
|
|
BLTouch_Reset(); // BLTouch复位
|
|
BLTouch_Set(UP); // BLTouch收回探针
|
|
delay_ms(250);
|
|
delay_ms(250);
|
|
if (BLTOUCH_TRIGGER) { // BLTouch异常
|
|
error(ERROR_BLTOUCH);
|
|
} else if (!Stepper_IsEnable()) { // 电机已经解锁
|
|
error(ERROR_TEST_INIT);
|
|
} else {
|
|
changeState(STATE_TEST_INIT); // 准备测试
|
|
}
|
|
break;
|
|
case STATE_TEST_INIT:
|
|
Stepper_SetHz(STEPPER_FAST_HZ);
|
|
if (Stepper_ComparePos(TEST_INIT_POS)) { // 当当前位置高于测试初始位置
|
|
Stepper_SetDir(DOWN);
|
|
changeState(STATE_TEST_FAST_DOWN_WAIT);
|
|
} else { // 当前位置低于测试初始位置
|
|
BLTouch_Set(DOWN);
|
|
Stepper_SetDir(UP);
|
|
changeState(STATE_TEST_FAST_UP_WAIT);
|
|
}
|
|
RUN_STEPPER;
|
|
break;
|
|
case STATE_TEST_FAST_DOWN_WAIT:
|
|
if (!Stepper_ComparePos(TEST_INIT_POS)) { // 已经降低到测试位置
|
|
STOP_STEPPER;
|
|
BLTouch_Set(DOWN); // BLTouch放下探针
|
|
Stepper_SetHz(STEPPER_SLOW_HZ); // 调整到测试速度
|
|
Stepper_SetDir(UP); // 上升去测试
|
|
changeState(STATE_TEST_SLOW_WAIT); // 等待测试
|
|
RUN_STEPPER;
|
|
}
|
|
break;
|
|
case STATE_TEST_FAST_UP_WAIT:
|
|
if (Stepper_ComparePos(TEST_INIT_POS)) { // 已经上升到测试位置
|
|
Stepper_SetHz(STEPPER_SLOW_HZ); // 调整到测试速度
|
|
changeState(STATE_TEST_SLOW_WAIT);
|
|
}
|
|
break;
|
|
case STATE_TEST_SLOW_WAIT:
|
|
if (Stepper_ComparePos(TEST_MAX_POS)) {
|
|
error(ERROR_TEST_MAX);
|
|
}
|
|
break;
|
|
case STATE_DONE:
|
|
BLTouch_Set(UP);
|
|
changeMode(MODE_IDLE); // 切换空闲模式
|
|
changeState(STATE_IDLE_SHOW); // 切换到显示状态
|
|
break;
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 回零距离过长报警
|
|
* BLTouch异常报警
|
|
*/
|
|
static void error(u8 errorNum) {
|
|
changeMode(MODE_IDLE); // 切换空闲模式
|
|
changeState(STATE_ERROR); // 切换错误状态
|
|
errorCode = errorNum; // 设置错误代码
|
|
}
|
|
|
|
void changeMode(u8 mode) {
|
|
runMode = mode;
|
|
runState = STATE_INIT;
|
|
errorCode = ERROR_NONE;
|
|
}
|
|
|
|
void changeState(u8 state) {
|
|
runState = state;
|
|
}
|