p_Intelligent_winding_robot_v2
/
Intelligent_winding_robot_main_board
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
abf6547e77
commit
902480e449
1 changed files with 250 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 270
      usrc/main.cpp

270
usrc/main.cpp
View File

@ -6,6 +6,8 @@
//
#include "sdk/os/zos.hpp"
#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
#include "sdk\components\iflytop_can_slave_module_master_end\stepmotor.hpp"
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_master.hpp"
#include "sdk\components\step_motor_45\step_motor_45.hpp"
#include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp"
@ -16,7 +18,12 @@ Main gmain;
using namespace iflytop;
using namespace std;
#define CHECK_ARGC(n) \
if (argc != (n + 1)) { \
ZLOGE(TAG, "argc != %d", n); \
context->breakflag = true; \
return; \
}
extern "C" {
void StartDefaultTask(void const* argument) { umain(); }
}
@ -94,16 +101,226 @@ static StepMotor45::cfg_t cfg5 = {
.driverPin = {PA13, PA14, PA15, PC10},
.driverPinMirror = false,
};
static StepMotor45::cfg_t cfg6 = {
.max_pos = -1,
.enable_zero_limit = false,
.enable_max_pos_limit = false,
.mirror = false,
.zeroPin = PinNull,
.zeroPinMirror = false,
.driverPin = {PC12, PD3, PD5, PD7},
.driverPinMirror = false,
};
map<int, float> screw_lead = {
{1, 10.0}, //
{2, 10.0}, //
{3, 10.0}, //
{4, 10.0}, //
{5, 10.0}, //
{6, 10.0}, //
};
/*******************************************************************************
* ´úÂë *
*******************************************************************************/
static StepMotor45 g_step_motor45_1;
static StepMotor45 g_step_motor45_2;
static StepMotor45 g_step_motor45_3;
static StepMotor45 g_step_motor45_4;
static StepMotor45 g_step_motor45_5;
static StepMotor45 g_step_motor45[7];
StepMotor g_step_motor[10];
IflytopCanMaster m_IflytopCanMaster;
static CmdScheduler cmdScheduler;
bool distance_mm_to_step(int motorid, float distance_mm, int32_t* distance) {
if (screw_lead.find(motorid) == screw_lead.end()) {
return false;
}
float lead = screw_lead[motorid];
*distance = distance_mm / lead * 51200;
return true;
}
void regfn() {
cmdScheduler.registerCmd("help", //
[](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); });
// stepmotor45
cmdScheduler.registerCmd("stepmotor45_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
// stepmotor45_rotate motorid direction
CHECK_ARGC(2);
int motorid = atoi(argv[1]);
bool direction = atoi(argv[2]);
if (motorid < 1 || motorid > 6) {
ZLOGE(TAG, "motorid out of range");
return;
}
g_step_motor45[motorid].rotate(direction);
});
cmdScheduler.registerCmd("stepmotor45_readPos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
// stepmotor45_rotate motorid direction
CHECK_ARGC(2);
int motorid = atoi(argv[1]);
if (motorid < 1 || motorid > 6) {
ZLOGE(TAG, "motorid out of range");
return;
}
ZLOGI(TAG, "motorid %d pos %d", motorid, g_step_motor45[motorid].getPos());
});
cmdScheduler.registerCmd("stepmotor45_moveTo", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
// stepmotor45_rotate motorid pos
CHECK_ARGC(2);
int motorid = atoi(argv[1]);
int pos = atoi(argv[2]);
if (motorid < 1 || motorid > 6) {
ZLOGE(TAG, "motorid out of range");
return;
}
g_step_motor45[motorid].moveTo(pos);
});
cmdScheduler.registerCmd("stepmotor45_moveBy", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
// stepmotor45_rotate motorid pos
CHECK_ARGC(2);
int motorid = atoi(argv[1]);
int pos = atoi(argv[2]);
if (motorid < 1 || motorid > 6) {
ZLOGE(TAG, "motorid out of range");
return;
}
g_step_motor45[motorid].moveBy(pos);
});
cmdScheduler.registerCmd("stepmotor45_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
// stepmotor45_rotate motorid pos
CHECK_ARGC(1);
int motorid = atoi(argv[1]);
if (motorid < 1 || motorid > 6) {
ZLOGE(TAG, "motorid out of range");
return;
}
g_step_motor45[motorid].stop();
});
/*******************************************************************************
* ²½½øµç»ú *
*******************************************************************************/
#define GET_MOTOR(motor) \
{ \
int motorid = atoi(argv[1]); \
motor = &g_step_motor[motorid]; \
if (motorid >= 6 || motorid < 1) { \
ZLOGE(TAG, "motor %d not found", motorid); \
context->breakflag = true; \
return; \
} \
}
/**
* @brief STEPMOTOR
*/
cmdScheduler.registerCmd("step_motor_setvelocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(2);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->setVelocity(atoi(argv[2]));
});
cmdScheduler.registerCmd("step_motor_set_acc", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(2);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->setAcc(atoi(argv[2]));
});
cmdScheduler.registerCmd("step_motor_set_dec", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(2);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->setDec(atoi(argv[2]));
});
cmdScheduler.registerCmd("step_motor_moveto", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(2);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->moveTo(atoi(argv[2]));
});
cmdScheduler.registerCmd("step_motor_moveby", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(2);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->moveBy(atoi(argv[2]));
});
cmdScheduler.registerCmd("step_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(2);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->rotate(atoi(argv[2]));
});
cmdScheduler.registerCmd("step_motor_movetozero", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(1);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->moveToZero();
});
cmdScheduler.registerCmd("step_motor_wait_for_idle", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(1);
StepMotor* motor = NULL;
GET_MOTOR(motor);
HAL_Delay(100);
while (!motor->isIdleState()) {
HAL_Delay(300);
ZLOGI(TAG, "step_motor_wait_for_idle %d", atoi(argv[1]));
}
});
cmdScheduler.registerCmd("step_motor_clear_exception", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(1);
StepMotor* motor = NULL;
GET_MOTOR(motor);
motor->clearException();
});
cmdScheduler.registerCmd("step_motor_moveto_mm", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
CHECK_ARGC(2);
StepMotor* motor = NULL;
GET_MOTOR(motor);
int32_t step;
if (!distance_mm_to_step(atoi(argv[1]), atof(argv[2]), &step)) {
ZLOGE(TAG, "step_motor_moveto_mm %d %f failed", atoi(argv[1]), atof(argv[2]));
context->breakflag = true;
return;
}
motor->moveTo(step);
});
}
void Main::run() {
/*******************************************************************************
@ -115,31 +332,44 @@ void Main::run() {
zos_cfg_t zoscfg;
zos_init(&zoscfg);
g_step_motor45_1.initialize(cfg1);
g_step_motor45_2.initialize(cfg2);
g_step_motor45_3.initialize(cfg3);
g_step_motor45_4.initialize(cfg4);
g_step_motor45_5.initialize(cfg5);
auto config = m_IflytopCanMaster.createDefaultConfig(1);
m_IflytopCanMaster.initialize(config);
int i = 1;
g_step_motor[i++].initialize(11, 10000, &m_IflytopCanMaster);
g_step_motor[i++].initialize(12, 10000, &m_IflytopCanMaster);
g_step_motor[i++].initialize(13, 10000, &m_IflytopCanMaster);
g_step_motor[i++].initialize(14, 10000, &m_IflytopCanMaster);
g_step_motor[i++].initialize(15, 10000, &m_IflytopCanMaster);
g_step_motor[i++].initialize(16, 10000, &m_IflytopCanMaster);
// g_step_motor45[0].initialize(cfg1);
g_step_motor45[1].initialize(cfg1);
g_step_motor45[2].initialize(cfg2);
g_step_motor45[3].initialize(cfg3);
g_step_motor45[4].initialize(cfg4);
g_step_motor45[5].initialize(cfg5);
g_step_motor45[6].initialize(cfg6);
StepMotor45Scheduler step_motor45_scheduler;
step_motor45_scheduler.initialize(&htim10, 1000);
step_motor45_scheduler.addMotor(&g_step_motor45_1);
step_motor45_scheduler.addMotor(&g_step_motor45_2);
step_motor45_scheduler.addMotor(&g_step_motor45_3);
step_motor45_scheduler.addMotor(&g_step_motor45_4);
step_motor45_scheduler.addMotor(&g_step_motor45_5);
step_motor45_scheduler.addMotor(&g_step_motor45[1]);
step_motor45_scheduler.addMotor(&g_step_motor45[2]);
step_motor45_scheduler.addMotor(&g_step_motor45[3]);
step_motor45_scheduler.addMotor(&g_step_motor45[4]);
step_motor45_scheduler.addMotor(&g_step_motor45[5]);
step_motor45_scheduler.addMotor(&g_step_motor45[6]);
// g_step_motor45_1.rotate(true, 1000);
step_motor45_scheduler.start();
CmdScheduler cmdScheduler;
cmdScheduler.initialize(&DEBUG_UART, 1000);
cmdScheduler.registerCmd("help", //
[this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); });
regfn();
while (true) {
OSDefaultSchduler::getInstance()->loop();
cmdScheduler.schedule();
m_IflytopCanMaster.periodicJob();
osDelay(1);
}
}
Write Preview
Loading…
Cancel
Save