Skip to content

GitLab

Commit 0e311aef authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

After test at corso

parent 0e9189bc
Show whitespace changes
Inline Side-by-side
README
View file @ 0e311aef
......@@ -21,3 +21,5 @@ Building the software
Just run "make". To see more targets, run "make help". You may also run make in
the individual directories.
./configure --host arm-none-eabihf CFLAGS=-specs /usr/local/arm-none-eabihf/lib/atsamx70x21_flash.specs --disable-am335x --enable-embedded --disable-test --disable-ethercat --disable-vesp
buildflags.mk.in
View file @ 0e311aef
......@@ -21,5 +21,5 @@ BUILD_FILEIO=@BUILD_FILEIO@
BUILD_EMBEDDED=@BUILD_EMBEDDED@
BUILD_SYSTICK=@BUILD_SYSTICK@
BUILD_CFLAGS=@CFLAGS@ @CFLAGS_PTHREAD@ @CFLAGS_TCP@ @CFLAGS_FILEIO@ @CFLAGS_EMBEDDED@ @CFLAGS_SYSTICK@
BUILD_CFLAGS=@CFLAGS@ @CFLAGS_PTHREAD@ @CFLAGS_TCP@ @CFLAGS_SEMAPHORE@ @CFLAGS_FILEIO@ @CFLAGS_EMBEDDED@ @CFLAGS_SYSTICK@
common/command/command.h
View file @ 0e311aef
......@@ -36,6 +36,7 @@ enum command_ptype {
COMMAND_PTYPE_SPEED = 5, /* Simple direct 1st derivative setpoint */
COMMAND_PTYPE_SETPOINT_TIME = 6, /* Setpoint to be reached @ time */
COMMAND_PTYPE_SETPOINT_TRACK = 7, /* Use setpoint from track input */
COMMAND_PTYPE_SPEED_TRACK = 8, /* Use 1st derivative from track input */
COMMAND_PTYPE_MAX, /* Always the last entry! */
};
......
configure.ac
View file @ 0e311aef
......@@ -193,8 +193,8 @@ AS_IF([test "$HAVE_TCP" = "1"],
[AC_SUBST(BUILD_TCP,[""])])
AS_IF([test "$HAVE_SEMAPHORE" = "1"],
[AC_SUBST(CFLAGS_TCP,["-DHAVE_SEMAPHORE"])],
[AC_SUBST(CFLAGS_TCP,[""])])
[AC_SUBST(CFLAGS_SEMAPHORE,["-DHAVE_SEMAPHORE"])],
[AC_SUBST(CFLAGS_SEMAPHORE,[""])])
AS_IF([test "$HAVE_HEADER_LINUX_JOYSTICK" = "1"],
[AC_SUBST(BUILD_LINUX_JOYSTICK,["yes"])],
......
controller/atsamx70/block_atsamx70_gmac.c 0 → 100644
View file @ 0e311aef
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2018
This program 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.
This program 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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <controller/controller_block.h>
#include <controller/controller_bus.h>
#include <controller/controller_mem.h>
#include <controller/controller_sample.h>
#include <log/log.h>
#include <packet/packet.h>
#include <packet/packet_bus.h>
#include <samx70.h>
#include <pio/samx70.h>
#include <ioport.h>
#include <gmac.h>
#define TX_Q_SIZE 4
struct controller_block_private {
struct controller_bus *bus;
void *rx_packet;
void *tx_packet;
size_t rx_size;
size_t tx_size;
struct gmac_state gmac;
struct gmac_tx_buffer_descriptor tx_q[TX_Q_SIZE];
char tx_buffer[TX_Q_SIZE][1600];
};
static void atsamx70_gmac_tx_calculate(struct controller_block *packet_tx)
{
struct controller_block_private *priv = packet_tx->private;
((char*)priv->tx_packet)[0]++;
gmac_tx(&priv->gmac, priv->tx_packet, priv->tx_size);
}
static void packet_sample_start(void *arg)
{
struct controller_block *packet = arg;
struct controller_bus *bus = packet->private->bus;
size_t header_size = 14;
packet->private->tx_size = bus->private->tx_end_offset + header_size;
packet->private->rx_size = bus->private->rx_end_offset + header_size;
packet->private->tx_packet = controller_mem_calloc(
CONTROLLER_MEM_PERIODIC_READ | CONTROLLER_MEM_PERIODIC_WRITE,
packet->private->tx_size, 1);
packet->private->rx_packet = controller_mem_calloc(
CONTROLLER_MEM_PERIODIC_READ | CONTROLLER_MEM_PERIODIC_WRITE,
packet->private->rx_size, 1);
char *tx_start = packet->private->tx_packet;
char *rx_start = packet->private->rx_packet;
memcpy(tx_start + 0, (char[]){0xff, 0xff, 0xff, 0xff, 0xff, 0xff}, 6);
memcpy(tx_start + 6, (char[]){0x01, 0x02, 0x03, 0x04, 0x05, 0x06}, 6);
tx_start[12] = 0x44;
tx_start[13] = 0x4d;
tx_start += header_size;
rx_start += header_size;
packet_client_callbacks(bus, rx_start, tx_start);
}
static enum controller_bus_state atsamx70_gmac_poll(struct controller_bus *bus)
{
struct gmac_state *gmac = &bus->owner->private->gmac;
uint32_t nsr = gmac_network_status_get(gmac);
uint32_t tsr = gmac_transmit_status_get(gmac);
uint32_t isr = gmac_interrupt_status_get(gmac);
log_send(LOG_T_DEBUG, "%s: NSR: 0x%08"PRIx32" TSR: 0x%08"PRIx32" ISR: 0x%08"PRIx32,
bus->owner->name, nsr, tsr, isr);
if (tsr) {
gmac_transmit_status_set(gmac, tsr);
}
return CONTROLLER_BUS_STATE_OK;
}
static int atsamx70_gmac_recover(struct controller_bus *bus)
{
return 0;
}
static struct controller_block * block_atsamx70_gmac_create(char *name, int argc,
va_list val)
{
struct controller_block *packet;
struct controller_bus *bus;
char *name_tx;
struct controller_block *packet_tx;
asprintf(&name_tx, "%s_tx", name);
if (!name_tx)
return NULL;
packet = controller_block_alloc("atsamx70_gmac", name, sizeof(struct controller_block_private));
if (!packet)
goto err_block;
packet_tx = controller_block_alloc("atsamx70_gmac_tx", name_tx, 0);
if (!packet)
goto err_block_tx;
packet_tx->private = packet->private;
free(name_tx);
name_tx = NULL;
bus = packet_bus_create(name, packet);
if (!bus)
goto err_bus;
controller_bus_output_set(bus, packet_tx);
controller_bus_input_set(bus, packet);
controller_bus_poll_set(bus, atsamx70_gmac_poll, atsamx70_gmac_recover);
packet->private->bus = bus;
packet_tx->calculate = atsamx70_gmac_tx_calculate;
if (controller_block_add(packet))
goto err_add;
if (controller_block_add(packet_tx))
goto err_add;
controller_sample_start_hook(packet_sample_start, packet);
/* RMII pins:
GTXCK PD0,A
GTXEN PD1,A
GTX0 PD2,A
GTX1 PD3,A
GRXDV PD4,A
GRX0 PD5,A
GRX1 PD6,A
GRXER PD7,A
GMDC PD8,A
GMDIO PD9,A
same70 xplained:
RESET PC10
INT PA14
*/
ioport_disable_pin(PIO_GTXCK_IDX);
ioport_set_pin_mode(PIO_GTXCK_IDX, IOPORT_MODE_GTXCK);
ioport_disable_pin(PIO_GTXEN_IDX);
ioport_set_pin_mode(PIO_GTXEN_IDX, IOPORT_MODE_GTXEN);
ioport_disable_pin(PIO_GTX0_IDX);
ioport_set_pin_mode(PIO_GTX0_IDX, IOPORT_MODE_GTX0);
ioport_disable_pin(PIO_GTX1_IDX);
ioport_set_pin_mode(PIO_GTX1_IDX, IOPORT_MODE_GTX1);
ioport_disable_pin(PIO_GRXDV_IDX);
ioport_set_pin_mode(PIO_GRXDV_IDX, IOPORT_MODE_GRX0);
ioport_disable_pin(PIO_GRX0_IDX);
ioport_set_pin_mode(PIO_GRX1_IDX, IOPORT_MODE_GRX1);
ioport_disable_pin(PIO_GRXER_IDX);
ioport_set_pin_mode(PIO_GRXER_IDX, IOPORT_MODE_GRXER);
ioport_disable_pin(PIO_GMDC_IDX);
ioport_set_pin_mode(PIO_GMDC_IDX, IOPORT_MODE_GMDC);
ioport_disable_pin(PIO_GMDIO_IDX);
ioport_set_pin_mode(PIO_GMDIO_IDX, IOPORT_MODE_GMDIO);
struct gmac_state *gmac = &packet->private->gmac;
log_send(LOG_T_DEBUG, "gmac: %p, tx_q: %p", gmac, packet->private->tx_q);
int i;
for (i = 0; i < TX_Q_SIZE; i++) {
packet->private->tx_q[i].buffer = packet->private->tx_buffer[i];
}
gmac_init(gmac, packet->private->tx_q, TX_Q_SIZE);
gmac_network_configuration_set(gmac,
GMAC_NCFGR_SPD | GMAC_NCFGR_FD | GMAC_NCFGR_DBW(0) |
GMAC_NCFGR_CLK_MCK_64 | GMAC_NCFGR_MAXFS | GMAC_NCFGR_PEN |
GMAC_NCFGR_RFCS);
return packet;
err_add:
err_bus:
err_block_tx:
controller_block_free(packet);
err_block:
free(name_tx);
return NULL;
}
BLOCK_CREATE(atsamx70_gmac) = {
.create = block_atsamx70_gmac_create,
.args = { NULL },
};
controller/atsamx70/block_atsamx70_pio_in.c
View file @ 0e311aef
......@@ -90,7 +90,9 @@ static struct controller_block * block_atsamx70_pio_in_create(char *name, int ar
ioport_init();
ioport_enable_pin(pio->private->pin);
ioport_set_pin_mode(pio->private->pin, IOPORT_MODE_PULLUP | IOPORT_MODE_GLITCH_FILTER);
ioport_set_pin_dir(pio->private->pin, IOPORT_DIR_INPUT);
ioport_set_pin_sense_mode(pio->private->pin, IOPORT_SENSE_BOTHEDGES);
if (controller_block_add(pio))
goto err_add;
......
controller/atsamx70/block_atsamx70_pio_out.c
View file @ 0e311aef
......@@ -92,6 +92,7 @@ static struct controller_block * block_atsamx70_pio_out_create(char *name, int a
ioport_enable_pin(pio->private->pin);
ioport_set_pin_dir(pio->private->pin, IOPORT_DIR_OUTPUT);
ioport_set_pin_mode(pio->private->pin, 0);
if (controller_block_add(pio))
goto err_add;
......
controller/atsamx70/block_atsamx70_tc.c
View file @ 0e311aef
......@@ -41,10 +41,12 @@ 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;
uint16_t prevcnt0 = priv->prevcnt0;
float speed;
......@@ -59,17 +61,19 @@ static void tc_calculate(struct controller_block *tc)
priv->homed |= qisr & TC_QISR_IDX;
priv->homed &= !(*priv->reset);
if (cnt0 > rev)
cnt0 += rev;
cnt0p = cnt0;
// if (cnt0p > rev)
// cnt0p += rev;
int16_t diff = cnt0 - prevcnt0;
if (diff > rev/2)
diff -= rev;
if (diff < -rev/2)
diff += rev;
speed = diff * priv->speedfac;
position = (cnt1 * rev + cnt0) * priv->posfac;
position = (/*cnt1 * rev + */cnt0p) * priv->posfac;
priv->prevcnt0 = cnt0;
priv->speed = speed;
......@@ -204,7 +208,8 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
*/
tc_init(tc->private->tcn, TC_CH0,
TC_CMR_ETRGEDG_RISING | TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0 | TC_CMR_CPCTRG,
TC_CMR_ETRGEDG_RISING | TC_CMR_ABETRG_A | TC_CMR_TCCLKS_XC0 |
((rev * 4 < 32768) ? TC_CMR_CPCTRG : 0),
0);
tc_write_rc(tc->private->tcn, TC_CH0, rev * 4);
......@@ -220,7 +225,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_period_get(tc->time) * M_PI * 2) * (rev * 4);
tc->private->speedfac = (controller_time_frequency_get(tc->time) * M_PI * 2) / (rev * 4);
tc->private->posfac = (M_PI * 2) / (rev * 4);
if (controller_block_add(tc))
......
controller/atsamx70/build.mk
View file @ 0e311aef
......@@ -3,6 +3,7 @@ ATSAMX70_TARGETS := $(LIBDIR)/libatsamx70.la
ATSAMX70_BLOCKS := \
atsamx70_afec \
atsamx70_gmac \
atsamx70_pio_in \
atsamx70_pio_out \
atsamx70_pwm \
......
controller/block/block_gain_ratio_abs.c
View file @ 0e311aef
......@@ -47,8 +47,16 @@ struct controller_block_private {
static void gain_ra_calculate(struct controller_block *gain)
{
struct controller_block_private *priv = gain->private;
float num = *priv->num;
float denom = *priv->denom;
float in = *priv->in;
float out;
priv->out = fabs((*priv->in * *priv->num)/ *priv->denom);
out = fabs((in * num)/ denom);
if (isnan(out)) {
out = 1.0;
}
priv->out = out;
}
static struct controller_block_interm_list interms[] = {
......
controller/block/block_i2t.c
View file @ 0e311aef
......@@ -31,6 +31,8 @@
----------------------
| |
---| 0 I 0 Ilim |----
| |
| 1 limiting |----
| |
----------------------
......@@ -39,6 +41,7 @@
struct controller_block_private {
float *I;
float Ilim;
bool limiting;
float i2t;
......@@ -64,10 +67,13 @@ static void i2t_calculate(struct controller_block *block)
if (i2t < 0.0)
i2t = 0.0;
if (i2t >= peak2t)
if (i2t >= peak2t) {
priv->Ilim = priv->continuous;
else
priv->limiting = true;
} else {
priv->limiting = false;
priv->Ilim = sqrtf(peak2t - i2t + continuous2);
}
priv->i2t = i2t;
}
......@@ -129,6 +135,7 @@ static struct controller_block_interm_list interms[] = {
static struct controller_block_outterm_list outterms[] = {
{ "Ilim", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Ilim) },
{ "limiting", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, limiting) },
{ NULL }
};
......
controller/block/block_motor_model_dc_v.c
View file @ 0e311aef
......@@ -63,17 +63,19 @@ static void mmdcv_calculate(struct controller_block *mmdcv)
struct controller_block_private *priv = mmdcv->private;
float V = *priv->V;
float w = *priv->w;
float I, Eb, T;
float I, Eb, Er, T;
float kt = priv->kt;
float ke = priv->ke;
float iR = priv->iR;
Eb = V - w * ke;
I = (Eb) * iR;
Eb = w * ke;
Er = V - Eb;
I = (Er) * iR;
T = I * kt;
priv->I = I;
priv->T = T;
priv->Eb = Eb;
}
static int param_set_kt(struct controller_block *mmdcv, char *param, int argc,
......
controller/block/block_motor_model_dc_v.test.ctrl
View file @ 0e311aef
......@@ -9,6 +9,7 @@ blocks (1000.0, 0.0) {
{ "test_input_float", "test_w" }
{ "test_output_float", "test_I" }
{ "test_output_float", "test_T" }
{ "test_output_float", "test_Eb" }
}
links {
......@@ -16,6 +17,7 @@ links {
{ "test_w", "value", "mm", "w", true }
{ "mm", "I", "test_I", "value", true }
{ "mm", "T", "test_T", "value", true }
{ "mm", "Eb", "test_Eb", "value", true }
}
params {
......@@ -33,6 +35,10 @@ params {
(float) { 0.0, 0.025, -0.225, 0.55 },
(float) { 0.0, 0.0, 0.0001, 0.0 }
}
{ "test_Eb", "value", 4,
(float) { 0.0, 1.0, -1.0, 2.0 },
(float) { 0.0, 0.0, 0.0001, 0.0001 }
}
}
set trace_server false
controller/block/block_setpoint_generator_3d.c
View file @ 0e311aef
......@@ -34,7 +34,7 @@
inputs outputs
nr name nr name
--------------------------
-------------------------------
| |
----| 0 reset_x 0 x |----
| |
......@@ -42,17 +42,22 @@
| |
----| 2 track_x 2 a |----
| |
| 3 j |----
----| 3 track_v 3 j |----
| |
| 4 setpoint |----
----| 4 track_x_cmd 4 setpoint |----
| |
--------------------------
----| 5 track_v_cmd |
| |
-------------------------------
'setpoint' is the setpoint as the user requested it.
'x' is the output of the setpoint generator.
'v', 'a' and 'j' are the first, second and third
order derivatives of 'x'.
'v', 'a' and 'j' are the first, second and third order derivatives of 'x'.
While reset is active the reset_x value is taken as new setpoint value.
(Both x and setpoint are updated and are not limited by v, a, j)
track_x and track_v are the inputs for setpoint and speed tracking commands.
*/
struct spg_command {
......@@ -80,6 +85,9 @@ struct controller_block_private {
float *reset_x;
bool *reset;
float *track_x;
float *track_v;
bool *track_x_cmd;
bool *track_v_cmd;
/* beware: 'samples' is the time unit, not 'seconds',
all parameters and commands are scaled on block entry/exit */
......@@ -256,6 +264,13 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
goto set_output;
}
if (*priv->track_x_cmd) {
priv->cur_done = false;
priv->cur_command.type = COMMAND_PTYPE_SETPOINT_TRACK;
} else if (*priv->track_v_cmd) {
priv->cur_done = false;
priv->cur_command.type = COMMAND_PTYPE_SPEED_TRACK;
}
if (priv->cur_done) {
int r;
......@@ -289,6 +304,9 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cmd_v = priv->cur_command.value.f * controller_time_period_get(spg->time);
priv->cur_done = true;
break;
case COMMAND_PTYPE_SPEED_TRACK:
priv->cur_done = true;
break;
case COMMAND_PTYPE_SETPOINT_TIME:
if (!priv->cur_start) {
controller_trigger_time command_t;
......@@ -330,6 +348,12 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
ignore_x = true;
priv->cmd_x = cur_x + priv->cmd_v;
}
if (priv->cur_command.type == COMMAND_PTYPE_SPEED_TRACK) {
float track_v = *priv->track_v;
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;
}
......@@ -809,6 +833,7 @@ static int block_setpoint_generator_command_filter(struct controller_command *co
break;
}
case COMMAND_PTYPE_SETPOINT_TRACK:
case COMMAND_PTYPE_SPEED_TRACK:
break;
default:
r = -1;
......@@ -945,6 +970,9 @@ static struct controller_block_interm_list interms[] = {
{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
{ "reset", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, reset) },
{ "track_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_x) },
{ "track_v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_v) },
{ "track_x_cmd", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, track_x_cmd) },
{ "track_v_cmd", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, track_v_cmd) },
{ NULL }
};
......@@ -1033,8 +1061,9 @@ static struct controller_block * block_setpoint_generator_3d_create(char *name,
spg->private->command->value_type = COMMAND_VALUE_TYPE_FLOAT;
spg->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;
spg->private->command->command_types[1] = COMMAND_PTYPE_SPEED;
spg->private->command->command_types[2] = COMMAND_PTYPE_SETPOINT_TIME;
spg->private->command->command_types[3] = COMMAND_PTYPE_SETPOINT_TRACK;
spg->private->command->command_types[2] = COMMAND_PTYPE_SPEED_TRACK;
spg->private->command->command_types[3] = COMMAND_PTYPE_SETPOINT_TIME;
spg->private->command->command_types[4] = COMMAND_PTYPE_SETPOINT_TRACK;
spg->private->command->filter =
block_setpoint_generator_command_filter;
......
controller/block/block_setpoint_generator_3d.test.ctrl
View file @ 0e311aef
......@@ -9,6 +9,8 @@ blocks (10.0, 0.0) {
{ "test_input_bool", "reset" }
{ "test_input_float", "reset_x" }
{ "test_input_float", "track_x" }
{ "test_input_float", "track_v" }
{ "value_bool", "false" }
{ "test_output_float", "setpoint" }
{ "test_output_float", "x" }
......@@ -24,6 +26,9 @@ links {
{ "reset", "value", "spg", "reset", true }
{ "reset_x", "value", "spg", "reset_x", true }
{ "track_x", "value", "spg", "track_x", true }
{ "track_v", "value", "spg", "track_v", true }
{ "false", "value", "spg", "track_x_cmd", true }
{ "false", "value", "spg", "track_v_cmd", true }
{ "spg", "x", "x", "value", true }
{ "spg", "v", "v", "value", true }
......@@ -75,6 +80,12 @@ params {
40.0, 1337.0
}
}
{ "track_v", "value", 22,
(float) { 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0,
42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0, 42.0,
40.0, 1337.0
}
}
{ "setpoint", "value", 22,
(float) { 50.0, 56.0, 62.0, 68.0, 74.0, 80.0, -100.0, 200.0, 61.0, 51.1395,
......
controller/block/build.mk
View file @ 0e311aef
......@@ -7,6 +7,9 @@ BLOCKS := \
add \
and \
bridge_pwm \
command_bool \
command_float \
command_uint32 \
controller_profile \
counter \
deadzone \
......@@ -38,6 +41,9 @@ BLOCKS := \
quantize \
random \
rangecheck \
setpoint_generator_1d \
setpoint_generator_3d \
servo_state \
sine \
state_machine \
subtract \
......@@ -51,16 +57,6 @@ BLOCKS := \
schmitt_trigger \
oneshot
ifdef BUILD_TCP
BLOCKS += \
command_bool \
command_float \
command_uint32 \