mirror of
https://github.com/rene-dev/stmbl.git
synced 2024-12-26 02:22:25 +00:00
iclarke, motor turns
This commit is contained in:
parent
19664162ad
commit
9f3d57579f
26
src/comps/iclarke.comp
Normal file
26
src/comps/iclarke.comp
Normal file
@ -0,0 +1,26 @@
|
||||
HAL_COMP(iclarke);
|
||||
|
||||
#define SQRT3 1.732050808
|
||||
|
||||
HAL_PIN(a) = 0;
|
||||
HAL_PIN(b) = 0;
|
||||
|
||||
HAL_PIN(u);
|
||||
HAL_PIN(v);
|
||||
HAL_PIN(w);
|
||||
|
||||
|
||||
RT(
|
||||
float ua = PIN(a);
|
||||
float ub = PIN(b);
|
||||
|
||||
float u = ua; // inverse clarke
|
||||
float v = - ua / 2.0 + ub / 2.0 * SQRT3;
|
||||
float w = - ua / 2.0 - ub / 2.0 * SQRT3;
|
||||
|
||||
PIN(u) = u;
|
||||
PIN(v) = v;
|
||||
PIN(w) = w;
|
||||
);
|
||||
|
||||
ENDCOMP;
|
@ -10,12 +10,15 @@ HAL_PIN(fault) = 0.0;
|
||||
HAL_PIN(led) = 0.0;
|
||||
|
||||
|
||||
RT(
|
||||
htim8.Instance->CCR3 = 4800 - PIN(u);
|
||||
htim8.Instance->CCR2 = 4800 - PIN(v);
|
||||
htim8.Instance->CCR1 = 4800 - PIN(w);
|
||||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, PIN(en) > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
NRT(
|
||||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, PIN(led) > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
);
|
||||
|
||||
RT(
|
||||
htim8.Instance->CCR3 = PIN(u) + 2400;
|
||||
htim8.Instance->CCR2 = PIN(v) + 2400;
|
||||
htim8.Instance->CCR1 = PIN(w) + 2400;
|
||||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, PIN(en) > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
);
|
||||
|
||||
ENDCOMP;
|
||||
|
@ -55,7 +55,11 @@
|
||||
#include <math.h>
|
||||
#include "defines.h"
|
||||
#include "hal.h"
|
||||
#include "hal_term.h"
|
||||
#include "angle.h"
|
||||
#include "scanf.h"
|
||||
#include "usbd_cdc_if.h"
|
||||
|
||||
/* USER CODE END Includes */
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
@ -251,6 +255,8 @@ int main(void)
|
||||
|
||||
#include "../src/comps/sim.comp"
|
||||
#include "../src/comps/term.comp"
|
||||
#include "../src/comps/idq.comp"
|
||||
#include "../src/comps/iclarke.comp"
|
||||
#include "comps/io.comp"
|
||||
|
||||
rt_time_hal_pin = hal_map_pin("net0.rt_calc_time");
|
||||
@ -258,11 +264,30 @@ int main(void)
|
||||
rt_period_time_hal_pin = hal_map_pin("net0.rt_period");
|
||||
frt_period_time_hal_pin = hal_map_pin("net0.frt_period");
|
||||
|
||||
//hal_set_pin("io0.rt_prio", 1.0);
|
||||
hal_set_pin("sim0.rt_prio", 15.0);
|
||||
hal_set_pin("sim0.rt_prio", 1.0);
|
||||
hal_set_pin("idq0.rt_prio", 2.0);
|
||||
hal_set_pin("iclarke0.rt_prio", 3.0);
|
||||
hal_set_pin("io0.rt_prio", 4.0);
|
||||
|
||||
hal_set_pin("term0.rt_prio", 15.0);
|
||||
hal_set_pin("term0.send_step", 0.0);
|
||||
hal_set_pin("term0.send_step", 50.0);
|
||||
hal_set_pin("term0.gain0", 20.0);
|
||||
hal_set_pin("term0.gain1", 20.0);
|
||||
hal_set_pin("term0.gain2", 20.0);
|
||||
hal_set_pin("term0.gain3", 20.0);
|
||||
hal_set_pin("term0.gain4", 20.0);
|
||||
hal_set_pin("term0.gain5", 20.0);
|
||||
hal_set_pin("term0.gain6", 20.0);
|
||||
hal_set_pin("term0.gain7", 20.0);
|
||||
|
||||
hal_link_pins("sim0.vel", "idq0.pos");
|
||||
hal_link_pins("idq0.a", "iclarke0.a");
|
||||
hal_link_pins("idq0.b", "iclarke0.b");
|
||||
hal_link_pins("iclarke0.u", "io0.u");
|
||||
hal_link_pins("iclarke0.v", "io0.v");
|
||||
hal_link_pins("iclarke0.w", "io0.w");
|
||||
|
||||
hal_link_pins("term0.con", "io0.led");
|
||||
|
||||
hal_comp_init();//call init function of all comps
|
||||
|
||||
@ -283,7 +308,7 @@ int main(void)
|
||||
/* USER CODE BEGIN 3 */
|
||||
hal_run_nrt(0.1);
|
||||
cdc_poll();
|
||||
HAL_Delay(2);
|
||||
HAL_Delay(1);
|
||||
}
|
||||
/* USER CODE END 3 */
|
||||
|
||||
|
@ -310,20 +310,28 @@ void cdc_usbtx(){
|
||||
}
|
||||
|
||||
int cdc_tx(void* data, uint32_t len){
|
||||
int ret;
|
||||
ret = rb_write(&tx_buf, data, len);
|
||||
cdc_usbtx();
|
||||
return ret;
|
||||
if(cdc_is_connected()){
|
||||
int ret;
|
||||
ret = rb_write(&tx_buf, data, len);
|
||||
cdc_usbtx();
|
||||
return ret;
|
||||
}else{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void cdc_poll(){
|
||||
cdc_usbtx();
|
||||
if(cdc_is_connected()){
|
||||
cdc_usbtx();
|
||||
}
|
||||
}
|
||||
|
||||
int cdc_is_connected(){
|
||||
//return (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.dev_connection_status; not implemented
|
||||
//hUsbDeviceFS.dev_state != USBD_STATE_CONFIGURED probably this
|
||||
return 1;
|
||||
if((USBD_CDC_HandleTypeDef*)hUsbDeviceFS.dev_state != USBD_STATE_CONFIGURED){
|
||||
return 0;
|
||||
}else{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
int cdc_getline(char *ptr, int len){
|
||||
|
Loading…
Reference in New Issue
Block a user