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

After test at corso

parent 0e9189bc
...@@ -21,3 +21,5 @@ Building the software ...@@ -21,3 +21,5 @@ Building the software
Just run "make". To see more targets, run "make help". You may also run make in Just run "make". To see more targets, run "make help". You may also run make in
the individual directories. 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
...@@ -21,5 +21,5 @@ BUILD_FILEIO=@BUILD_FILEIO@ ...@@ -21,5 +21,5 @@ BUILD_FILEIO=@BUILD_FILEIO@
BUILD_EMBEDDED=@BUILD_EMBEDDED@ BUILD_EMBEDDED=@BUILD_EMBEDDED@
BUILD_SYSTICK=@BUILD_SYSTICK@ 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@
...@@ -36,7 +36,8 @@ enum command_ptype { ...@@ -36,7 +36,8 @@ enum command_ptype {
COMMAND_PTYPE_SPEED = 5, /* Simple direct 1st derivative setpoint */ COMMAND_PTYPE_SPEED = 5, /* Simple direct 1st derivative setpoint */
COMMAND_PTYPE_SETPOINT_TIME = 6, /* Setpoint to be reached @ time */ COMMAND_PTYPE_SETPOINT_TIME = 6, /* Setpoint to be reached @ time */
COMMAND_PTYPE_SETPOINT_TRACK = 7, /* Use setpoint from track input */ 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! */ COMMAND_PTYPE_MAX, /* Always the last entry! */
}; };
......
...@@ -193,8 +193,8 @@ AS_IF([test "$HAVE_TCP" = "1"], ...@@ -193,8 +193,8 @@ AS_IF([test "$HAVE_TCP" = "1"],
[AC_SUBST(BUILD_TCP,[""])]) [AC_SUBST(BUILD_TCP,[""])])
AS_IF([test "$HAVE_SEMAPHORE" = "1"], AS_IF([test "$HAVE_SEMAPHORE" = "1"],
[AC_SUBST(CFLAGS_TCP,["-DHAVE_SEMAPHORE"])], [AC_SUBST(CFLAGS_SEMAPHORE,["-DHAVE_SEMAPHORE"])],
[AC_SUBST(CFLAGS_TCP,[""])]) [AC_SUBST(CFLAGS_SEMAPHORE,[""])])
AS_IF([test "$HAVE_HEADER_LINUX_JOYSTICK" = "1"], AS_IF([test "$HAVE_HEADER_LINUX_JOYSTICK" = "1"],
[AC_SUBST(BUILD_LINUX_JOYSTICK,["yes"])], [AC_SUBST(BUILD_LINUX_JOYSTICK,["yes"])],
......
/*
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 },
};
...@@ -90,7 +90,9 @@ static struct controller_block * block_atsamx70_pio_in_create(char *name, int ar ...@@ -90,7 +90,9 @@ static struct controller_block * block_atsamx70_pio_in_create(char *name, int ar
ioport_init(); ioport_init();
ioport_enable_pin(pio->private->pin); 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_dir(pio->private->pin, IOPORT_DIR_INPUT);
ioport_set_pin_sense_mode(pio->private->pin, IOPORT_SENSE_BOTHEDGES);
if (controller_block_add(pio)) if (controller_block_add(pio))
goto err_add; goto err_add;
......
...@@ -92,6 +92,7 @@ static struct controller_block * block_atsamx70_pio_out_create(char *name, int a ...@@ -92,6 +92,7 @@ static struct controller_block * block_atsamx70_pio_out_create(char *name, int a
ioport_enable_pin(pio->private->pin); ioport_enable_pin(pio->private->pin);
ioport_set_pin_dir(pio->private->pin, IOPORT_DIR_OUTPUT); ioport_set_pin_dir(pio->private->pin, IOPORT_DIR_OUTPUT);
ioport_set_pin_mode(pio->private->pin, 0);
if (controller_block_add(pio)) if (controller_block_add(pio))
goto err_add; goto err_add;
......
...@@ -41,10 +41,12 @@ struct controller_block_private { ...@@ -41,10 +41,12 @@ struct controller_block_private {
uint32_t tcn; uint32_t tcn;
}; };
#include <stdio.h>
static void tc_calculate(struct controller_block *tc) static void tc_calculate(struct controller_block *tc)
{ {
struct controller_block_private *priv = tc->private; struct controller_block_private *priv = tc->private;
uint16_t cnt0; uint16_t cnt0;
int16_t cnt0p;
int16_t cnt1; int16_t cnt1;
uint16_t prevcnt0 = priv->prevcnt0; uint16_t prevcnt0 = priv->prevcnt0;
float speed; float speed;
...@@ -59,17 +61,19 @@ static void tc_calculate(struct controller_block *tc) ...@@ -59,17 +61,19 @@ static void tc_calculate(struct controller_block *tc)
priv->homed |= qisr & TC_QISR_IDX; priv->homed |= qisr & TC_QISR_IDX;
priv->homed &= !(*priv->reset); priv->homed &= !(*priv->reset);
if (cnt0 > rev) cnt0p = cnt0;
cnt0 += rev; // if (cnt0p > rev)
// cnt0p += rev;
int16_t diff = cnt0 - prevcnt0; int16_t diff = cnt0 - prevcnt0;
if (diff > rev/2) if (diff > rev/2)
diff -= rev; diff -= rev;
if (diff < -rev/2) if (diff < -rev/2)
diff += rev; diff += rev;
speed = diff * priv->speedfac;
position = (cnt1 * rev + cnt0) * priv->posfac; speed = diff * priv->speedfac;
position = (/*cnt1 * rev + */cnt0p) * priv->posfac;
priv->prevcnt0 = cnt0; priv->prevcnt0 = cnt0;
priv->speed = speed; priv->speed = speed;
...@@ -204,7 +208,8 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc, ...@@ -204,7 +208,8 @@ static struct controller_block * block_atsamx70_tc_create(char *name, int argc,
*/ */
tc_init(tc->private->tcn, TC_CH0, 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); 0);
tc_write_rc(tc->private->tcn, TC_CH0, rev * 4); 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, ...@@ -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_CH0);
tc_start(tc->private->tcn, TC_CH1); 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); tc->private->posfac = (M_PI * 2) / (rev * 4);
if (controller_block_add(tc)) if (controller_block_add(tc))
......
...@@ -3,6 +3,7 @@ ATSAMX70_TARGETS := $(LIBDIR)/libatsamx70.la ...@@ -3,6 +3,7 @@ ATSAMX70_TARGETS := $(LIBDIR)/libatsamx70.la
ATSAMX70_BLOCKS := \ ATSAMX70_BLOCKS := \
atsamx70_afec \ atsamx70_afec \
atsamx70_gmac \
atsamx70_pio_in \ atsamx70_pio_in \
atsamx70_pio_out \ atsamx70_pio_out \
atsamx70_pwm \ atsamx70_pwm \
......
...@@ -47,8 +47,16 @@ struct controller_block_private { ...@@ -47,8 +47,16 @@ struct controller_block_private {
static void gain_ra_calculate(struct controller_block *gain) static void gain_ra_calculate(struct controller_block *gain)
{ {
struct controller_block_private *priv = gain->private; 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[] = { static struct controller_block_interm_list interms[] = {
......
...@@ -30,7 +30,9 @@ ...@@ -30,7 +30,9 @@
---------------------- ----------------------
| | | |
---| 0 I 0 Ilim |---- ---| 0 I 0 Ilim |----
| |
| 1 limiting |----
| | | |
---------------------- ----------------------
...@@ -39,6 +41,7 @@ ...@@ -39,6 +41,7 @@
struct controller_block_private { struct controller_block_private {
float *I; float *I;
float Ilim; float Ilim;
bool limiting;
float i2t; float i2t;
...@@ -64,10 +67,13 @@ static void i2t_calculate(struct controller_block *block) ...@@ -64,10 +67,13 @@ static void i2t_calculate(struct controller_block *block)
if (i2t < 0.0) if (i2t < 0.0)
i2t = 0.0; i2t = 0.0;
if (i2t >= peak2t) if (i2t >= peak2t) {
priv->Ilim = priv->continuous; priv->Ilim = priv->continuous;
else priv->limiting = true;
} else {
priv->limiting = false;
priv->Ilim = sqrtf(peak2t - i2t + continuous2); priv->Ilim = sqrtf(peak2t - i2t + continuous2);
}
priv->i2t = i2t; priv->i2t = i2t;
} }
...@@ -128,7 +134,8 @@ static struct controller_block_interm_list interms[] = { ...@@ -128,7 +134,8 @@ static struct controller_block_interm_list interms[] = {
}; };
static struct controller_block_outterm_list outterms[] = { static struct controller_block_outterm_list outterms[] = {
{ "Ilim", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Ilim) }, { "Ilim", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Ilim) },
{ "limiting", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, limiting) },
{ NULL } { NULL }
}; };
......
...@@ -63,17 +63,19 @@ static void mmdcv_calculate(struct controller_block *mmdcv) ...@@ -63,17 +63,19 @@ static void mmdcv_calculate(struct controller_block *mmdcv)
struct controller_block_private *priv = mmdcv->private; struct controller_block_private *priv = mmdcv->private;
float V = *priv->V; float V = *priv->V;
float w = *priv->w; float w = *priv->w;
float I, Eb, T; float I, Eb, Er, T;
float kt = priv->kt; float kt = priv->kt;
float ke = priv->ke; float ke = priv->ke;
float iR = priv->iR; float iR = priv->iR;
Eb = V - w * ke; Eb = w * ke;
I = (Eb) * iR; Er = V - Eb;
I = (Er) * iR;
T = I * kt; T = I * kt;
priv->I = I; priv->I = I;
priv->T = T; priv->T = T;
priv->Eb = Eb;
} }
static int param_set_kt(struct controller_block *mmdcv, char *param, int argc, static int param_set_kt(struct controller_block *mmdcv, char *param, int argc,
......
...@@ -9,13 +9,15 @@ blocks (1000.0, 0.0) { ...@@ -9,13 +9,15 @@ blocks (1000.0, 0.0) {
{ "test_input_float", "test_w" } { "test_input_float", "test_w" }
{ "test_output_float", "test_I" } { "test_output_float", "test_I" }
{ "test_output_float", "test_T" } { "test_output_float", "test_T" }
{ "test_output_float", "test_Eb" }
} }
links { links {
{ "test_V", "value", "mm", "V", true } { "test_V", "value", "mm", "V", true }
{ "test_w", "value", "mm", "w", true } { "test_w", "value", "mm", "w", true }
{ "mm", "I", "test_I", "value", true } { "mm", "I", "test_I", "value", true }
{ "mm", "T", "test_T", "value", true } { "mm", "T", "test_T", "value", true }
{ "mm", "Eb", "test_Eb", "value", true }
} }
params { params {
...@@ -33,6 +35,10 @@ params { ...@@ -33,6 +35,10 @@ params {
(float) { 0.0, 0.025, -0.225, 0.55 }, (float) { 0.0, 0.025, -0.225, 0.55 },
(float) { 0.0, 0.0, 0.0001, 0.0 } (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 set trace_server false
...@@ -34,25 +34,30 @@ ...@@ -34,25 +34,30 @@
inputs outputs inputs outputs
nr name nr name nr name nr name
-------------------------- -------------------------------
| | | |
----| 0 reset_x 0 x |---- ----| 0 reset_x 0 x |----
| | | |
----| 1 reset 1 v |---- ----| 1 reset 1 v |----
| | | |
----| 2 track_x 2 a |---- ----| 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. 'setpoint' is the setpoint as the user requested it.
'x' is the output of the setpoint generator. 'x' is the output of the setpoint generator.
'v', 'a' and 'j' are the first, second and third 'v', 'a' and 'j' are the first, second and third order derivatives of 'x'.
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 { struct spg_command {
...@@ -80,6 +85,9 @@ struct controller_block_private { ...@@ -80,6 +85,9 @@ struct controller_block_private {
float *