You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

299 lines
7.6 KiB

/*
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 <samx70.h>
#include <tc.h>
#include <pio/samx70.h>
#include <ioport.h>
#include <controller/controller_block.h>
#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;
float speed;
bool homed;
bool *reset;
uint16_t prevcnt0;
int rev;
float speedfac;
float posfac;
uint32_t tcn;
};
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;
float position;
int rev = priv->rev;
cnt0 = tc_get_cv(priv->tcn, 0);
// 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);
bool dir = qisr & TC_QISR_DIR;
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->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) },
{ "homed", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, homed) },
{ "speed", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed) },
{ NULL },
};
static struct controller_block_interm_list interms[] = {
{ "reset", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, reset) },
{ NULL }
};
static struct controller_block * block_atsamx70_tc_create(char *name, int argc, va_list ap)
{
struct controller_block *tc;
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) {
log_send(LOG_T_ERROR, "%s: tc%d is not valid. (valid: 0-3)",
name, tc_nr);
return NULL;
}
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;
if (controller_block_outterm_list_init(tc, outterms))
goto err_outterm;
if (controller_block_interm_list_init(tc, interms))
goto err_interm;
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;
switch (tc_nr) {
case 0:
tc->private->tcn = TC0;
pa = PIO_TIOA0_IDX;
pb = PIO_TIOB0_IDX;
pi = PIO_TIOB1_IDX;
fa = IOPORT_MODE_TIOA0;
fb = IOPORT_MODE_TIOB0;
fi = IOPORT_MODE_TIOB1;
break;
case 1:
tc->private->tcn = TC1;
pa = PIO_TIOA3_IDX;
pb = PIO_TIOB3_IDX;
pi = PIO_TIOB4_IDX;
fa = IOPORT_MODE_TIOA3;
fb = IOPORT_MODE_TIOB3;
fi = IOPORT_MODE_TIOB4;
break;
case 2:
tc->private->tcn = TC2;
pa = PIO_TIOA6_IDX;
pb = PIO_TIOB6_IDX;
pi = PIO_TIOB7_IDX;
fa = IOPORT_MODE_TIOA6;
fb = IOPORT_MODE_TIOB6;
fi = IOPORT_MODE_TIOB7;
break;
case 3:
tc->private->tcn = TC3;
pa = PIO_TIOA9_IDX;
pb = PIO_TIOB9_IDX;
pi = PIO_TIOB10_IDX;
fa = IOPORT_MODE_TIOA9;
fb = IOPORT_MODE_TIOB9;
fi = IOPORT_MODE_TIOB10;
break;
}
tc->private->rev = rev * 4;
tc->private->homed = false;
ioport_init();
ioport_disable_pin(pa);
ioport_set_pin_mode(pa, fa);
ioport_set_pin_dir(pa, IOPORT_DIR_INPUT);
ioport_disable_pin(pb);
ioport_set_pin_mode(pb, fb);
ioport_set_pin_dir(pb, IOPORT_DIR_INPUT);
ioport_disable_pin(pi);
ioport_set_pin_mode(pi, fi);
ioport_set_pin_dir(pi, IOPORT_DIR_INPUT);
/*
When writing a 0 to TC_BMR.QDEN, the QDEC is bypassed and the IO pins are
directly routed to the timer counter function.
When TC_BMR.POSEN is set, the motor axis position is processed on channel 0
(by means of the PHA, PHB edge detections) and the number of motor revolutions
are recorded on channel 1 if the IDX signal is provided on the TIOB1 input.
If no IDX signal is available, the internal counter can be cleared for each
revolution if the number of counts per revolution is configured in TC_RC0.RC
and the TC_CMR.CPCTRG bit is written to 1.
The position measurement can be read in the TC_CV0 register and the rotation
measurement can be read in the TC_CV1 register.
Channel 0 and 1 must be configured in Capture mode (TC_CMR0.WAVE = 0).
Rising edge must be selected as the External Trigger Edge (TC_CMR.ETRGEDG = 0x01)
and TIOAx must be selected as the External Trigger (TC_CMR.ABETRG = 0x1).
The process must be started by configuring TC_CCR.CLKEN and TC_CCR.SWTRG.
In parallel, the number of edges are accumulated on TC channel 0 and
can be read on the TC_CV0 register.
Therefore, the accurate position can be read on both TC_CV registers and
concatenated to form a 32-bit word.
The TC channel 0 is cleared for each increment of IDX count value.
Depending on the quadrature signals, the direction is decoded
and allows to count up or down in TC channels 0 and 1.
The direction status is reported on TC_QISR.
*/
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,
TC_CMR_TCCLKS_XC0,
0);
tc_init(tc->private->tcn, TC_CH2, 0, 0);
tc_block_mode_set(tc->private->tcn, TC_BMR_QDEN | TC_BMR_POSEN | TC_BMR_EDGPHA |
(inverted ? (TC_BMR_INVA | TC_BMR_INVB | TC_BMR_INVIDX) : (0)));
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.0) / (double)(rev * 4);
tc->private->posfac = (M_PI * 2) / (rev * 4);
if (controller_block_add(tc))
goto err_add;
return tc;
err_add:
err_interm:
err_outterm:
controller_block_free(tc);
err_alloc:
return NULL;
}
BLOCK_CREATE(atsamx70_tc) = {
.create = block_atsamx70_tc_create,
.args = { "int,int,int", "int,int,int,int", NULL },
};