Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 07:14:22 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

moved USB CDC motor control to task

Browse Source
This commit is contained in:
Nick Knudson
2017-02-28 00:11:53 -08:00
parent d694abe113
commit f3768c1ab5
4 changed files with 61 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotorControl/low_level.c
+46 -2
View File
@@ -126,6 +126,10 @@ Motor_t motors[] = {
};
const int num_motors = sizeof(motors)/sizeof(motors[0]);
//Pending USB buffer
uint8_t pending_usb_buf[64];
osThreadId usb_mc_thread_id;
/* Private constant data -----------------------------------------------------*/
static const float one_by_sqrt3 = 0.57735026919f;
static const float sqrt3_by_2 = 0.86602540378;
@@ -178,6 +182,47 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint) {
printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint);
}
void usb_mc_thread(void const * argument) {
// store threadId
usb_mc_thread_id = osThreadGetId();
// run processing loop
for(;;) {
// wait for USB motor control packets
osSignalWait(M_SIGNAL_USB_MOTOR_CONTROL, osWaitForever);
// check incoming packet type
if (pending_usb_buf[0] == 'p') {
// position control
uint8_t motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
sscanf(pending_usb_buf, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (pending_usb_buf[0] == 'v') {
// velocity control
uint8_t motor_number;
float vel_feed_forward, current_feed_forward;
sscanf(pending_usb_buf, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (pending_usb_buf[0] == 'c') {
// velocity control
uint8_t motor_number;
float current_feed_forward;
sscanf(pending_usb_buf, "c %u %f ", &motor_number, &current_feed_forward);
if (motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
}
// clear the buffer
memset(pending_usb_buf, 0, sizeof(pending_usb_buf));
}
// If we get here, then this task is done
vTaskDelete(usb_mc_thread_id);
}
// Initalises the low level motor control and then starts the motor control threads
void init_motor_control() {
//Init gate drivers
@@ -744,7 +789,7 @@ static void control_motor_loop(Motor_t* motor) {
if (motor->control_mode >= POSITION_CONTROL) {
float pos_err = motor->pos_setpoint - motor->rotor.pll_pos;
vel_des += motor->pos_gain * pos_err;
}
}
float vel_lim = motor->vel_limit;
if (vel_des > vel_lim) vel_des = vel_lim;
if (vel_des < -vel_lim) vel_des = -vel_lim;
@@ -812,4 +857,3 @@ void motor_thread(void const * argument) {
//De-energize motor
queue_voltage_timings(motor, 0.0f, 0.0f);
}
MotorControl/low_level.h
+5 -1
View File
@@ -8,7 +8,8 @@
/* Exported types ------------------------------------------------------------*/
typedef enum {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0,
M_SIGNAL_USB_MOTOR_CONTROL = 1u << 1
} Motor_thread_signals_t;
typedef enum {
@@ -67,6 +68,8 @@ typedef struct {
extern float vbus_voltage;
extern Motor_t motors[];
extern const int num_motors;
extern uint8_t pending_usb_buf[64];
extern osThreadId usb_mc_thread_id;
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
@@ -84,5 +87,6 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc);
//@TODO move motor thread to high level file
void motor_thread(void const * argument);
void usb_mc_thread(void const * argument);
#endif //__LOW_LEVEL_H
Src/freertos.c
+4
View File
@@ -58,6 +58,7 @@ osThreadId defaultTaskHandle;
osThreadDef(task_motor_0, motor_thread, osPriorityHigh+1, 0, 512);
osThreadDef(task_motor_1, motor_thread, osPriorityHigh, 0, 512);
osThreadDef(task_usb_mc, usb_mc_thread, osPriorityNormal, 0, 512);
/* USER CODE END Variables */
@@ -127,6 +128,9 @@ void StartDefaultTask(void const * argument)
osThreadDef(task_usb_cdc, usb_cdc_thread, osPriorityIdle, 0, 256);
osThreadCreate(osThread(task_usb_cdc), NULL);
// Start USB motor control thread
osThreadCreate(osThread(task_usb_mc), NULL);
//If we get to here, then the default task is done.
vTaskDelete(defaultTaskHandle);
Src/usbd_cdc_if.c
+6 -25
View File
@@ -264,31 +264,12 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
/* USER CODE BEGIN 6 */
USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
USBD_CDC_ReceivePacket(&hUsbDeviceFS);
// check incoming packet type
if (Buf[0] == 'p') {
// position control
uint8_t motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
sscanf(Buf, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (Buf[0] == 'v') {
// velocity control
uint8_t motor_number;
float vel_feed_forward, current_feed_forward;
sscanf(Buf, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (Buf[0] == 'c') {
// velocity control
uint8_t motor_number;
float current_feed_forward;
sscanf(Buf, "c %u %f ", &motor_number, &current_feed_forward);
if (motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
// if usb_mc_thread exists
if (usb_mc_thread_id) {
// copy to temporary buffer
memcpy(pending_usb_buf, Buf, *Len);
// alert USB motor control
osSignalSet(usb_mc_thread_id, M_SIGNAL_USB_MOTOR_CONTROL);
}
return (USBD_OK);
/* USER CODE END 6 */