Skip to content

GitLab

Commit 3b378c24 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Working after test at corso

parent 8b90f674
Hide whitespace changes
Inline Side-by-side
Showing with 189 additions and 85 deletions
controller/atsamx70/block_atsamx70_gmac.c
View file @ 3b378c24
......@@ -53,7 +53,7 @@ static void atsamx70_gmac_tx_calculate(struct controller_block *packet_tx)
{
struct controller_block_private *priv = packet_tx->private;
((char*)priv->tx_packet)[0]++;
((char*)priv->tx_packet)[11]++;
gmac_tx(&priv->gmac, priv->tx_packet, priv->tx_size);
}
......
controller/atsamx70/block_atsamx70_pwm.c
View file @ 3b378c24
......@@ -49,8 +49,9 @@ static void pwm_calculate(struct controller_block *pwm)
uint32_t dty = abs(duty);
/* Make sure we generate negative pulses for capacitor loading */
// TODO make this a parameter (in seconds off)
if (dty >= period) {
dty--;
dty-=1 + period / 1000;
}
if (duty > 0) {
......
controller/atsamx70/block_atsamx70_tc.c
View file @ 3b378c24
......@@ -25,6 +25,11 @@
#include <controller/controller_time.h>
#include <log/log.h>
enum tc_mode {
TC_MODE_POSITION = 0,
TC_MODE_SPEED = 1,
};
struct controller_block_private {
float position;
......@@ -41,45 +46,81 @@ struct controller_block_private {
uint32_t tcn;
};
#include <stdio.h>
static void tc_calculate(struct controller_block *tc)
{
struct controller_block_private *priv = tc->private;
uint16_t cnt0;
int16_t cnt0p;
int16_t cnt1;
// int16_t cnt1;
uint16_t prevcnt0 = priv->prevcnt0;
float speed;
float position;
int rev = priv->rev;
cnt0 = tc_get_cv(priv->tcn, 0);
cnt1 = tc_get_cv(priv->tcn, 1);
// cnt1 = tc_get_cv(priv->tcn, 1);
uint32_t qisr = tc_get_qisr(priv->tcn);
priv->homed |= qisr & TC_QISR_IDX;
priv->homed &= !(*priv->reset);
cnt0p = cnt0;
// if (cnt0p > rev)
// cnt0p += rev;
int16_t diff = cnt0 - prevcnt0;
if (diff > rev/2)
diff -= rev;
if (diff < -rev/2)
diff += rev;
bool dir = qisr & TC_QISR_DIR;
speed = diff * priv->speedfac;
bool setspeed = true;
cnt0p = cnt0;
if (cnt0p > rev)
cnt0p += rev;
if (dir && cnt0 == 0)
cnt0 = -rev;
int diff;
// int16_t diff = cnt0 - prevcnt0;
if (!dir) {
diff = (int)cnt0 - (int)prevcnt0;
/* positive */
if (diff < 0) {
setspeed = false;
}
} else {
diff = ((int)cnt0 - 65536) - ((int)prevcnt0 - 65536);
if (diff > 0) {
setspeed = false;
}
}
if (setspeed)
priv->speed = diff * priv->speedfac;
// priv->speed = diff;
position = (/*cnt1 * rev + */cnt0p) * priv->posfac;
priv->prevcnt0 = cnt0;
priv->speed = speed;
priv->position = position;
}
static void tc_calculate_speed(struct controller_block *tc)
{
struct controller_block_private *priv = tc->private;
uint16_t cnt0;
uint16_t prevcnt0 = priv->prevcnt0;
float speed;
float position = priv->position;
cnt0 = tc_get_cv(priv->tcn, 0);
int16_t diff = cnt0 - prevcnt0;
speed = diff * priv->speedfac;
position += diff * priv->posfac;
if (*priv->reset)
position = 0;
priv->prevcnt0 = cnt0;
priv->position = position;
priv->speed = speed;
}
static struct controller_block_outterm_list outterms[] = {
{ "position", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position) },
......@@ -100,6 +141,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
int tc_nr;
int rev;
int inverted;
enum tc_mode tc_mode = TC_MODE_POSITION;
tc_nr = va_arg(ap, int);
if (tc_nr < 0 || tc_nr > 3) {
......@@ -109,6 +151,11 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
}
rev = va_arg(ap, int);
inverted = va_arg(ap, int);
if (argc > 3) {
tc_mode = va_arg(ap, int);
}
log_send(LOG_T_DEBUG, "Mode: %d", tc_mode);
if (!(tc = controller_block_alloc("atsamx70_tc", name, sizeof(struct controller_block_private))))
goto err_alloc;
......@@ -119,7 +166,10 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
if (controller_block_interm_list_init(tc, interms))
goto err_interm;
tc->calculate = tc_calculate;
if (tc_mode == TC_MODE_POSITION)
tc->calculate = tc_calculate;
else
tc->calculate = tc_calculate_speed;
uint32_t pa, pb, pi;
uint32_t fa, fb, fi;
......@@ -207,10 +257,13 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
The direction status is reported on TC_QISR.
*/
tc_init(tc->private->tcn, TC_CH0,
TC_CMR_ETRGEDG_RISING | TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0 |
((rev * 4 < 32768) ? TC_CMR_CPCTRG : 0),
0);
uint32_t cmr = TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0;
if (tc_mode == TC_MODE_POSITION) {
cmr |= TC_CMR_ETRGEDG_RISING;
if (rev * 4 < 32768)
cmr |= TC_CMR_CPCTRG;
}
tc_init(tc->private->tcn, TC_CH0, cmr, 0);
tc_write_rc(tc->private->tcn, TC_CH0, rev * 4);
tc_init(tc->private->tcn, TC_CH1,
......@@ -225,7 +278,7 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
tc_start(tc->private->tcn, TC_CH0);
tc_start(tc->private->tcn, TC_CH1);
tc->private->speedfac = (controller_time_frequency_get(tc->time) * M_PI * 2) / (rev * 4);
tc->private->speedfac = (controller_time_frequency_get(tc->time) * M_PI * 2.0) / (double)(rev * 4);
tc->private->posfac = (M_PI * 2) / (rev * 4);
if (controller_block_add(tc))
......@@ -242,5 +295,5 @@ err_alloc:
BLOCK_CREATE(atsamx70_tc) = {
.create = block_atsamx70_tc_create,
.args = { "int,int,int", NULL },
.args = { "int,int,int", "int,int,int,int", NULL },
};
controller/block/block_setpoint_generator_3d.c
View file @ 3b378c24
......@@ -109,6 +109,7 @@ struct controller_block_private {
float freq; /* ticks per second */
float freq2;
float freq3;
float tick;
double t_max_a;
double v_delta_from_max_a;
controller_trigger_time period;
......@@ -141,6 +142,8 @@ struct controller_block_private {
uint32_t id;
bool softreset;
struct controller_command_entry cur_command;
bool cur_done;
bool cur_start;
......@@ -248,21 +251,22 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cmd_x = *priv->reset_x;
cur_x = priv->cmd_x;
priv->cur_done = true;
priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0;
cur_v = 0.0;
priv->cur_a = 0.0;
priv->cur_j = 0.0;
priv->start_x = cur_x;
priv->start_v = 0.0;
priv->start_a = 0.0;
priv->start_j = 0.0;
priv->start_t = 0;
priv->id = COMMAND_ID_NONE;
if (!priv->softreset) {
priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0;
cur_v = 0.0;
priv->cur_a = 0.0;
priv->cur_j = 0.0;
priv->start_x = cur_x;
priv->start_v = 0.0;
priv->start_a = 0.0;
priv->start_j = 0.0;
priv->start_t = 0;
priv->id = COMMAND_ID_NONE;
goto set_output;
}
controller_command_queue_read(priv->command, &entry);
goto set_output;
}
if (*priv->track_x_cmd) {
priv->cur_done = false;
......@@ -297,7 +301,6 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cur_done = true;
break;
case COMMAND_PTYPE_SETPOINT_TRACK:
priv->cmd_v = 0.0;
priv->cur_done = true;
break;
case COMMAND_PTYPE_SPEED:
......@@ -349,13 +352,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cmd_x = cur_x + priv->cmd_v;
}
if (priv->cur_command.type == COMMAND_PTYPE_SPEED_TRACK) {
float track_v = *priv->track_v;
float track_v = *priv->track_v * priv->tick;
ignore_x = true;
priv->cmd_v = track_v;
priv->cmd_x = cur_x + track_v * controller_time_period_get(spg->time);
}
if (priv->cur_command.type == COMMAND_PTYPE_SETPOINT_TRACK) {
priv->cmd_x = *priv->track_x;
priv->cmd_v = 0.0;
}
if (priv->cmd_x > priv->max_x)
......@@ -550,7 +554,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
if (fabs(error_x_at_v) < fabs(error_x) ||
(signbit(error_x) != signbit(error_x_at_v) && !state_to_max_a && !state_at_max_a) ||
(signbit(error_x) != signbit(error_x_at_v) /*&& !state_to_max_a*/ && !state_at_max_a) ||
ignore_x) {
if (!ignore_x &&
signbit(error_x_at_v) == signbit(error_x) ) {
......@@ -712,9 +716,9 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
/* Is the difference between spg and command small enough?
If so, make outputs equal to command */
if (!must_brake &&
fabs(priv->cmd_x - cur_x) < priv->precision_x &&
fabs(priv->cmd_v - cur_v) < priv->precision_v &&
fabs(priv->cur_a) < priv->precision_a) {
fabs(priv->cmd_x - cur_x) <= priv->precision_x &&
fabs(priv->cmd_v - cur_v) <= priv->precision_v &&
fabs(priv->cur_a) <= priv->precision_a) {
priv->cur_j = 0.0;
priv->cur_a = 0.0;
cur_v = priv->cmd_v;
......@@ -857,6 +861,7 @@ static void scale(struct controller_block *spg)
spg->private->precision_v = spg->private->precision_v_sec * tick;
spg->private->precision_a = spg->private->precision_a_sec * tick * tick;
spg->private->tick = tick;
spg->private->freq = 1.0 / tick;
spg->private->freq2 = spg->private->freq * spg->private->freq;
spg->private->freq3 = spg->private->freq2 * spg->private->freq;
......@@ -952,6 +957,14 @@ static int param_set_precision_a(struct controller_block *spg, char *param,
return 0;
}
static int param_set_softreset(struct controller_block *spg, char *param,
int argc, va_list val)
{
spg->private->softreset = va_arg(val, int);
return 0;
}
static struct controller_block_param_list params[] = {
{ "setpoint", true, param_set_setpoint, .args = { "double", NULL } },
{ "max_x", true, param_set_max_x, .args = { "double", NULL } },
......@@ -962,6 +975,7 @@ static struct controller_block_param_list params[] = {
{ "precision_x", true, param_set_precision_x, .args = { "double", NULL } },
{ "precision_v", true, param_set_precision_v, .args = { "double", NULL } },
{ "precision_a", true, param_set_precision_a, .args = { "double", NULL } },
{ "softreset", false, param_set_softreset, .args = { "int", NULL } },
{ NULL },
};
......@@ -1001,6 +1015,7 @@ static struct controller_block * block_setpoint_generator_3d_create(char *name,
if (!spg)
return NULL;
spg->private->softreset = false;
spg->private->cmd_x = 0.0;
spg->private->cmd_v = 0.0;
spg->private->max_x = 0.0;
......
controller/ctrl_embedded.ctrl
View file @ 3b378c24
......@@ -20,7 +20,7 @@ blocks ($(frequency), $(delay)) {
#{ "controller_profile", "profile" }
{ "atsamx70_tc", "counter0", 0, 2500, 0 }
{ "atsamx70_tc", "counter1", 1, 10000, 0 }
{ "atsamx70_tc", "counter1", 1, 10000, 0, 1 }
{ "atsamx70_afec", "afec0", 0 }
{ "atsamx70_pwm", "pwm0", 0, 0, 10000.0 }
......@@ -37,17 +37,21 @@ blocks ($(frequency), $(delay)) {
{ "gain", "w2V" }
{ "gain", "V2pwm" }
{ "deadzone", "joystick_deadzone" }
{ "gain", "joystick2w" }
{ "gain", "joystick2v" }
{ "gain", "joystick2p" }
{ "motor_model_dc_v", "motor_model" }
{ "i2t", "motor_i2t" }
{ "gain", "Ilim2Vlim" }
{ "limit", "Vlim_supply" }
{ "limit_dyn", "Vlim" }
{ "not", "button0n" }
{ "not", "button1n" }
{ "not", "button_v" }
{ "not", "button_x" }
{ "add", "position_offset" }
{ "and2", "track_x_cmd" }
{ "delay", "spg_reset" }
{ "not", "spg_reset" }
{ "delay_bool", "spg_enable" }
{ "setpoint_generator_3d", "spg", "spg", "rad" }
{ "subtract", "error" }
{ "deadzone", "error_deadzone" }
......@@ -55,27 +59,31 @@ blocks ($(frequency), $(delay)) {
{ "add", "speed_ff" }
{ "value_bool", "false" }
{ "value_float", "zero" }
{ "value", "position_calibrate" }
{ "atsamx70_gmac", "packet" }
{ "packet_out_float_be", "out_position", "packet", 0 }
{ "packet_out_float_be", "out_stick", "packet", 4 }
{ "packet_out_float_be", "out_speed", "packet", 8 }
{ "packet_out_bool_byte", "out_homed", "packet", 9 }
{ "packet_out_float_be", "out_speedff", "packet", 12 }
{ "packet_out_float_be", "out_positionff", "packet", 16 }
{ "packet_out_float_be", "out_pid", "packet", 20 }
{ "packet_out_float_be", "out_motor_I", "packet", 24 }
{ "packet_out_bool_byte", "out_homed", "packet", 28 }
}
alias {
{ "position", "counter0", "position" }
{ "position", "position_offset", "out" }
{ "motor_speed", "counter1", "speed" }
{ "error", "error", "difference" }
{ "setpoint", "spg", "x" }
}
set w_nom 1800.0
set w_nom rpm2rads(1800.0)
set V_nom 24.0
set I_nom 28.0
set I_peak 95.0
set t_peak 0.5 #???
set t_peak 0.1 #???
set R_peak $(V_nom)/$(I_peak)
set V_supply 24.0 #27.6?
......@@ -83,44 +91,49 @@ set V_supply 24.0 #27.6?
set gear_ratio 100.0 * 19.33
set max_speed $(w_nom)/$(gear_ratio)
set max_accel $(max_speed)
set max_jerk $(max_jerk)
set max_accel $(max_speed)*2.0
set max_jerk $(max_accel)*2.0
links {
{ "button0", "value", "button0n", "input", true }
{ "button1", "value", "button1n", "input", true }
{ "button0", "value", "button_v", "input", true }
{ "button1", "value", "button_x", "input", true }
{ "button1n", "output", "track_x_cmd", "a", true }
{ "button_x", "output", "track_x_cmd", "a", true }
{ "counter0", "homed", "track_x_cmd", "b", true }
{ "counter0", "position", "position_offset", "in0", true }
{ "position_calibrate", "value", "position_offset", "in1", true }
{ "false", "value", "counter0", "reset", true }
{ "false", "value", "counter1", "reset", true }
{ "afec0", "value", "joystick_deadzone", "in", true }
{ "joystick_deadzone", "out", "joystick2w", "in", true }
{ "joystick_deadzone", "out", "joystick2v", "in", true }
{ "joystick_deadzone", "out", "joystick2p", "in", true }
{ $<position>, "spg", "reset_x", true }
{ "zero", "value", "spg", "track_x", true }
{ "joystick2w", "out", "spg", "track_v", true }
{ "joystick2v", "out", "spg", "track_v", true }
{ "joystick2p", "out", "spg", "track_x", true }
{ "track_x_cmd", "q", "spg", "track_x_cmd", true }
{ "button0n", "output", "spg", "track_v_cmd", true }
{ "counter0", "homed", "spg_reset", "in", true }
{ "spg_reset", "out", "spg", "reset", true }
{ "button_v", "output", "spg", "track_v_cmd", true }
{ "spg_enable", "out", "spg_reset", "input", true }
{ "spg_reset", "output", "spg", "reset", true }
{ "counter0", "homed", "spg_enable", "in", true }
{ "spg", "v", "speed_ff", "in1", true }
{ $<setpoint>, "error", "positive", true }
{ $<position>, "error", "negative", true }
{ $<error>, "error_deadzone", "in", true }
{ "error_deadzone", "out", "pid", "in", true }
{ "spg_enable", "out", "pid", "enable", true }
{ "pid", "out", "speed_ff", "in0", true }
{ "speed_ff", "out", "w2V", "in", true }
{ "w2V", "out", "motor_model", "V", false }
{ $<motor_peed>, "motor_model", "w", true }
{ "Vlim", "out", "motor_model", "V", false }
{ $<motor_speed>, "motor_model", "w", true }
{ "motor_model", "I", "motor_i2t", "I", true }
{ "motor_i2t", "Ilim", "Ilim2Vlim", "in", true }
{ "Ilim2Vlim", "out", "Vlim", "limit", true }
{ "Ilim2Vlim", "out", "Vlim_supply", "in", true }
{ "Vlim_supply", "out", "Vlim", "limit", true }
{ "w2V", "out", "Vlim", "in", true }
{ "Vlim", "out", "V2pwm", "in", true }
......@@ -135,27 +148,35 @@ links {
{ "blinky", "output", "blinky_LED", "value", true }
{ "blinky", "output", "status0_LED", "value", true }
{ "button0", "value", "status3_LED", "value", true }
{ "button1", "value", "status4_LED", "value", true }
{ "button_v", "output", "status3_LED", "value", true }
{ "button_x", "output", "status4_LED", "value", true }
{ "button2", "value", "status5_LED", "value", true }
# Packet output
{ $<position>, "out_position", "value", true }
{ "afec0", "value", "out_stick", "value", true }
{ $<motor_speed>, "out_speed", "value", true }
{ "counter0", "homed", "out_homed", "value", true }
{ $<position>, "out_position", "value", true }
{ "joystick_deadzone", "out", "out_stick", "value", true }
{ $<motor_speed>, "out_speed", "value", true }
{ "counter0", "homed", "out_homed", "value", true }
{ "spg", "v", "out_speedff", "value", true }
{ "spg", "x", "out_positionff", "value", true }
{ "pid", "out", "out_pid", "value", true }
{ "motor_model", "I", "out_motor_I", "value", true }
}
params {
{ "w2V", "gain", (float)$(gear_ratio)*$(V_nom)/$(w_nom) }
{ "V2pwm", "gain", (float)1.0/$(V_supply) }
{ "joystick2w", "gain", (float)$(max_speed) }
{ "joystick2v", "gain", (float)$(max_speed) }
{ "joystick2p", "gain", (float)deg2rad(45.0) }
{ "position_calibrate", "value", 0.0 }
# GNM8055/4
# 24V 28A 490W
# zul. Spitzenstrom 95A
# S3 25% I.CI F
#
# 24/95 --> R=0.25263158 (max R)
# 672W - 490W = 182W --> R=0.23214286 (6.5V) (17.5V emf)
# 490W @1800rpm --> T = 2.6Nm
......@@ -168,22 +189,28 @@ params {
{ "motor_i2t", "peak", (float)$(I_peak) }
{ "motor_i2t", "duration", (float)$(t_peak) }
{ "Vlim_supply", "min", (float)-$(V_supply) }
{ "Vlim_supply", "max", (float) $(V_supply) }
{ "Ilim2Vlim", "gain", (float)$(R_peak) }
{ "joystick_deadzone", "deadzone", (float)0.1 }
{ "joystick_deadzone", "intercept", (float)1.0 }
{ "spg", "max_x", (float)deg2rad( 45.0) }
{ "spg", "min_x", (float)deg2rad(-30.0) }
{ "spg", "max_x", (float)deg2rad( 43.0) }
{ "spg", "min_x", (float)deg2rad(-28.0) }
{ "spg", "max_v", (float)$(max_speed) }
{ "spg", "max_a", (float)$(max_accel) }
{ "spg", "max_j", (float)$(max_jerk) }
{ "spg", "softreset", true }
{ "spg", "precision_x", (float)deg2rad(0.1) }
{ "spg", "precision_v", 0.0005 }
{ "spg", "precision_a", $(max_jerk) }
{ "error_deadzone", "deadzone", (float)deg2rad(1.0) }
{ "error_deadzone", "intercepts", (float)deg2rad(45.0) }
{ "error_deadzone", "deadzone", (float)deg2rad(0.1) }
# { "error_deadzone", "intercept", (float)deg2rad(45.0) }
{ "pid", "kp", (float)$(max_speed) }
{ "pid", "ki", (float)$(max_speed)/10.0 }
{ "pid", "kp", (float)$(max_speed) * 20.0 }
{ "pid", "ki", (float)$(max_speed) * 0.0 }
{ "pid", "kd", (float)0.0 }
{ "pid", "maxw", (float)$(max_speed)/10.0 }
{ "pid", "minw", (float)$(max_speed)/-10.0 }
......
controller/dt_ctrl.c
View file @ 3b378c24
......@@ -52,7 +52,10 @@
extern char _binary_controller_ctrl_embedded_ctrl_start;
extern char _binary_controller_ctrl_embedded_ctrl_end;
#endif
#ifdef HAVE_SYSTICK
#include <samx70.h>
#include <wdt.h>
#endif
int main(int argc, char **argv)
{
......@@ -93,11 +96,13 @@ int main(int argc, char **argv)
}
#else
log_send(LOG_T_DEBUG, "Going to load embedded controller file");
controller_load_mem("ctrl_embedded",
if (controller_load_mem("ctrl_embedded",
&_binary_controller_ctrl_embedded_ctrl_start,
&_binary_controller_ctrl_embedded_ctrl_end - &_binary_controller_ctrl_embedded_ctrl_start);
&_binary_controller_ctrl_embedded_ctrl_end - &_binary_controller_ctrl_embedded_ctrl_start))
goto err_init;
controller_block_link();
if (controller_block_link())
goto err_init;
#endif
if (controller_time_process() != 0) {
......@@ -183,6 +188,9 @@ int main(int argc, char **argv)
controller_bus_recover();
}
#ifdef HAVE_SYSTICK
wdt_restart();
#endif
sleep(1);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment