mirror of https://github.com/rusefi/bldc.git
Added Finns app
This commit is contained in:
Benjamin Vedder
parent
998bc17ba7
commit
89d854f4c4
|
|
@ -0,0 +1,484 @@
|
|||
/*
|
||||
Copyright 2020 - 2021 Benjamin Vedder benjamin@vedder.se
|
||||
|
||||
This file is part of the VESC firmware.
|
||||
|
||||
The VESC firmware is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
The VESC firmware is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "app.h"
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
|
||||
#include "mc_interface.h"
|
||||
#include "utils.h"
|
||||
#include "encoder.h"
|
||||
#include "terminal.h"
|
||||
#include "comm_can.h"
|
||||
#include "hw.h"
|
||||
#include "commands.h"
|
||||
#include "timeout.h"
|
||||
|
||||
#include "app_finn_types.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* HW Connections:
|
||||
*
|
||||
* btn_left: UART_RX and VCC
|
||||
* btn_right: ADC_EXT and VCC
|
||||
* limit_sw: ADC_EXT2 and VCC
|
||||
*
|
||||
* Motor:
|
||||
* blue: A
|
||||
* yellow: B
|
||||
* red: C
|
||||
*
|
||||
* Motor sensor connector should be plugged in.
|
||||
*
|
||||
*/
|
||||
|
||||
// Settings
|
||||
#define FILTER_CONST 0.02 // Range: 0 - 1. Higher values make the control more aggressive.
|
||||
#define BUTTON_RATE 5.0 // How fast the home adjustment buttons move the pod. Unit: Deg/s
|
||||
#define START_DELAY 10.0 // Start delay in seconds
|
||||
|
||||
/**
|
||||
* Homing procedure
|
||||
*
|
||||
* The pod will rotate left, stay there for left_time, then rotate right
|
||||
* until the limit switch is found or until HOMING_ANGLE_MAX is reached. If
|
||||
* HOMING_ANGLE_MAX an error is thrown and the pod will rotate back to the initial
|
||||
* position. If a homing error occurs, no new commands are accepted.
|
||||
*/
|
||||
#define HOMING_RATE 20.0 // Deg/s
|
||||
#define HOMING_ANGLE_BACK -30.0
|
||||
#define HOMING_ANGLE_MAX 270.0
|
||||
#define HOMING_BACK_TIME 3.0 // Seconds
|
||||
|
||||
#define P_ADDR_OFFSET 0
|
||||
|
||||
// Threads
|
||||
static THD_FUNCTION(control_thread, arg);
|
||||
static THD_WORKING_AREA(control_thread_wa, 1024);
|
||||
|
||||
static THD_FUNCTION(status_thread, arg);
|
||||
static THD_WORKING_AREA(status_thread_wa, 1024);
|
||||
static thread_t *status_tp;
|
||||
|
||||
// Private variables
|
||||
static volatile bool stop_now = true;
|
||||
static volatile bool control_is_running = false;
|
||||
static volatile bool status_is_running = false;
|
||||
static volatile POD_STATE m_pod_state;
|
||||
|
||||
// Private functions
|
||||
static bool can_eid_callback(uint32_t id, uint8_t *data, uint8_t len);
|
||||
static void terminal_mon(int argc, const char **argv);
|
||||
static void terminal_home(int argc, const char **argv);
|
||||
|
||||
void app_custom_start(void) {
|
||||
memset((void*)&m_pod_state, 0, sizeof(m_pod_state));
|
||||
m_pod_state.homing_angle_now = HOMING_ANGLE_BACK;
|
||||
|
||||
comm_can_set_eid_rx_callback(can_eid_callback);
|
||||
|
||||
palSetPadMode(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN, PAL_MODE_INPUT_PULLDOWN);
|
||||
palSetPadMode(HW_ADC_EXT2_GPIO, HW_ADC_EXT2_PIN, PAL_MODE_INPUT_PULLDOWN);
|
||||
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT_PULLDOWN);
|
||||
|
||||
terminal_register_command_callback(
|
||||
"sd_mon",
|
||||
"Monitor IO for 30 seconds.",
|
||||
0,
|
||||
terminal_mon);
|
||||
|
||||
terminal_register_command_callback(
|
||||
"sd_home",
|
||||
"Start homing procedure.",
|
||||
0,
|
||||
terminal_home);
|
||||
|
||||
eeprom_var v;
|
||||
if (conf_general_read_eeprom_var_custom(&v, P_ADDR_OFFSET)) {
|
||||
m_pod_state.angle_offset = v.as_float;
|
||||
}
|
||||
|
||||
stop_now = false;
|
||||
chThdCreateStatic(control_thread_wa, sizeof(control_thread_wa),
|
||||
NORMALPRIO, control_thread, NULL);
|
||||
chThdCreateStatic(status_thread_wa, sizeof(status_thread_wa),
|
||||
NORMALPRIO, status_thread, NULL);
|
||||
}
|
||||
|
||||
void app_custom_stop(void) {
|
||||
comm_can_set_eid_rx_callback(0);
|
||||
terminal_unregister_callback(terminal_mon);
|
||||
|
||||
palSetPadMode(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(HW_ADC_EXT2_GPIO, HW_ADC_EXT2_PIN, PAL_MODE_INPUT_ANALOG);
|
||||
|
||||
stop_now = true;
|
||||
while (control_is_running || status_is_running) {
|
||||
chThdSleepMilliseconds(1);
|
||||
}
|
||||
}
|
||||
|
||||
void app_custom_configure(app_configuration *conf) {
|
||||
m_pod_state.pod_id = conf->controller_id;
|
||||
}
|
||||
|
||||
/*
|
||||
* Here we handle CAN-frames from the joystick.
|
||||
*/
|
||||
static bool can_eid_callback(uint32_t id, uint8_t *data, uint8_t len) {
|
||||
bool res = false;
|
||||
|
||||
uint32_t sourceType = (id >> 0) & 0xFF;
|
||||
uint32_t sourceIndex = (id >> 8) & 0xFF;
|
||||
uint32_t packetType = (id >> 16) & 0xFF;
|
||||
uint32_t prio = (id >> 24) & 0x1F;
|
||||
|
||||
// commands_printf("sourceType: %02X sourceIndex: %02X, packetType: %02X, prio: %02X",
|
||||
// sourceType, sourceIndex, packetType, prio);
|
||||
|
||||
(void)sourceType;
|
||||
(void)sourceIndex;
|
||||
(void)prio;
|
||||
|
||||
CAN_PAYLOAD_UNION packet_union;
|
||||
memcpy(packet_union.bytes, data, len);
|
||||
|
||||
switch(packetType)
|
||||
{
|
||||
case CAN_MESSAGE_POD_REQUEST:
|
||||
{
|
||||
CAN_PACKET_POD_REQUEST *pkt = &packet_union.podRequest;
|
||||
uint8_t pktpodid = pkt->pod_id;
|
||||
|
||||
if (m_pod_state.pod_id == pktpodid) {// 0-7
|
||||
int16_t ang_int = pkt->req_angle;
|
||||
|
||||
// Ignore angles outside of range
|
||||
if (ang_int > 5100.0 || ang_int < -5100.0) {
|
||||
res = false;
|
||||
break;
|
||||
}
|
||||
|
||||
m_pod_state.req_angle = (float)ang_int / 5000.0 * 90.0;
|
||||
m_pod_state.last_update = chVTGetSystemTimeX();
|
||||
}
|
||||
|
||||
res = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CAN_MESSAGE_POD_REQ_FOUR:
|
||||
{
|
||||
CAN_PACKET_POD_REQ_FOUR *pkt = &packet_union.podReqFour;
|
||||
|
||||
if (m_pod_state.pod_id < 4) {
|
||||
int16_t ang_int = pkt->req_angle[m_pod_state.pod_id];
|
||||
|
||||
// Ignore angles outside of range
|
||||
if (ang_int > 5100.0 || ang_int < -5100.0) {
|
||||
res = false;
|
||||
break;
|
||||
}
|
||||
|
||||
m_pod_state.req_angle = (float)ang_int / 5000.0 * 90.0;
|
||||
m_pod_state.last_update = chVTGetSystemTimeX();
|
||||
}
|
||||
|
||||
res = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CAN_MESSAGE_POD_REQ_FOUR_HI:
|
||||
{
|
||||
CAN_PACKET_POD_REQ_FOUR *pkt = &packet_union.podReqFour;
|
||||
if (m_pod_state.pod_id >= 4 && m_pod_state.pod_id < 8) {
|
||||
int16_t ang_int = pkt->req_angle[m_pod_state.pod_id-4];
|
||||
|
||||
// Ignore angles outside of range
|
||||
if (ang_int > 5100.0 || ang_int < -5100.0) {
|
||||
res = false;
|
||||
break;
|
||||
}
|
||||
|
||||
m_pod_state.req_angle = (float)ang_int / 5000.0 * 90.0;
|
||||
m_pod_state.last_update = chVTGetSystemTimeX();
|
||||
}
|
||||
|
||||
res = true;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (res) {
|
||||
// Trigger status thread to send update
|
||||
chEvtSignal(status_tp, (eventmask_t) 1);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
static THD_FUNCTION(control_thread, arg) {
|
||||
(void)arg;
|
||||
|
||||
chRegSetThreadName("Finn AZ");
|
||||
|
||||
control_is_running = true;
|
||||
float angle_target = 0.0;
|
||||
systime_t time_last = chVTGetSystemTimeX();
|
||||
|
||||
int btn_left_samples = 0;
|
||||
int btn_right_samples = 0;
|
||||
int btn_lim_samples = 0;
|
||||
|
||||
for(;;) {
|
||||
if (stop_now) {
|
||||
control_is_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (ST2MS(chVTGetSystemTimeX()) < (START_DELAY * 1000)) {
|
||||
chThdSleepMilliseconds(10);
|
||||
time_last = chVTGetSystemTimeX();
|
||||
continue;
|
||||
}
|
||||
|
||||
{
|
||||
static int samp_cnt = 0;
|
||||
samp_cnt++;
|
||||
if (samp_cnt == 5) {
|
||||
samp_cnt = 0;
|
||||
|
||||
// Sample push button slower for some debouncing
|
||||
|
||||
btn_left_samples += palReadPad(HW_UART_RX_PORT, HW_UART_RX_PIN) ? 1 : -1;
|
||||
utils_truncate_number_int(&btn_left_samples, -4, 5);
|
||||
m_pod_state.btn_left_pressed = btn_left_samples > 0;
|
||||
|
||||
btn_right_samples += palReadPad(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN) ? 1 : -1;
|
||||
utils_truncate_number_int(&btn_right_samples, -4, 5);
|
||||
m_pod_state.btn_right_pressed = btn_right_samples > 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Sample limit switch faster to not miss the pulse. Also do some filtering.
|
||||
btn_lim_samples += (!palReadPad(HW_ADC_EXT2_GPIO, HW_ADC_EXT2_PIN)) ? 1 : -1;
|
||||
utils_truncate_number_int(&btn_lim_samples, -4, 5);
|
||||
m_pod_state.btn_limit_pressed = btn_lim_samples > 0;
|
||||
|
||||
float dt = UTILS_AGE_S(time_last);
|
||||
time_last = chVTGetSystemTimeX();
|
||||
|
||||
float angle_target_no_filter = m_pod_state.req_angle + m_pod_state.angle_home + m_pod_state.angle_offset;
|
||||
|
||||
if (!m_pod_state.homing_done) {
|
||||
m_pod_state.homing_back_time += dt;
|
||||
|
||||
if (m_pod_state.homing_back_time >= HOMING_BACK_TIME) {
|
||||
m_pod_state.homing_angle_now += HOMING_RATE * dt;
|
||||
}
|
||||
|
||||
angle_target_no_filter = m_pod_state.homing_angle_now;
|
||||
|
||||
if (m_pod_state.btn_limit_pressed) {
|
||||
m_pod_state.homing_done = true;
|
||||
m_pod_state.angle_home = mc_interface_get_pid_pos_now() * 4.0;
|
||||
}
|
||||
|
||||
if (m_pod_state.homing_angle_now > HOMING_ANGLE_MAX) {
|
||||
m_pod_state.homing_error = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_pod_state.homing_error) {
|
||||
angle_target_no_filter = 0.0;
|
||||
}
|
||||
|
||||
{
|
||||
// Offset update
|
||||
static bool offset_updated = false;
|
||||
systime_t offset_update_time = 0;
|
||||
|
||||
if (m_pod_state.btn_left_pressed) {
|
||||
m_pod_state.angle_offset -= BUTTON_RATE * dt;
|
||||
offset_updated = true;
|
||||
offset_update_time = chVTGetSystemTimeX();
|
||||
}
|
||||
|
||||
if (m_pod_state.btn_right_pressed) {
|
||||
m_pod_state.angle_offset += BUTTON_RATE * dt;
|
||||
offset_updated = true;
|
||||
offset_update_time = chVTGetSystemTimeX();
|
||||
}
|
||||
|
||||
utils_truncate_number_abs((float*)&m_pod_state.angle_offset, 180.0);
|
||||
|
||||
// Store offset update 2s after the last adjustment
|
||||
if (offset_updated && UTILS_AGE_S(offset_update_time) > 2.0) {
|
||||
offset_updated = false;
|
||||
eeprom_var v;
|
||||
v.as_float = m_pod_state.angle_offset;
|
||||
conf_general_store_eeprom_var_custom(&v, P_ADDR_OFFSET);
|
||||
}
|
||||
}
|
||||
|
||||
UTILS_LP_FAST(angle_target, angle_target_no_filter, FILTER_CONST);
|
||||
|
||||
if (UTILS_AGE_S(m_pod_state.last_update) < 2.0) {
|
||||
timeout_reset();
|
||||
mc_interface_set_pid_pos(angle_target / 4.0);
|
||||
m_pod_state.actual_angle = mc_interface_get_pid_pos_now() * 4.0 - m_pod_state.angle_home - m_pod_state.angle_offset;
|
||||
} else {
|
||||
// m_pod_state.req_angle = mc_interface_get_pid_pos_now() * 4.0 - m_pod_state.angle_home - m_pod_state.angle_offset;
|
||||
}
|
||||
|
||||
chThdSleepMilliseconds(5);
|
||||
}
|
||||
}
|
||||
|
||||
static THD_FUNCTION(status_thread, arg) {
|
||||
(void)arg;
|
||||
|
||||
chRegSetThreadName("Finn AZ stat");
|
||||
status_tp = chThdGetSelfX();
|
||||
|
||||
status_is_running = true;
|
||||
|
||||
for(;;) {
|
||||
chEvtWaitAnyTimeout((eventmask_t)1, MS2ST(100));
|
||||
|
||||
if (stop_now) {
|
||||
status_is_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
CAN_PACKET_POD_STATUS podstatus_actual = {0};
|
||||
|
||||
podstatus_actual.pod_id_lo = m_pod_state.pod_id & 3;
|
||||
podstatus_actual.pod_id_hi = m_pod_state.pod_id & 4 ? 1 : 0;
|
||||
podstatus_actual.error = false;
|
||||
podstatus_actual.ready = m_pod_state.homing_done;
|
||||
podstatus_actual.stepdriver_alarm = false;
|
||||
podstatus_actual.calib_running = !m_pod_state.homing_done;
|
||||
podstatus_actual.calib_error = m_pod_state.homing_error;
|
||||
podstatus_actual.config_mode = 0;
|
||||
podstatus_actual.limitswitch = m_pod_state.btn_limit_pressed;
|
||||
podstatus_actual.fake_ready = false;
|
||||
|
||||
podstatus_actual.accepted_angle = (int16_t)(m_pod_state.req_angle / 90.0 * 5000.0);
|
||||
podstatus_actual.actual_angle = (int16_t)(m_pod_state.actual_angle / 90.0 * 5000.0);
|
||||
|
||||
uint32_t sourceType = 0x0B;
|
||||
uint32_t sourceIndex = m_pod_state.pod_id;
|
||||
uint32_t packetType = CAN_MESSAGE_POD_STATUS;
|
||||
uint32_t prio = CAN_HEADER_PRIORITY_LOW | 0x03;
|
||||
|
||||
uint32_t id_ext = 0;
|
||||
id_ext |= (sourceType & 0xFF) << 0;
|
||||
id_ext |= (sourceIndex & 0xFF) << 8;
|
||||
id_ext |= (packetType & 0xFF) << 16;
|
||||
id_ext |= (prio & 0x1F) << 24;
|
||||
|
||||
comm_can_transmit_eid(id_ext, (uint8_t*)&podstatus_actual, 8);
|
||||
}
|
||||
}
|
||||
|
||||
static void terminal_home(int argc, const char **argv) {
|
||||
(void)argc;
|
||||
(void)argv;
|
||||
|
||||
m_pod_state.homing_angle_now = HOMING_ANGLE_BACK + m_pod_state.req_angle + m_pod_state.angle_home + m_pod_state.angle_offset;
|
||||
m_pod_state.homing_back_time = 0.0;
|
||||
m_pod_state.homing_done = false;
|
||||
m_pod_state.homing_error = false;
|
||||
|
||||
commands_printf("OK");
|
||||
}
|
||||
|
||||
static void terminal_mon(int argc, const char **argv) {
|
||||
(void)argc;
|
||||
(void)argv;
|
||||
|
||||
commands_printf("Monitoring IO for 30 seconds");
|
||||
|
||||
float req_ang_last = m_pod_state.req_angle;
|
||||
bool btn_left_last = m_pod_state.btn_left_pressed;
|
||||
bool btn_right_last = m_pod_state.btn_right_pressed;
|
||||
bool btn_limit_last = m_pod_state.btn_limit_pressed;
|
||||
float offset_last = m_pod_state.angle_offset;
|
||||
bool homing_done_last = m_pod_state.homing_done;
|
||||
bool homing_error_last = m_pod_state.homing_error;
|
||||
|
||||
commands_printf("req_ang : %.0f", (double)req_ang_last);
|
||||
commands_printf("SW_LEFT : %d", m_pod_state.btn_left_pressed);
|
||||
commands_printf("SW_RIGHT : %d", m_pod_state.btn_right_pressed);
|
||||
commands_printf("SW_LIMIT : %d", m_pod_state.btn_limit_pressed);
|
||||
commands_printf("Offset : %.1f", (double)m_pod_state.angle_offset);
|
||||
commands_printf("Homing Done : %d", m_pod_state.homing_done);
|
||||
commands_printf("Homing Error : %d", m_pod_state.homing_error);
|
||||
|
||||
for (int i = 0;i < 3000;i++) {
|
||||
if (m_pod_state.req_angle != req_ang_last) {
|
||||
req_ang_last = m_pod_state.req_angle;
|
||||
commands_printf("req_ang : %.0f", (double)req_ang_last);
|
||||
}
|
||||
|
||||
if (m_pod_state.btn_left_pressed != btn_left_last) {
|
||||
btn_left_last = m_pod_state.btn_left_pressed;
|
||||
commands_printf("SW_LEFT : %d", m_pod_state.btn_left_pressed);
|
||||
}
|
||||
|
||||
if (m_pod_state.btn_right_pressed != btn_right_last) {
|
||||
btn_right_last = m_pod_state.btn_right_pressed;
|
||||
commands_printf("SW_RIGHT : %d", m_pod_state.btn_right_pressed);
|
||||
}
|
||||
|
||||
if (m_pod_state.btn_limit_pressed != btn_limit_last) {
|
||||
btn_limit_last = m_pod_state.btn_limit_pressed;
|
||||
commands_printf("SW_LIMIT : %d", m_pod_state.btn_limit_pressed);
|
||||
}
|
||||
|
||||
if (m_pod_state.angle_offset != offset_last) {
|
||||
offset_last = m_pod_state.angle_offset;
|
||||
commands_printf("Offset : %.1f", (double)m_pod_state.angle_offset);
|
||||
}
|
||||
|
||||
if (m_pod_state.homing_done != homing_done_last) {
|
||||
homing_done_last = m_pod_state.homing_done;
|
||||
commands_printf("Homing Done : %d", m_pod_state.homing_done);
|
||||
}
|
||||
|
||||
if (m_pod_state.homing_error != homing_error_last) {
|
||||
homing_error_last = m_pod_state.homing_error;
|
||||
commands_printf("Homing Error : %d", m_pod_state.homing_error);
|
||||
}
|
||||
|
||||
chThdSleepMilliseconds(20);
|
||||
}
|
||||
|
||||
commands_printf("Monitoring IO ended\n");
|
||||
}
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
/*
|
||||
Copyright 2020 Benjamin Vedder benjamin@vedder.se
|
||||
|
||||
This file is part of the VESC firmware.
|
||||
|
||||
The VESC firmware is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
The VESC firmware is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef APP_FINN_AZ_CONF_H_
|
||||
#define APP_FINN_AZ_CONF_H_
|
||||
|
||||
#define APP_CUSTOM_TO_USE "finn/app_finn_az.c"
|
||||
#define APPCONF_APP_TO_USE APP_CUSTOM
|
||||
#define APPCONF_CAN_BAUD_RATE CAN_BAUD_75K
|
||||
#define APPCONF_CONTROLLER_ID 0
|
||||
|
||||
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
|
||||
#define MCCONF_FOC_SENSOR_MODE FOC_SENSOR_MODE_HALL
|
||||
#define MCCONF_M_INVERT_DIRECTION true
|
||||
|
||||
// Limits
|
||||
#define MCCONF_L_RPM_MIN -7000
|
||||
#define MCCONF_L_RPM_MAX 7000
|
||||
#define MCCONF_L_RPM_START 0.3
|
||||
#define MCCONF_L_CURRENT_MAX 20.0
|
||||
#define MCCONF_L_CURRENT_MIN -20.0
|
||||
#define MCCONF_L_IN_CURRENT_MAX 30.0
|
||||
#define MCCONF_L_IN_CURRENT_MIN -10.0
|
||||
|
||||
// Position PID controller
|
||||
#define MCCONF_P_PID_KP (0.2 * 4.0)
|
||||
#define MCCONF_P_PID_KI (0.2 * 4.0)
|
||||
#define MCCONF_P_PID_KD (0.01 * 4.0)
|
||||
#define MCCONF_P_PID_ANG_DIV (75.0 * 7.0 * 4.0) // 1:75 gearing and 7 pole pairs. 4.0 to avoid wrap around.
|
||||
#define MCCONF_P_PID_KD_FILTER 0.2
|
||||
|
||||
// FOC
|
||||
#define MCCONF_FOC_CURRENT_KP 0.027
|
||||
#define MCCONF_FOC_CURRENT_KI 34.0
|
||||
#define MCCONF_FOC_MOTOR_L 26e-6
|
||||
#define MCCONF_FOC_MOTOR_R 34e-3
|
||||
#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 6.495e-3
|
||||
#define MCCONF_FOC_OBSERVER_GAIN 20e6
|
||||
|
||||
// Hall sensors
|
||||
#define MCCONF_FOC_HALL_TAB_0 255
|
||||
#define MCCONF_FOC_HALL_TAB_1 142
|
||||
#define MCCONF_FOC_HALL_TAB_2 8
|
||||
#define MCCONF_FOC_HALL_TAB_3 174
|
||||
#define MCCONF_FOC_HALL_TAB_4 74
|
||||
#define MCCONF_FOC_HALL_TAB_5 108
|
||||
#define MCCONF_FOC_HALL_TAB_6 42
|
||||
#define MCCONF_FOC_HALL_TAB_7 255
|
||||
#define MCCONF_FOC_SL_ERPM 2000.0
|
||||
|
||||
// Other
|
||||
#define MCCONF_FOC_PLL_KP 500.0
|
||||
#define MCCONF_FOC_PLL_KI 5000.0
|
||||
|
||||
#endif /* APP_FINN_AZ_CONF_H_ */
|
||||
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
#ifndef APPLICATIONS_FINN_APP_FINN_TYPES_H_
|
||||
#define APPLICATIONS_FINN_APP_FINN_TYPES_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
|
||||
/** ***************************************************************************
|
||||
\struct CAN_HEADER_UNION
|
||||
\brief CAN message header
|
||||
|
||||
extid is 0 -> 0x1FFFFFFF. last field in struct is first byte
|
||||
first byte bits, mask 0x1F000000 -> prio0 = 0x10, prio1 = 0x08, prio2 = 0x04, res0 = 0x02, res1 = 0x01.
|
||||
second byte is packetType, mask 0x00FF0000.
|
||||
third byte is sourceIndex, mask 0x0000FF00.
|
||||
fourth byte is sourceType, mask 0x000000FF. 0-0xFF;
|
||||
|
||||
extid = prio0, prio1, prio2, res0, res1, packetType, sourceIndex, sourceType.
|
||||
word/extid -> 0x000055AA -> sourcetype=0xAA, sourceIndex=0x55
|
||||
******************************************************************************/
|
||||
|
||||
//typedef union CAN_HEADER_UNION {
|
||||
// struct fields { ///< Use this when accessing data
|
||||
// uint8_t sourceType; ///< Last byte in header (sending device type)
|
||||
// uint8_t sourceIndex; ///< Sometimes from DIP-switches val 0-3
|
||||
// uint8_t packetType; ///< CAN_PACKET_TYPE (ex CAN_MESSAGE_THRUSTER_REQUEST)
|
||||
//
|
||||
// union meta {
|
||||
// struct bits { ///< (bus bits 28-24)
|
||||
// uint8_t reserved1:1; ///< Set reserved bits to 0b11 for normal packets
|
||||
// uint8_t reserved0:1; ///< Set reserved bits to 0b11 for normal packets
|
||||
// uint8_t priority2:1; ///< Bit 2, third bit on the bus (bus bit 26)
|
||||
// uint8_t priority1:1; ///< Bit 3, second bit on the bus (bus bit 27)
|
||||
// uint8_t priority0:1; ///< Bit 4, first bit on the bus (bus bit 28)
|
||||
// uint8_t unused2:1; ///< Bit 5, unused (not on bus)
|
||||
// uint8_t unused1:1; ///< Bit 6, unused (not on bus)
|
||||
// uint8_t unused0:1; ///< Bit 7, MSB, unused (not on bus)
|
||||
// };
|
||||
// uint8_t byte;
|
||||
// } meta;
|
||||
// };
|
||||
//
|
||||
// uint32_t word; ///< Used when stuffing data from the CAN read function
|
||||
// uint8_t bytes[4]; ///< Addressable with CAN_HEADER_BYTES
|
||||
//} CAN_HEADER_UNION;
|
||||
|
||||
/** ***************************************************************************
|
||||
\enum CAN_HEADER_PRIORITIES
|
||||
\brief Packet priority field values
|
||||
\details The priority bits work are read from MSB to LSB:
|
||||
0b111 Lowest priority
|
||||
0b110 Medium Priority
|
||||
0b100 High priority
|
||||
0b000 Extra high priority
|
||||
|
||||
Actual bits used in the meta byte:
|
||||
0x1C = 0b00011100
|
||||
******************************************************************************/
|
||||
|
||||
enum CAN_HEADER_PRIORITIES {
|
||||
CAN_HEADER_PRIORITY_MASK = 0x1C,
|
||||
CAN_HEADER_PRIORITY_LOW = 0x1C,
|
||||
CAN_HEADER_PRIORITY_MID = 0x18,
|
||||
CAN_HEADER_PRIORITY_HIGH = 0x10,
|
||||
CAN_HEADER_PRIORITY_EXTRA = 0x00,
|
||||
};
|
||||
|
||||
typedef struct CAN_PACKET_POD_REQ_FOUR {
|
||||
int16_t req_angle[4];
|
||||
} CAN_PACKET_POD_REQ_FOUR;
|
||||
|
||||
typedef struct __attribute__((packed)) CAN_PACKET_POD_REQUEST {
|
||||
int16_t req_angle;
|
||||
uint8_t pod_id:3;
|
||||
uint8_t use_req_angle:1;
|
||||
|
||||
uint8_t byte4;
|
||||
uint8_t byte5;
|
||||
uint8_t byte6;
|
||||
uint8_t byte7;
|
||||
uint8_t byte8;
|
||||
} CAN_PACKET_POD_REQUEST;
|
||||
|
||||
typedef struct __attribute__((packed)) CAN_PACKET_POD_STATUS {
|
||||
// Byte 1-4
|
||||
int16_t actual_angle; // -5000 to +5000
|
||||
int16_t accepted_angle;
|
||||
|
||||
// Byte 5
|
||||
uint8_t pod_id_lo:2; // pod id low bits
|
||||
uint8_t ready:1;
|
||||
uint8_t error:1;
|
||||
uint8_t stepdriver_alarm:1;
|
||||
uint8_t calib_running:1;
|
||||
uint8_t calib_error:1;
|
||||
uint8_t pod_id_hi:1; // pod id hi bit
|
||||
|
||||
// Byte 6
|
||||
uint8_t config_mode:1;
|
||||
uint8_t limitswitch:1;
|
||||
uint8_t fake_ready:1;
|
||||
|
||||
uint8_t byte7;
|
||||
uint8_t byte8;
|
||||
} CAN_PACKET_POD_STATUS;
|
||||
|
||||
typedef enum {
|
||||
CAN_MESSAGE_POD_REQUEST = 0x54, // Request to POD (use POD_REQ_FOUR normally)
|
||||
CAN_MESSAGE_POD_STATUS = 0x55, // Status from POD (actual angle, ready, errors)
|
||||
CAN_MESSAGE_POD_REQ_FOUR = 0x58, // Request to up to four PODS in same packet 0-3
|
||||
CAN_MESSAGE_POD_REQ_FOUR_HI = 0x63, // Request to up to four PODS in same packet 4-7
|
||||
} CAN_MESSAGE_POD;
|
||||
|
||||
enum TIMING_CAN {
|
||||
TIMING_CAN_SEND_NORMAL = 2000, ///< CAN send interval
|
||||
TIMING_CAN_SEND_UPDATE = 50, ///< CAN send interval when changing status
|
||||
TIMING_CAN_RECEIVE_TIMEOUT = 2500, ///< Timeout for received message, go to idle
|
||||
};
|
||||
|
||||
enum POD_POS_ENUM {
|
||||
POD_POS_MAX = 4,
|
||||
POD_POS_REAR = 0,
|
||||
POD_POS_FRONT = 1,
|
||||
POD_POS_LEFT_REAR = 0,
|
||||
POD_POS_LEFT_FRONT = 1,
|
||||
POD_POS_RIGHT_REAR = 2,
|
||||
POD_POS_RIGHT_FRONT = 3,
|
||||
};
|
||||
|
||||
typedef union {
|
||||
uint8_t bytes[8];
|
||||
// CAN_PACKET_SLEEP_REQUEST sleepRequest;
|
||||
CAN_PACKET_POD_REQ_FOUR podReqFour;
|
||||
CAN_PACKET_POD_REQUEST podRequest;
|
||||
CAN_PACKET_POD_STATUS podStatus;
|
||||
// CAN_PACKET_JOY_POS_RAW joyposraw;
|
||||
} CAN_PAYLOAD_UNION;
|
||||
|
||||
typedef struct {
|
||||
int pod_id;
|
||||
|
||||
int16_t req_angle;
|
||||
int16_t actual_angle;
|
||||
|
||||
float angle_offset;
|
||||
float angle_home;
|
||||
|
||||
bool homing_done;
|
||||
bool homing_error;
|
||||
|
||||
float homing_angle_now;
|
||||
float homing_back_time;
|
||||
|
||||
bool btn_limit_pressed;
|
||||
bool btn_left_pressed;
|
||||
bool btn_right_pressed;
|
||||
systime_t last_update;
|
||||
} POD_STATE;
|
||||
|
||||
#endif /* APPLICATIONS_FINN_APP_FINN_TYPES_H_ */
|
||||
Loading…
Reference in New Issue