Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-02-05 18:01:21 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

auto, makefile, hal fix

Browse Source
This commit is contained in:
crinq
2015-01-11 00:23:30 +01:00
parent b6d4759676
commit 7e95c4a6e6
5 changed files with 56 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@@ -26,7 +26,7 @@ CC=arm-none-eabi-gcc
OBJCOPY=arm-none-eabi-objcopy
#CCDIR = /Users/rene/Downloads/gcc-arm-none-eabi-4_7-2013q3/bin
CFLAGS = -g -Wall -Tstm32_flash.ld -std=gnu99 -fno-builtin -CC
CFLAGS = -g -Wall -Tstm32_flash.ld -std=gnu99 -fno-builtin -CC -fdiagnostics-color=always
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4 -mthumb-interwork -nostartfiles
CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16 -nostartfiles -fsingle-precision-constant
CFLAGS += -ffunction-sections -fdata-sections

40
src/comps/auto.comp
View File

@@ -1,7 +1,7 @@
#include "../hal.h"
COMP(auto);
HAL_PIN(pwm_in) = 0.0;
HAL_PIN(mag_pos_in) = 0.0;
HAL_PIN(pwm_out) = 0.0;
HAL_PIN(mag_pos_out) = 0.0;
@@ -11,30 +11,35 @@ HAL_PIN(ready) = 0.0;
HAL_PIN(offset) = 0.0;
HAL_PIN(fb_in) = 0.0;
HAL_PIN(fb_d_in) = 0.0;
HAL_PIN(pole_count) = 0.0;
HAL_PIN(freq) = 500.0;
HAL_PIN(freq) = 77.0;
HAL_PIN(time) = 0.1;
HAL_PIN(scale) = 0.5;
HAL_PIN(scale) = 0.9;
HAL_PIN(state) = 0.0;
MEM(float mpos) = 0.0;
MEM(float mpos_avg) = 0.0;
MEM(float fb_avg) = 0.0;
MEM(float fb_d_avg) = 0.0;
MEM(float startpos) = 0.0;
MEM(float cov) = 0.0;
MEM(float covs) = 0.0;
MEM(float covc) = 0.0;
MEM(float pwm) = 0.0;
MEM(float offset_) = 0.0;
RT_CALC(
float fb = PIN(fb_in);
float pwm;
float fb_d = PIN(fb_d_in);
HT(
GOTO(0);
STATE(0){
mpos = mod(DEG(180.0) + mpos);
SLEEP(0.5 / PIN(freq));
SLEEP((0.5 / PIN(freq)));
}
);
@@ -42,55 +47,62 @@ RT_CALC(
GOTO(0);
STATE(0){
mpos_avg = mpos_avg * 0.9 + mpos * 0.1;
fb_avg = fb_avg * 0.9 + fb * 0.1;
fb_d_avg = fb_d_avg * 0.9 + fb_d * 0.1;
cov += (mpos - mpos_avg) * (fb - fb_avg);
cov += (mpos - mpos_avg) * (fb_d - fb_d_avg);
}
);
HT(
GOTO(0);
STATE(0){
PIN(state) = 0.0;
pwm = PIN(pwm_in);
mpos = PIN(mag_pos_in);
mpos = mod((fb + offset_) * PIN(pole_count) + DEG(90));
PIN(ready) = 1.0;
if(PIN(start) > 0.0){
PIN(state) = 0.5;
startpos = fb;
mpos = 0.0;
mpos_avg = 0.0;
fb_avg = 0.0;
fb_d_avg = 0.0;
pwm = PIN(scale);
PIN(ready) = 0.0;
GOTO(1);
}
}
STATE(1){
PIN(state) = 1.0;
SLEEP(PIN(time));
PIN(state) = 1.1;
covs = cov;
pwm = 0.0;
SLEEP(PIN(time));
PIN(state) = 1.2;
cov = 0.0;
mpos = DEG(90.0);
mpos_avg = 0.0;
fb_avg = 0.0;
fb_d_avg = 0.0;
pwm = PIN(scale);
SLEEP(PIN(time));
PIN(state) = 1.3;
covc = cov;
pwm = 0.0;
PIN(offset) = minus(mod(startpos * PIN(pole_count)), atan2f(covc, covs)); // TODO polecount, ...
offset_ = minus(mod(startpos * PIN(pole_count)), atan2f(covs, covc)); // TODO polecount ...
PIN(ready) = 1.0;
GOTO(2);
}
STATE(2){
PIN(state) = 2.0;
pwm = PIN(pwm_in);
mpos = PIN(mag_pos_in);
mpos = mod((fb + offset_) * PIN(pole_count) + DEG(90));
if(PIN(start) == 0.0){
GOTO(0);
}
}
);
PIN(offset) = offset_;
PIN(pwm_out) = pwm;
PIN(mag_pos_out) = mpos;
);

12
src/comps/test.comp
View File

@@ -1,12 +1,16 @@
#include "../hal.h"
COMP(test);
HAL_PIN(test0) = 0.0;
HAL_PIN(test1) = 0.0;
HAL_PIN(test2) = 0.0;
HAL_PIN(test0) = 1.0;
HAL_PIN(test1) = 1.0;
HAL_PIN(test2) = 1.0;
HAL_PIN(start) = 0.0;
RT_CALC(
HT(
PIN(test0) = 0.0;
PIN(test1) = 0.0;
PIN(test2) = 0.0;
GOTO(0);
STATE(0){
PIN(test0) = 1.0;
@@ -24,7 +28,7 @@ RT_CALC(
SLEEP(0.5);
PIN(test0) = 0.0;
PIN(test1) = 0.0;
PIN(test2) = 2.0;
PIN(test2) = 1.0;
SLEEP(0.5);
GOTO(0);
}

12
src/hal.h
View File

@@ -181,9 +181,10 @@ int addf_nrt(void (*nrt)(float period));
inline void ht_function(){ \
static float ht_time_count; \
(void) ht_time_count; \
static int jump_label_pointer = -__COUNTER__ - 1; \
static int jump_label_pointer = -__COUNTER__ - 2; \
static int jump_label_pointer_old = 0; \
switch(jump_label_pointer){ \
case -__COUNTER__ - 2:; \
case -__COUNTER__ - 1:; \
ht_code; \
default: \
goto jump_label_ht_end; \
@@ -207,11 +208,14 @@ case (ht_state):
#define SLEEP(time) \
ht_time_count = 0.0; \
jump_label_pointer_old = jump_label_pointer; \
case -__COUNTER__ - 2:; jump_label_pointer = -__COUNTER__ - 1; \
if(ht_time_count < (systime_s)){ \
if(ht_time_count < (time)){ \
ht_time_count += period; \
goto jump_label_ht_end; \
}
} \
jump_label_pointer = jump_label_pointer_old;
#define ENDCOMP \
addf_init(init); \

13
src/main.c
View File

@@ -42,6 +42,7 @@ void link_pid(){
link_hal_pins("net0.fb", "pos_minus0.in1");
link_hal_pins("pos_minus0.out", "pid0.pos_error");
link_hal_pins("net0.fb", "auto0.fb_in");
link_hal_pins("net0.fb_d", "auto0.fb_d_in");
// vel
@@ -259,6 +260,18 @@ int main(void)
link_pid();
link_ac_sync_res();
link_hal_pins("led0.r", "test0.test0");
link_hal_pins("led0.y", "test0.test1");
link_hal_pins("led0.g", "test0.test2");
link_hal_pins("auto0.mag_pos_out", "term0.wave0");
link_hal_pins("auto0.pwm_out", "term0.wave1");
link_hal_pins("net0.fb_d", "term0.wave2");
link_hal_pins("auto0.state", "term0.wave3");
set_hal_pin("term0.gain2", 10.0);
set_hal_pin("term0.gain3", 50.0);
//set_hal_pin("ap0.start", 1.0);
// link_hal_pins("sim0.sin", "net0.cmd");