835 lines
25 KiB
C
835 lines
25 KiB
C
#include "elemachinery.h"
|
||
//#include "stm32f4xx_hal.h"
|
||
|
||
typedef uint32_t u32;
|
||
typedef uint16_t u16;
|
||
typedef uint8_t u8;
|
||
|
||
|
||
#define Poutup(n) GPIO_SetBits(GPIOC, n) //输出
|
||
#define Poutdown(n) GPIO_ResetBits(GPIOC, n) //输出
|
||
|
||
|
||
u32 lcd_key;
|
||
char if_mobilemotor_rotate = 0; // 推拉移动电机是否旋转 旋转为1 不旋转为0
|
||
char if_rotatingmotor_rotate = 1; // 旋转采集电机是否旋转 旋转为1 不旋转为0
|
||
char mobile_configure = 0; // 推拉移动电机是否怕配置参数 0为未配置参数 1为配置参数
|
||
char rotating_configure = 0; // 旋转采集电机是否怕配置参数 0为未配置参数 1为配置参数
|
||
char step;
|
||
|
||
|
||
|
||
enum process_stage{
|
||
CHANGE_STAGE_TO_RIGHT_LIMIT_POSITION, // 找零阶段向右移动
|
||
CHANGE_STAGE_TO_LEFT_LIMIT_POSITION, // 找零阶段向左移
|
||
CLOESE_THE_LID, // 关盖阶段 左移压紧
|
||
FIND_THE_RIGHT_LIMIT_POSITION, // 找右线位阶段
|
||
FAILD_FIND, // 没有找到试剂
|
||
TESTING_SAMPLES, // 检测阶段
|
||
FIND_THE_LEFT_LIMIT_POSITION, //找右线位阶段
|
||
MOTOR_STOP //电机停止状态
|
||
};
|
||
enum rotating_stage{
|
||
GIVE_CHANGE, // 找零阶段
|
||
ROTATE, // 电机旋转
|
||
RO_MOTOR_STOP, // 找零阶段向左移
|
||
FIND_READ_FROM_CHANGE // 从零点到红色
|
||
};
|
||
enum motor_movement_direction{
|
||
MOVE_LEFT, // 向左移动
|
||
MOVE_RIGHT // 向右移动
|
||
};
|
||
|
||
enum motor_movement_direction motor_move = MOVE_LEFT;
|
||
enum process_stage pro_stage = CHANGE_STAGE_TO_RIGHT_LIMIT_POSITION; // 电机启动 进入找零阶段
|
||
enum rotating_stage rot_stage = RO_MOTOR_STOP;
|
||
|
||
void TMC2240_CS_LOW(){
|
||
|
||
Poutdown(GPIO_Pin_4);
|
||
Poutdown(GPIO_Pin_1);
|
||
#if 0
|
||
if(step == 1){
|
||
Poutdown(4);
|
||
}
|
||
else{
|
||
Poutdown(1);
|
||
}
|
||
#endif
|
||
}
|
||
void TMC2240_CS_HIGH(){
|
||
Poutup(GPIO_Pin_4);Poutup(GPIO_Pin_1);
|
||
}
|
||
|
||
// 向TMC2240写入数据
|
||
void tmc2240_write(uint8_t address, uint8_t data) {
|
||
// 先发送地址
|
||
spi1_read_write_byte(address);
|
||
// 再发送数据
|
||
spi1_read_write_byte(data);
|
||
}
|
||
|
||
// TMC2240寄存器写操作（地址+32位数据）
|
||
void TMC2240_WriteReg(uint8_t addr, uint32_t data) {
|
||
|
||
TMC2240_CS_LOW();
|
||
|
||
spi1_read_write_byte(addr | 0x80); // 写命令（bit7=1）?:ml-citation{ref="1" data="citationList"}
|
||
spi1_read_write_byte((data >> 24) & 0xFF);
|
||
spi1_read_write_byte((data >> 16) & 0xFF);
|
||
spi1_read_write_byte((data >> 8) & 0xFF);
|
||
spi1_read_write_byte(data & 0xFF);
|
||
|
||
// SoftSPI_TransferByte(addr | 0x80); // 写命令（bit7=1）?:ml-citation{ref="1" data="citationList"}
|
||
// SoftSPI_TransferByte((data >> 24) & 0xFF);
|
||
// SoftSPI_TransferByte((data >> 16) & 0xFF);
|
||
// SoftSPI_TransferByte((data >> 8) & 0xFF);
|
||
// SoftSPI_TransferByte(data & 0xFF);
|
||
|
||
|
||
TMC2240_CS_HIGH();
|
||
}
|
||
|
||
// TMC2240寄存器读操作
|
||
uint32_t TMC2240_ReadReg(uint8_t addr) {
|
||
uint32_t data = 0;
|
||
|
||
TMC2240_CS_LOW();
|
||
|
||
spi1_read_write_byte(addr & 0x7F); // 读命令（bit7=0）?:ml-citation{ref="1" data="citationList"}
|
||
data = (uint32_t)spi1_read_write_byte(0xFF) << 24;
|
||
data |= (uint32_t)spi1_read_write_byte(0xFF) << 16;
|
||
data |= (uint32_t)spi1_read_write_byte(0xFF) << 8;
|
||
data |= spi1_read_write_byte(0xFF);
|
||
|
||
// SoftSPI_TransferByte(addr & 0x7F); // 读命令（bit7=0）?:ml-citation{ref="1" data="citationList"}
|
||
// data = (uint32_t)SoftSPI_TransferByte(0xFF) << 24;
|
||
// data |= (uint32_t)SoftSPI_TransferByte(0xFF) << 16;
|
||
// data |= (uint32_t)SoftSPI_TransferByte(0xFF) << 8;
|
||
// data |= SoftSPI_TransferByte(0xFF);
|
||
|
||
TMC2240_CS_HIGH();
|
||
|
||
return data;
|
||
}
|
||
|
||
|
||
// direct_mode (Bit 16)
|
||
void TMC2240_SetDirectMode(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return; // 允许值0或1
|
||
*gconf = (*gconf & ~(1 << 16)) | ((value & 0x01) << 16);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// stop_enable (Bit 15)
|
||
void TMC2240_SetStopEnable(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 15)) | ((value & 0x01) << 15);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// small_hysteresis (Bit 14)
|
||
void TMC2240_SetSmallHysteresis(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 14)) | ((value & 0x01) << 14);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag1_pushpull (Bit 13)
|
||
void TMC2240_SetDiag1PushPull(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 13)) | ((value & 0x01) << 13);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag0_pushpull (Bit 12)
|
||
void TMC2240_SetDiag0PushPull(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 12)) | ((value & 0x01) << 12);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag1_onstate (Bit 10)
|
||
void TMC2240_SetDiag1OnState(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 10)) | ((value & 0x01) << 10);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag1_index (Bit 9)
|
||
void TMC2240_SetDiag1Index(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 9)) | ((value & 0x01) << 9);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag1_stall (Bit 8)
|
||
void TMC2240_SetDiag1Stall(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 8)) | ((value & 0x01) << 8);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag0_stall (Bit 7)
|
||
void TMC2240_SetDiag0Stall(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 7)) | ((value & 0x01) << 7);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag0_otpw (Bit 6)
|
||
void TMC2240_SetDiag0Otpw(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 6)) | ((value & 0x01) << 6);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// diag0_error (Bit 5)
|
||
void TMC2240_SetDiag0Error(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 5)) | ((value & 0x01) << 5);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// shaft (Bit 4)
|
||
void TMC2240_SetShaft(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 4)) | ((value & 0x01) << 4);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// multistep_filt (Bit 3)
|
||
void TMC2240_SetMultistepFilt(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 3)) | ((value & 0x01) << 3);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// en_pwm_mode (Bit 2)
|
||
void TMC2240_SetEnPwmMode(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 2)) | ((value & 0x01) << 2);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
// fast_standstill (Bit 1)
|
||
void TMC2240_SetFastStandstill(uint8_t value, uint32_t *gconf) {
|
||
if (value > 1) return;
|
||
*gconf = (*gconf & ~(1 << 1)) | ((value & 0x01) << 1);
|
||
TMC2240_WriteReg(0x00, *gconf);
|
||
}
|
||
|
||
void set_GCONF(uint8_t field_selector, uint8_t param_value, uint32_t *gconf) {
|
||
switch(field_selector) {
|
||
case 0x00: TMC2240_SetDirectMode(param_value, gconf); break;
|
||
case 0x01: TMC2240_SetStopEnable(param_value, gconf); break;
|
||
case 0x02: TMC2240_SetSmallHysteresis(param_value, gconf); break;
|
||
case 0x03: TMC2240_SetDiag1PushPull(param_value, gconf); break;
|
||
case 0x04: TMC2240_SetDiag0PushPull(param_value, gconf); break;
|
||
case 0x05: TMC2240_SetDiag1OnState(param_value, gconf); break;
|
||
case 0x06: TMC2240_SetDiag1Index(param_value, gconf); break;
|
||
case 0x07: TMC2240_SetDiag1Stall(param_value, gconf); break;
|
||
case 0x08: TMC2240_SetDiag0Stall(param_value, gconf); break;
|
||
case 0x09: TMC2240_SetDiag0Otpw(param_value, gconf); break;
|
||
case 0x0A: TMC2240_SetDiag0Error(param_value, gconf); break;
|
||
case 0x0B: TMC2240_SetShaft(param_value, gconf); break;
|
||
case 0x0C: TMC2240_SetMultistepFilt(param_value, gconf); break;
|
||
case 0x0D: TMC2240_SetEnPwmMode(param_value, gconf); break;
|
||
case 0x0E: TMC2240_SetFastStandstill(param_value, gconf); break;
|
||
default:
|
||
// 可添加错误处理（如打印无效选择器）
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
// IRUNDELAY (Bits 27:24)
|
||
void TMC2240_SetIRunDelay(uint8_t value, uint32_t *current_config) {
|
||
if (value > 15) return; // 错误：超出范围（0-15）
|
||
*current_config = (*current_config & 0xF0FFFFFF) | ((value & 0x0F) << 24);
|
||
TMC2240_WriteReg(0x10, *current_config);
|
||
}
|
||
|
||
// IHOLDDELAY (Bits 19:16)
|
||
void TMC2240_SetIHoldDelay(uint8_t value, uint32_t *current_config) {
|
||
if (value > 15) return;
|
||
*current_config = (*current_config & 0xFFF0FFFF) | ((value & 0x0F) << 16);
|
||
TMC2240_WriteReg(0x10, *current_config);
|
||
}
|
||
|
||
// IRUN (Bits 12:8)
|
||
void TMC2240_SetIRun(uint8_t value, uint32_t *current_config) {
|
||
if (value > 31) return; // 错误：超出范围（0-31）
|
||
*current_config = (*current_config & 0xFFFFE0FF) | ((value & 0x1F) << 8);
|
||
TMC2240_WriteReg(0x10, *current_config);
|
||
}
|
||
|
||
// IHOLD (Bits 4:0)
|
||
void TMC2240_SetIHold(uint8_t value, uint32_t *current_config) {
|
||
if (value > 31) return;
|
||
*current_config = (*current_config & 0xFFFFFFE0) | (value & 0x1F);
|
||
TMC2240_WriteReg(0x10, *current_config);
|
||
}
|
||
|
||
// CHOPCONF寄存器配置（0x6C）
|
||
void set_IHOLD_IRUN(uint8_t field_selector, uint8_t param_value, uint32_t *current_config) {
|
||
switch(field_selector) {
|
||
case 0x00: // IHOLD (0-31)
|
||
TMC2240_SetIHold(param_value, current_config);
|
||
break;
|
||
case 0x01: // IRUN (0-31)
|
||
TMC2240_SetIRun(param_value, current_config);
|
||
break;
|
||
case 0x02: // IHOLDDELAY (0-15)
|
||
TMC2240_SetIHoldDelay(param_value, current_config);
|
||
break;
|
||
case 0x03: // IRUNDELAY (0-15)
|
||
TMC2240_SetIRunDelay(param_value, current_config);
|
||
break;
|
||
default:
|
||
// 可添加错误处理（如日志输出）
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
// 设置TOFF（位0-3）
|
||
void TMC2240_SetTOFF(uint8_t toff_value, uint32_t *chopconf) {
|
||
if (toff_value > 15) return; // 错误：超出范围
|
||
*chopconf = (*chopconf & 0xFFFFFFF0) | (toff_value & 0x0F);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置HSTRT（位4-6）
|
||
void TMC2240_SetHSTRT(uint8_t hstrt_value, uint32_t *chopconf) {
|
||
if (hstrt_value > 7) return; // 错误：超出范围
|
||
*chopconf = (*chopconf & 0xFFFFF8FF) | ((hstrt_value & 0x07) << 4);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置HEND（位7-10）
|
||
void TMC2240_SetHEND(uint8_t hend_value, uint32_t *chopconf) {
|
||
if (hend_value > 15) return; // 错误：超出范围
|
||
*chopconf = (*chopconf & 0xFFFFF0FF) | ((hend_value & 0x0F) << 7);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置FD3（位11）
|
||
void TMC2240_SetFD3(uint8_t enable, uint32_t *chopconf) {
|
||
enable = (enable != 0) ? 1 : 0; // 强制转换为0或1
|
||
*chopconf = (*chopconf & 0xFFFFF7FF) | (enable << 11);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置DISFDCC（位12）
|
||
void TMC2240_SetDISFDCC(uint8_t enable, uint32_t *chopconf) {
|
||
enable = (enable != 0) ? 1 : 0;
|
||
*chopconf = (*chopconf & 0xFFFFEFFF) | (enable << 12);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置RNDTF（位13）
|
||
void TMC2240_SetRNDTF(uint8_t enable, uint32_t *chopconf) {
|
||
enable = (enable != 0) ? 1 : 0;
|
||
*chopconf = (*chopconf & 0xFFFFDFFF) | (enable << 13);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置CHM模式（位14）
|
||
void TMC2240_SetCHM(uint8_t enable, uint32_t *chopconf) {
|
||
enable = (enable != 0) ? 1 : 0;
|
||
*chopconf = (*chopconf & 0xFFFFBFFF) | (enable << 14);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置TBL（位15-16）
|
||
void TMC2240_SetTBL(uint8_t tbl_value, uint32_t *chopconf) {
|
||
if (tbl_value > 3) return; // 错误：超出范围
|
||
*chopconf = (*chopconf & 0xFFF9FFFF) | ((tbl_value & 0x03) << 15);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置VHIGHFS（位18）
|
||
void TMC2240_SetVHIGHFS(uint8_t enable, uint32_t *chopconf) {
|
||
enable = (enable != 0) ? 1 : 0;
|
||
*chopconf = (*chopconf & 0xFFFBFFFF) | (enable << 18);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置VHIGHCHM（位19）
|
||
void TMC2240_SetVHIGHCHM(uint8_t enable, uint32_t *chopconf) {
|
||
enable = (enable != 0) ? 1 : 0;
|
||
*chopconf = (*chopconf & 0xFFF7FFFF) | (enable << 19);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置TPFD（位20-23）
|
||
void TMC2240_SetTPFD(uint8_t tpfd_value, uint32_t *chopconf) {
|
||
if (tpfd_value > 15) return; // 错误：超出范围
|
||
*chopconf = (*chopconf & 0xFF0FFFFF) | ((tpfd_value & 0x0F) << 20);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
// 设置MRES（位24-27）
|
||
void TMC2240_SetMRES(uint8_t mres_value, uint32_t *chopconf) {
|
||
if (mres_value > 15) return; // 错误：超出范围
|
||
*chopconf = (*chopconf & 0xF0FFFFFF) | ((mres_value & 0x0F) << 24);
|
||
TMC2240_WriteReg(0x6C, *chopconf);
|
||
}
|
||
|
||
void set_CHOPCONF(uint8_t field_selector, uint8_t param_value, uint32_t *chopconf) {
|
||
switch(field_selector) {
|
||
case 0x00: TMC2240_SetTOFF(param_value, chopconf); break; // Bits 0-3
|
||
case 0x01: TMC2240_SetHSTRT(param_value, chopconf); break; // Bits 4-6
|
||
case 0x02: TMC2240_SetHEND(param_value, chopconf); break; // Bits 7-10
|
||
case 0x03: TMC2240_SetFD3(param_value, chopconf); break; // Bit 11
|
||
case 0x04: TMC2240_SetDISFDCC(param_value, chopconf); break; // Bit 12
|
||
case 0x05: TMC2240_SetRNDTF(param_value, chopconf); break; // Bit 13
|
||
case 0x06: TMC2240_SetCHM(param_value, chopconf); break; // Bit 14
|
||
case 0x07: TMC2240_SetTBL(param_value, chopconf); break; // Bits 15-16
|
||
case 0x08: TMC2240_SetVHIGHFS(param_value, chopconf); break; // Bit 18
|
||
case 0x09: TMC2240_SetVHIGHCHM(param_value, chopconf); break; // Bit 19
|
||
case 0x0A: TMC2240_SetTPFD(param_value, chopconf); break; // Bits 20-23
|
||
case 0x0B: TMC2240_SetMRES(param_value, chopconf); break; // Bits 24-27
|
||
default:
|
||
lcd_key |= 0x80000000; // 假设最高位用作错误标志
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
// sfilt (Bit 24)
|
||
void TMC2240_SetSFilt(uint8_t value, uint32_t *coolstep_config) {
|
||
if (value > 1) return; // 允许值0或1
|
||
*coolstep_config = (*coolstep_config & ~(1 << 24)) | ((value & 0x01) << 24);
|
||
TMC2240_WriteReg(0x6D, *coolstep_config);
|
||
}
|
||
|
||
// sgt (Bits 22:16) 有符号整数范围 -64~63
|
||
void TMC2240_SetSGT(int8_t value, uint32_t *coolstep_config) {
|
||
if (value < -64 || value > 63) return; // 错误：超出范围
|
||
*coolstep_config = (*coolstep_config & ~(0x7F << 16)) | ((value & 0x7F) << 16);
|
||
TMC2240_WriteReg(0x6D, *coolstep_config);
|
||
}
|
||
|
||
// seimin (Bit 15)
|
||
void TMC2240_SetSEIMin(uint8_t value, uint32_t *coolstep_config) {
|
||
if (value > 1) return;
|
||
*coolstep_config = (*coolstep_config & ~(1 << 15)) | ((value & 0x01) << 15);
|
||
TMC2240_WriteReg(0x6D, *coolstep_config);
|
||
}
|
||
|
||
// sedn (Bits 14:13)
|
||
void TMC2240_SetSEDN(uint8_t value, uint32_t *coolstep_config) {
|
||
if (value > 3) return; // 允许值0~3
|
||
*coolstep_config = (*coolstep_config & ~(0x03 << 13)) | ((value & 0x03) << 13);
|
||
TMC2240_WriteReg(0x6D, *coolstep_config);
|
||
}
|
||
|
||
// semax (Bits 11:8)
|
||
void TMC2240_SetSEMax(uint8_t value, uint32_t *coolstep_config) {
|
||
if (value > 15) return; // 允许值0~15
|
||
*coolstep_config = (*coolstep_config & ~(0x0F << 8)) | ((value & 0x0F) << 8);
|
||
TMC2240_WriteReg(0x6D, *coolstep_config);
|
||
}
|
||
|
||
// seup (Bits 6:5)
|
||
void TMC2240_SetSEUp(uint8_t value, uint32_t *coolstep_config) {
|
||
if (value > 3) return; // 允许值0~3
|
||
*coolstep_config = (*coolstep_config & ~(0x03 << 5)) | ((value & 0x03) << 5);
|
||
TMC2240_WriteReg(0x6D, *coolstep_config);
|
||
}
|
||
|
||
// semin (Bits 3:0)
|
||
void TMC2240_SetSEMin(uint8_t value, uint32_t *coolstep_config) {
|
||
if (value > 15) return; // 允许值0~15
|
||
*coolstep_config = (*coolstep_config & ~0x0F) | (value & 0x0F);
|
||
TMC2240_WriteReg(0x6D, *coolstep_config);
|
||
}
|
||
|
||
// COOLCONF寄存器配置函数
|
||
void set_COOLCONF(uint8_t field_selector, uint8_t param_value, uint32_t *coolstep_config) {
|
||
switch(field_selector) {
|
||
case 0x00: TMC2240_SetSEMin(param_value, coolstep_config); break;
|
||
case 0x01: TMC2240_SetSEUp(param_value, coolstep_config); break;
|
||
case 0x02: TMC2240_SetSEMax(param_value, coolstep_config); break;
|
||
case 0x03: TMC2240_SetSEDN(param_value, coolstep_config); break;
|
||
case 0x04: TMC2240_SetSEIMin(param_value, coolstep_config); break;
|
||
case 0x05: TMC2240_SetSGT((int8_t)param_value, coolstep_config); break;
|
||
case 0x06: TMC2240_SetSFilt(param_value, coolstep_config); break;
|
||
default:
|
||
// 可添加错误处理（如日志输出）
|
||
break;
|
||
}
|
||
}
|
||
|
||
//---- 左右平移电机电机参数配置
|
||
// 0x00 GCONF 参数配置
|
||
uint32_t mobile_gconf = 0x000000C4 | (1 << 7);
|
||
// 全局变量声明
|
||
uint32_t mobile_current_config = 0x00180C10; // 地址 0x10 的寄存器值
|
||
// CHOPCONF寄存器配置（地址0x6C）
|
||
uint32_t mobile_chopconf = 0x020080C3;
|
||
// 全局变量声明
|
||
uint32_t mobile_coolstep_config = 0; // 地址 0x6D 的寄存器值
|
||
//---- 旋转采集电机参数配置
|
||
// 0x00 GCONF 参数配置
|
||
uint32_t rotating_gconf = 0x000000C4 | (1 << 7);
|
||
// 全局变量声明
|
||
uint32_t rotating_current_config = 0x00180C10; // 地址 0x10 的寄存器值
|
||
// CHOPCONF寄存器配置（地址0x6C）
|
||
uint32_t rotating_chopconf = 0x020080C3;
|
||
// 全局变量声明
|
||
uint32_t rotating_coolstep_config = 0; // 地址 0x6D 的寄存器值
|
||
|
||
void motor_left(){
|
||
delay_us(500);
|
||
MOVE_THE_MOTOR_TO_THE_LEFT;
|
||
TMC2240_SetMRES(6, &mobile_gconf);
|
||
motor_move = MOVE_LEFT;
|
||
//TMC2240_WriteReg(0x10, 0x00180C10);
|
||
}
|
||
void motor_right(){
|
||
delay_us(500);
|
||
MOVE_THE_MOTOR_TO_THE_RIGHT;
|
||
TMC2240_SetMRES(6, &mobile_gconf);
|
||
motor_move = MOVE_RIGHT;
|
||
//TMC2240_WriteReg(0x10, 0x00180C10);
|
||
}
|
||
#if 0
|
||
//---- 左右平移电机电机参数配置
|
||
// 0x00 GCONF 参数配置
|
||
uint32_t mobile_gconf = 0x000000C4 | (1 << 7);
|
||
// 全局变量声明
|
||
uint32_t mobile_current_config = 0x00180C10; // 地址 0x10 的寄存器值
|
||
// CHOPCONF寄存器配置（地址0x6C）
|
||
uint32_t mobile_chopconf = 0x020080C3;
|
||
// 全局变量声明
|
||
uint32_t mobile_coolstep_config = 0; // 地址 0x6D 的寄存器值
|
||
//---- 旋转采集电机参数配置
|
||
// 0x00 GCONF 参数配置
|
||
uint32_t rotating_gconf = 0x000000C4 | (1 << 7);
|
||
// 全局变量声明
|
||
uint32_t rotating_current_config = 0x00180C10; // 地址 0x10 的寄存器值
|
||
// CHOPCONF寄存器配置（地址0x6C）
|
||
uint32_t rotating_chopconf = 0x020080C3;
|
||
// 全局变量声明
|
||
uint32_t rotating_coolstep_config = 0; // 地址 0x6D 的寄存器值
|
||
|
||
extern uint16_t Rotation_times;
|
||
char rotatingbit = 0;
|
||
void change_spi(char *str){
|
||
|
||
uint8_t stepselete = str[0];
|
||
uint8_t reg_addr = str[1];
|
||
uint8_t field_selector = str[2];
|
||
uint16_t param_value = str[3] ; // 合并高/低字节
|
||
if(stepselete == 1){
|
||
mobile_configure = 1; //配置参数结束
|
||
step = 1;
|
||
switch(reg_addr) {
|
||
case 0x00: // 用于全局状态监控和故障诊断。
|
||
set_CHOPCONF(field_selector, param_value,&mobile_gconf);
|
||
break;
|
||
case 0x10: // 用于动态调节电机电流以优化能耗和性能
|
||
set_IHOLD_IRUN(field_selector, param_value,&mobile_current_config);
|
||
break;
|
||
|
||
case 0x6C: // 用于配置静音、高效或高动态的驱动模式。
|
||
set_CHOPCONF(field_selector, param_value,&mobile_chopconf);
|
||
break;
|
||
|
||
case 0x0D: // 提供实时状态反馈，支持负载检测和温度保护。
|
||
set_COOLCONF(field_selector, param_value,&mobile_coolstep_config);
|
||
break;
|
||
case 0x01: // 正转 反转
|
||
if(param_value == 1) GPIO_SetBits(GPIOD,GPIO_Pin_8);
|
||
else GPIO_ResetBits(GPIOD,GPIO_Pin_8);
|
||
break;
|
||
case 0xFF: // 电机是否启动
|
||
if_mobilemotor_rotate = param_value;
|
||
break;
|
||
case 0xF1: // 设置电机旋转圈数
|
||
pro_stage = CLOESE_THE_LID;
|
||
rot_stage = GIVE_CHANGE;
|
||
rotatingbit = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_15);
|
||
ProcessControl();
|
||
OPENMOTOR;
|
||
break;
|
||
default:
|
||
// 可添加错误处理（如日志输出）
|
||
break;
|
||
}
|
||
mobile_configure = 0; //配置参数结束
|
||
}
|
||
else{
|
||
switch(reg_addr) {
|
||
rotating_configure = 1; //配置参数结束
|
||
step = 2;
|
||
case 0x00: // 用于全局状态监控和故障诊断。
|
||
set_CHOPCONF(field_selector, param_value,&rotating_gconf);
|
||
break;
|
||
case 0x10: // 用于动态调节电机电流以优化能耗和性能
|
||
set_IHOLD_IRUN(field_selector, param_value,&rotating_current_config);
|
||
break;
|
||
|
||
case 0x6C: // 用于配置静音、高效或高动态的驱动模式。
|
||
set_CHOPCONF(field_selector, param_value,&rotating_chopconf);
|
||
break;
|
||
|
||
case 0x0D: // 提供实时状态反馈，支持负载检测和温度保护。
|
||
set_COOLCONF(field_selector, param_value,&rotating_coolstep_config);
|
||
break;
|
||
case 0x01: // 正转 反转
|
||
if(param_value == 1) GPIO_SetBits(GPIOB,GPIO_Pin_14);
|
||
else GPIO_ResetBits(GPIOB,GPIO_Pin_14);
|
||
break;
|
||
case 0xFF: // 电机是否启动
|
||
if_rotatingmotor_rotate = param_value;
|
||
break;
|
||
case 0xF0: // 设置电机旋转圈数
|
||
Rotation_times = param_value*100;
|
||
break;
|
||
|
||
default:
|
||
// 可添加错误处理（如日志输出）
|
||
break;
|
||
}
|
||
rotating_configure = 0; //配置参数结束
|
||
}
|
||
}
|
||
extern volatile uint32_t pulse_count;
|
||
// 电机移动
|
||
//void motor_move(){
|
||
// step = 1;
|
||
// GPIO_SetBits(GPIOD,GPIO_Pin_9);
|
||
// delay_us(50);
|
||
// GPIO_ResetBits(GPIOD,GPIO_Pin_9);
|
||
// Rotation_quantity++;
|
||
// if(Rotation_quantity == 1500){
|
||
// delay_us(100);
|
||
// // TMC2240_WriteReg(0x10, 0x00190C10);
|
||
// TMC2240_SetMRES(7, &mobile_gconf);
|
||
// }
|
||
//
|
||
//}
|
||
void accelerate(){
|
||
step = 1;
|
||
if(pulse_count == 1600){
|
||
TMC2240_SetMRES(7, &mobile_gconf);
|
||
}
|
||
}
|
||
void motor_left(){
|
||
delay_us(500);
|
||
MOVE_THE_MOTOR_TO_THE_LEFT;
|
||
TMC2240_SetMRES(6, &mobile_gconf);
|
||
motor_move = MOVE_LEFT;
|
||
//TMC2240_WriteReg(0x10, 0x00180C10);
|
||
pulse_count = 0;
|
||
}
|
||
void motor_right(){
|
||
delay_us(500);
|
||
MOVE_THE_MOTOR_TO_THE_RIGHT;
|
||
TMC2240_SetMRES(6, &mobile_gconf);
|
||
motor_move = MOVE_RIGHT;
|
||
//TMC2240_WriteReg(0x10, 0x00180C10);
|
||
pulse_count = 0;
|
||
}
|
||
uint16_t rotating_rotation_times = 0; // 旋转采集电机需要旋转的圈数
|
||
char number_of_detection_cycles = 0;
|
||
// 清除线位
|
||
|
||
extern char right_limit; // 到达右线位
|
||
extern char left_limit; // 到达左线位
|
||
extern char mid_limit; // 到达左线位
|
||
void clean_limit(void){
|
||
right_limit = 0;
|
||
left_limit = 0;
|
||
mid_limit = 0;
|
||
}
|
||
// 左右平移电机电机流程控制函数
|
||
void ProcessControl(void){
|
||
step = 1;
|
||
switch(pro_stage){
|
||
case CHANGE_STAGE_TO_RIGHT_LIMIT_POSITION: // 找零阶段
|
||
// 向右
|
||
motor_right();
|
||
OPENMOTOR;
|
||
break;
|
||
case CHANGE_STAGE_TO_LEFT_LIMIT_POSITION: // 找零阶段
|
||
motor_left();
|
||
break;
|
||
case CLOESE_THE_LID: // 关盖阶段 左移压紧
|
||
motor_left();
|
||
break;
|
||
case FIND_THE_RIGHT_LIMIT_POSITION: // 找右线位阶段
|
||
motor_right();
|
||
/////--------------------还需要开启等光 待完善
|
||
break;
|
||
case TESTING_SAMPLES: // 检测阶段
|
||
rotating_rotation_times = 1600;
|
||
number_of_detection_cycles = 0;
|
||
motor_left();
|
||
break;
|
||
case FIND_THE_LEFT_LIMIT_POSITION: //找左线位阶段
|
||
rotating_rotation_times = 1600; // 因为绿色在上时 就是处于0点位置 防止出现校准问题 转开他
|
||
motor_left();
|
||
break;
|
||
case FAILD_FIND:
|
||
motor_left();
|
||
break;
|
||
}
|
||
}
|
||
extern u8 if_read7683;
|
||
void detection_steps(){
|
||
step = 1;
|
||
static u32 steps = 0;
|
||
static char needstep = 0; //记录上一次是否路过左线位 0为向左的阶段 1为向右的阶段
|
||
if(motor_move == MOVE_LEFT){
|
||
if((steps == 0) && (mid_limit == 1)){
|
||
clean_limit();
|
||
steps = 3000;
|
||
}
|
||
else if(steps > 0){
|
||
steps --;
|
||
if(steps <= 0){
|
||
motor_right();
|
||
}
|
||
}
|
||
if(pulse_count > START_THE_BLACK_LIGHT){
|
||
if_read7683 = 1;
|
||
}
|
||
}
|
||
else{
|
||
// 右移 到达右限位
|
||
if(right_limit == 1)
|
||
{
|
||
motor_left();
|
||
clean_limit();
|
||
number_of_detection_cycles++;
|
||
if(number_of_detection_cycles >= 8){ //完成8次检测
|
||
pro_stage = FIND_THE_LEFT_LIMIT_POSITION;
|
||
ProcessControl();
|
||
number_of_detection_cycles = 0;
|
||
}
|
||
}
|
||
if(pulse_count > END_THE_BLACK_LIGHT){
|
||
if_read7683 = 2;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 大电机运动
|
||
void motor_motion(void){
|
||
step = 1;
|
||
switch(pro_stage){
|
||
case CHANGE_STAGE_TO_RIGHT_LIMIT_POSITION:
|
||
if(right_limit == 1){
|
||
pro_stage = CHANGE_STAGE_TO_LEFT_LIMIT_POSITION; //进入下个阶段 向左移动
|
||
ProcessControl();
|
||
clean_limit();
|
||
}
|
||
break;
|
||
case CHANGE_STAGE_TO_LEFT_LIMIT_POSITION: // 找零阶段
|
||
if(left_limit == 1){ //找零阶段结束
|
||
clean_limit();
|
||
pro_stage = MOTOR_STOP;
|
||
CLOSEMOTOR;
|
||
pulse_count = 0;
|
||
}
|
||
break;
|
||
case CLOESE_THE_LID: // 关盖阶段 左移压紧
|
||
if(800 <= pulse_count){
|
||
// 停止左移 进入下一个步骤
|
||
pro_stage = FIND_THE_RIGHT_LIMIT_POSITION;
|
||
ProcessControl();
|
||
}
|
||
break;
|
||
case FIND_THE_RIGHT_LIMIT_POSITION: // 找右线位阶段
|
||
if(rot_stage == RO_MOTOR_STOP){
|
||
rot_stage = FIND_READ_FROM_CHANGE;
|
||
rotating_rotation_times = 4800; // 1600 转90度 目的时为了找红色
|
||
}
|
||
if((rot_stage != FIND_READ_FROM_CHANGE)||(rotating_rotation_times !=0))
|
||
{
|
||
pulse_count = 1498;
|
||
}
|
||
if(right_limit == 1){
|
||
pro_stage = TESTING_SAMPLES; //进入下个阶段 向左移动
|
||
ProcessControl();
|
||
clean_limit();
|
||
}
|
||
if(pulse_count > INFRARED_STARTING_POINT && pulse_count < INFRARED_ENDING_POINT) // 进行AD采集
|
||
if_read7683 = 1;
|
||
else if_read7683 = 2;
|
||
break;
|
||
case TESTING_SAMPLES: // 检测阶段
|
||
detection_steps();
|
||
///----采集dac数据 待完善
|
||
break;
|
||
case FIND_THE_LEFT_LIMIT_POSITION: //找左线位阶段
|
||
if(left_limit == 1){ //找零阶段结束
|
||
clean_limit();
|
||
pro_stage = MOTOR_STOP;
|
||
CLOSEMOTOR;
|
||
pulse_count =0;
|
||
////---- 是否需要关闭相应的灯光
|
||
}
|
||
break;
|
||
case FAILD_FIND:
|
||
if(left_limit == 1){ //找零阶段结束
|
||
clean_limit();
|
||
pro_stage = MOTOR_STOP;
|
||
///--- 弹出错误预警
|
||
}
|
||
break;
|
||
case MOTOR_STOP:
|
||
break;
|
||
};
|
||
}
|
||
// 电机旋转
|
||
void rotating_move(){
|
||
GPIO_SetBits(GPIOB,GPIO_Pin_15);
|
||
delay_us(100);
|
||
GPIO_ResetBits(GPIOB,GPIO_Pin_15);
|
||
}
|
||
// 小电机运动
|
||
void motor_rotating(void){
|
||
switch(rot_stage){
|
||
case ROTATE:
|
||
if(rotating_rotation_times <= 0){
|
||
rot_stage = RO_MOTOR_STOP;
|
||
}
|
||
rotating_move();
|
||
rotating_rotation_times--;
|
||
break;
|
||
case RO_MOTOR_STOP:
|
||
break;
|
||
case GIVE_CHANGE:
|
||
if(rotatingbit == 0){ // 在0点上 需要进行移动
|
||
rotatingbit = 1;
|
||
rotating_rotation_times = 1600;
|
||
}
|
||
if(rotating_rotation_times > 0){
|
||
rotating_rotation_times--;
|
||
rotating_move();
|
||
}
|
||
else{
|
||
rotating_move();
|
||
if(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_15) == 0){
|
||
rot_stage = ROTATE;
|
||
rotating_rotation_times = 200;
|
||
}
|
||
}
|
||
break;
|
||
case FIND_READ_FROM_CHANGE:
|
||
if(rotating_rotation_times > 0){
|
||
rotating_move();
|
||
rotating_rotation_times--;
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
#endif
|