Dummy-Robot/2.Firmware/Core-STM32F4-fw/UserApp/main.cpp
2022-02-09 11:52:03 +08:00

184 lines
5.9 KiB
C++

#include "common_inc.h"
// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c)
SSD1306 oled(&hi2c0);
// On-board Sensor, used hi2c1
MPU6050 mpu6050(&hi2c1);
// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14
Timer timerCtrlLoop(&htim7, 200);
// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs
PWM pwm(21000, 21000);
// Robot instance
DummyRobot dummy(&hcan1);
/* Thread Definitions -----------------------------------------------------*/
osThreadId_t controlLoopFixUpdateHandle;
void ThreadControlLoopFixUpdate(void* argument)
{
for (;;)
{
// Suspended here until got Notification.
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
if (dummy.IsEnabled())
{
// Send control command to Motors & update Joint states
switch (dummy.commandMode)
{
case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
dummy.MoveJoints(dummy.targetJoints);
dummy.UpdateJointPose6D();
break;
case DummyRobot::COMMAND_MOTOR_TUNING:
dummy.tuningHelper.Tick(10);
dummy.UpdateJointPose6D();
break;
}
} else
{
// Just update Joint states
dummy.UpdateJointAngles();
dummy.UpdateJointPose6D();
}
}
}
osThreadId_t ControlLoopUpdateHandle;
void ThreadControlLoopUpdate(void* argument)
{
for (;;)
{
dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever));
}
}
osThreadId_t oledTaskHandle;
void ThreadOledUpdate(void* argument)
{
uint32_t t = micros();
char buf[16];
char cmdModeNames[4][4] = {"SEQ", "INT", "TRJ", "TUN"};
for (;;)
{
mpu6050.Update(true);
oled.clearBuffer();
oled.setFont(u8g2_font_5x8_tr);
oled.setCursor(0, 10);
oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay);
oled.setCursor(85, 10);
oled.printf("| FPS:%lu", 1000000 / (micros() - t));
t = micros();
oled.drawBox(0, 15, 128, 3);
oled.setCursor(0, 30);
oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d",
(int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]),
(int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]),
(int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5]));
oled.drawBox(40, 35, 128, 24);
oled.setFont(u8g2_font_6x12_tr);
oled.setDrawColor(0);
oled.setCursor(42, 45);
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X),
(int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z));
oled.setCursor(42, 56);
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A),
(int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C));
oled.setDrawColor(1);
oled.setCursor(0, 45);
oled.printf("[XYZ]:");
oled.setCursor(0, 56);
oled.printf("[ABC]:");
oled.setFont(u8g2_font_10x20_tr);
oled.setCursor(0, 78);
if (dummy.IsEnabled())
{
for (int i = 1; i <= 6; i++)
buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_');
buf[6] = 0;
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf);
} else
{
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], "======");
}
oled.sendBuffer();
}
}
/* Timer Callbacks -------------------------------------------------------*/
void OnTimer7Callback()
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// Wake & invoke thread IMMEDIATELY.
vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
/* Default Entry -------------------------------------------------------*/
void Main(void)
{
// Init all communication staff, including USB-CDC/VCP/UART/CAN etc.
InitCommunication();
// Init Robot.
dummy.Init();
// Init IMU.
do
{
mpu6050.Init();
osDelay(100);
} while (!mpu6050.testConnection());
mpu6050.InitFilter(200, 100, 50);
// Init OLED 128x80.
oled.Init();
pwm.Start();
// Init & Run User Threads.
const osThreadAttr_t controlLoopTask_attributes = {
.name = "ControlLoopFixUpdateTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityRealtime,
};
controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr,
&controlLoopTask_attributes);
const osThreadAttr_t ControlLoopUpdateTask_attributes = {
.name = "ControlLoopUpdateTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr,
&ControlLoopUpdateTask_attributes);
const osThreadAttr_t oledTask_attributes = {
.name = "OledTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal, // should >= Normal
};
oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes);
// Start Timer Callbacks.
timerCtrlLoop.SetCallback(OnTimer7Callback);
timerCtrlLoop.Start();
// System started, light switch-led up.
Respond(*uart4StreamOutputPtr, "[sys] Heap remain: %d Bytes\n", xPortGetMinimumEverFreeHeapSize());
pwm.SetDuty(PWM::CH_A1, 0.5);
}