224 lines
7.1 KiB
C++
224 lines
7.1 KiB
C++
#include "conveyorBeltControlUnit.h"
|
|
|
|
// 202行记得修改
|
|
// x86 无法运行
|
|
|
|
// 定义状态
|
|
const int MODE_INIT = 0; // 初始状态
|
|
const int MODE_FRUN = 1; // 正转运行状态
|
|
const int MODE_RRUN = 2; // 反转运行状态
|
|
const int MODE_CLOSING_STEP1 = 3; // 进入关闭状态步骤1
|
|
const int MODE_CLOSING_STEP2 = 4; // 进入关闭状态步骤2
|
|
const int MODE_STOP = 5;
|
|
int ConveyorBeltControlUnit::getWaitCount() {
|
|
return 2 * (0.8 / speed) / 0.1;
|
|
}
|
|
|
|
std::string ConveyorBeltControlUnit::modeToString() {
|
|
switch (currentMode) {
|
|
case MODE_INIT: return "MODE_INIT";
|
|
case MODE_FRUN: return "MODE_FRUN";
|
|
case MODE_RRUN: return "MODE_RRUN";
|
|
case MODE_CLOSING_STEP1: return "MODE_CLOSING_STEP1";
|
|
case MODE_CLOSING_STEP2: return "MODE_CLOSING_STEP2";
|
|
case MODE_STOP: return "MODE_STOP";
|
|
default: return "INVALID";
|
|
}
|
|
}
|
|
|
|
void ConveyorBeltControlUnit::action(int event) {
|
|
/* int destMode = modeSwitchMap[currentMode][event];
|
|
if (destMode == currentMode) { return; }
|
|
|
|
switch (destMode) {
|
|
case MODE_INIT:
|
|
digitalWrite(OUT_FRUN, OUT_OFF);
|
|
digitalWrite(OUT_RRUN, OUT_OFF);
|
|
break;
|
|
case MODE_FRUN:
|
|
digitalWrite(OUT_FRUN, OUT_ON);
|
|
digitalWrite(OUT_RRUN, OUT_OFF);
|
|
break;
|
|
case MODE_RRUN:
|
|
digitalWrite(OUT_FRUN, OUT_OFF);
|
|
digitalWrite(OUT_RRUN, OUT_ON);
|
|
break;
|
|
case MODE_CLOSING_STEP1: break;
|
|
case MODE_CLOSING_STEP2:
|
|
digitalWrite(OUT_FRUN, OUT_OFF);
|
|
digitalWrite(OUT_RRUN, OUT_ON);
|
|
break;
|
|
case MODE_STOP:
|
|
digitalWrite(OUT_FRUN, OUT_OFF);
|
|
digitalWrite(OUT_RRUN, OUT_OFF);
|
|
workState = STANDBY;
|
|
break;
|
|
default: break;
|
|
}
|
|
currentMode = destMode;
|
|
LOG(INFO) << GREEN << "now mode is " << modeToString() << EMPTY; */
|
|
}
|
|
|
|
void ConveyorBeltControlUnit::conveyorBeltWorkingProcess() {
|
|
/* workState = RUNNING;
|
|
int waitCount = 0;
|
|
int count = 0;
|
|
while (workState == RUNNING) {
|
|
waitCount = getWaitCount();
|
|
if (currentMode == MODE_CLOSING_STEP1 || currentMode == MODE_CLOSING_STEP2) {
|
|
count--;
|
|
if (count < 0) { action(EVENT_RSENSOR_OBSTRUCT); }
|
|
}
|
|
else { count = waitCount; }
|
|
|
|
if (current_start_signal == 1) {
|
|
LOG(INFO) << YELLOW << "find a start signal" << EMPTY;
|
|
current_start_signal = 0;
|
|
action(EVENT_START_RISING);
|
|
}
|
|
|
|
if (digitalRead(INPUT_FSENSOR) == IN_LOGIC_1) {
|
|
// LOG(INFO) << YELLOW << "find a Fsensor signal" << EMPTY;
|
|
action(EVENT_FSENSOR_OBSTRUCT);
|
|
}
|
|
|
|
if (digitalRead(INPUT_RSENSOR) == IN_LOGIC_1) {
|
|
// LOG(INFO) << YELLOW << "find a Rsensor signal" << EMPTY;
|
|
action(EVENT_RSENSOR_OBSTRUCT);
|
|
}
|
|
|
|
if (current_stop_signal == 1) {
|
|
LOG(INFO) << YELLOW << "find a stop signal" << EMPTY;
|
|
LOG(INFO) << YELLOW << "wait count is " << waitCount << " last wait count left "
|
|
<< count << EMPTY;
|
|
current_stop_signal = 0;
|
|
action(EVENT_STOP_PUSH);
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
} */
|
|
}
|
|
|
|
int ConveyorBeltControlUnit::init() {
|
|
current_start_signal = 0;
|
|
current_stop_signal = 0;
|
|
currentMode = MODE_INIT;
|
|
speed = 0;
|
|
workState = STANDBY;
|
|
return 0;
|
|
}
|
|
|
|
int ConveyorBeltControlUnit::setSpeed(float speed) {
|
|
// if (workState == RUNNING)
|
|
// {
|
|
// return -1;
|
|
// }
|
|
// 原本已经有的注释
|
|
|
|
/* int regValue = speed * 62500;
|
|
modbus_t* ctx = modbus_new_rtu("/dev/ttyUSB0", 9600, 'N', 8, 2);
|
|
modbus_set_slave(ctx, 1);
|
|
if (modbus_connect(ctx) == -1) {
|
|
fprintf(stderr, "Connection failed:%s\n", modbus_strerror(errno));
|
|
modbus_free(ctx);
|
|
return -1;
|
|
}
|
|
LOG(INFO) << YELLOW << "set speed:" << speed << " set register:" << regValue << EMPTY;
|
|
int result = modbus_write_register(ctx, 4096, regValue);
|
|
if (result == -1) {
|
|
fprintf(stderr, "write failed:%s\n", modbus_strerror(errno));
|
|
modbus_free(ctx);
|
|
return -1;
|
|
}
|
|
LOG(INFO) << YELLOW << "get result from com" << ":" << result << EMPTY;
|
|
uint16_t* speedBuffer = (uint16_t*)malloc(sizeof(uint16_t));
|
|
int getBytes = modbus_read_registers(ctx, 4096, 2, speedBuffer);
|
|
LOG(INFO) << YELLOW << "get result from com : " << getBytes << " register:" << *speedBuffer
|
|
<< EMPTY;
|
|
this->speed = speed;
|
|
modbus_free(ctx); */
|
|
return 0;
|
|
}
|
|
|
|
int ConveyorBeltControlUnit::setDirection(int direction) {
|
|
if (workState == RUNNING) { return -1; }
|
|
|
|
if (direction == 0) {
|
|
LOG(INFO) << YELLOW << "set direction to foreward" << EMPTY;
|
|
INPUT_FSENSOR = 28;
|
|
INPUT_RSENSOR = 29;
|
|
OUT_FRUN = 14;
|
|
OUT_RRUN = 10;
|
|
}
|
|
else if (direction == 1) {
|
|
LOG(INFO) << YELLOW << "set direction to reversal" << EMPTY;
|
|
INPUT_FSENSOR = 29;
|
|
INPUT_RSENSOR = 28;
|
|
OUT_FRUN = 10;
|
|
OUT_RRUN = 14;
|
|
}
|
|
else { return -1; }
|
|
return 0;
|
|
}
|
|
|
|
int ConveyorBeltControlUnit::startConveyorBelt() {
|
|
current_start_signal = 1;
|
|
return 0;
|
|
}
|
|
|
|
int ConveyorBeltControlUnit::stopConveyorBelt() {
|
|
current_stop_signal = 1;
|
|
return 0;
|
|
}
|
|
|
|
int ConveyorBeltControlUnit::getWorkState() {
|
|
if (workState == RUNNING) { return 0; }
|
|
else { return -1; }
|
|
}
|
|
|
|
ConveyorBeltControlUnit::ConveyorBeltControlUnit(/* args */) {
|
|
modeSwitchMap[MODE_INIT]
|
|
= {{EVENT_START_RISING, MODE_FRUN},
|
|
{EVENT_FSENSOR_OBSTRUCT, MODE_INIT},
|
|
{EVENT_RSENSOR_OBSTRUCT, MODE_INIT},
|
|
{EVENT_STOP_PUSH, MODE_INIT}};
|
|
modeSwitchMap[MODE_FRUN]
|
|
= {{EVENT_START_RISING, MODE_FRUN},
|
|
{EVENT_FSENSOR_OBSTRUCT, MODE_RRUN},
|
|
{EVENT_RSENSOR_OBSTRUCT, MODE_FRUN},
|
|
{EVENT_STOP_PUSH, MODE_CLOSING_STEP1}};
|
|
modeSwitchMap[MODE_RRUN]
|
|
= {{EVENT_START_RISING, MODE_RRUN},
|
|
{EVENT_FSENSOR_OBSTRUCT, MODE_RRUN},
|
|
{EVENT_RSENSOR_OBSTRUCT, MODE_FRUN},
|
|
{EVENT_STOP_PUSH, MODE_CLOSING_STEP2}};
|
|
modeSwitchMap[MODE_CLOSING_STEP1]
|
|
= {{EVENT_START_RISING, MODE_CLOSING_STEP1},
|
|
{EVENT_FSENSOR_OBSTRUCT, MODE_CLOSING_STEP2},
|
|
{EVENT_RSENSOR_OBSTRUCT, MODE_STOP},
|
|
{EVENT_STOP_PUSH, MODE_CLOSING_STEP1}};
|
|
modeSwitchMap[MODE_CLOSING_STEP2]
|
|
= {{EVENT_START_RISING, MODE_CLOSING_STEP2},
|
|
{EVENT_FSENSOR_OBSTRUCT, MODE_CLOSING_STEP2},
|
|
{EVENT_RSENSOR_OBSTRUCT, MODE_STOP},
|
|
{EVENT_STOP_PUSH, MODE_CLOSING_STEP2}};
|
|
// x86 无法运行
|
|
/* wiringPiSetup();
|
|
pinMode(BTN_START, INPUT);
|
|
pinMode(BTN_DIRSWITCH, INPUT);
|
|
pinMode(BTN_STOP, INPUT);
|
|
pinMode(INPUT_FSENSOR, INPUT);
|
|
pinMode(INPUT_RSENSOR, INPUT);
|
|
pullUpDnControl(BTN_START, PUD_UP);
|
|
pullUpDnControl(BTN_DIRSWITCH, PUD_UP);
|
|
pullUpDnControl(BTN_STOP, PUD_UP);
|
|
pullUpDnControl(INPUT_FSENSOR, PUD_UP);
|
|
pullUpDnControl(INPUT_RSENSOR, PUD_UP);
|
|
pinMode(OUT_RRUN, OUTPUT);
|
|
pinMode(OUT_FRUN, OUTPUT);
|
|
digitalWrite(OUT_FRUN, OUT_OFF);
|
|
digitalWrite(OUT_RRUN, OUT_OFF); */
|
|
|
|
}
|
|
|
|
ConveyorBeltControlUnit::~ConveyorBeltControlUnit() {}
|