Skip to content

Commit

Permalink
Merge pull request #12 from harp-tech/fw-create-encoder-relative-reading
Browse files Browse the repository at this point in the history 
Create displacement reading option for quadrature encoder.
  • Loading branch information
@bruno-f-cruz
bruno-f-cruz authored Sep 18, 2023
2 parents 619bc1e + 6639a79 commit 10df806
Show file tree
Hide file tree
Showing 7 changed files with 64 additions and 30 deletions.
38 changes: 25 additions & 13 deletions Firmware/Behavior/app.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ void hwbp_app_initialize(void)
uint8_t hwH = 2;
uint8_t hwL = 0;
uint8_t fwH = 3;
uint8_t fwL = 1;
uint8_t fwL = 2;
uint8_t ass = 0;

/* Start core */
Expand All @@ -49,9 +49,9 @@ void hwbp_app_initialize(void)
APP_NBYTES_OF_REG_BANK,
APP_REGS_ADD_MAX - APP_REGS_ADD_MIN + 1,
default_device_name,
true, // The device is _not_ able to repeat the harp timestamp clock
true, // The device is _not_ able to generate the harp timestamp clock
3 // Default timestamp offset
false, // The device is _not_ able to repeat the harp timestamp clock
false, // The device is _not_ able to generate the harp timestamp clock
0 // Default timestamp offset
);
}

Expand Down Expand Up @@ -363,6 +363,8 @@ uint8_t t1ms = 0;

bool first_adc_channel;

int16_t previous_encoder_poke2;

void core_callback_t_before_exec(void)
{
if (t1ms++ & 1)
Expand All @@ -379,15 +381,25 @@ void core_callback_t_before_exec(void)
if (app_regs.REG_EN_ENCODERS & B_EN_ENCODER_PORT2)
{
int16_t timer_cnt = TCD1_CNT;

if (timer_cnt > 32768)
{
app_regs.REG_DATA[1] = 0xFFFF - timer_cnt;
}
else
{
app_regs.REG_DATA[1] = (32768 - timer_cnt) * -1;
}

if (app_regs.REG_CONF_ENCODERS == GM_POSITION)
{
if (timer_cnt > 32768)
{
app_regs.REG_DATA[1] = 0xFFFF - timer_cnt;
}
else
{
app_regs.REG_DATA[1] = (32768 - timer_cnt) * -1;
}
}
else
{
app_regs.REG_DATA[1] = previous_encoder_poke2 - timer_cnt;

previous_encoder_poke2 = timer_cnt;
}

}
}
}
Expand Down
24 changes: 18 additions & 6 deletions Firmware/Behavior/app_funcs.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ void (*app_func_rd_pointer[])(void) = {
&app_read_REG_EN_SERVOS,
&app_read_REG_DIS_SERVOS,
&app_read_REG_EN_ENCODERS,
&app_read_REG_RESERVED1,
&app_read_REG_CONF_ENCODERS,
&app_read_REG_RESERVED2,
&app_read_REG_RESERVED3,
&app_read_REG_RESERVED4,
Expand Down Expand Up @@ -166,7 +166,7 @@ bool (*app_func_wr_pointer[])(void*) = {
&app_write_REG_EN_SERVOS,
&app_write_REG_DIS_SERVOS,
&app_write_REG_EN_ENCODERS,
&app_write_REG_RESERVED1,
&app_write_REG_CONF_ENCODERS,
&app_write_REG_RESERVED2,
&app_write_REG_RESERVED3,
&app_write_REG_RESERVED4,
Expand Down Expand Up @@ -1469,6 +1469,8 @@ bool app_write_REG_DIS_SERVOS(void *a)
/************************************************************************/
/* REG_EN_ENCODERS */
/************************************************************************/
extern int16_t previous_encoder_poke2;

void app_read_REG_EN_ENCODERS(void)
{
app_regs.REG_EN_ENCODERS = 0;
Expand Down Expand Up @@ -1505,6 +1507,7 @@ bool app_write_REG_EN_ENCODERS(void *a)
TCD1_CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH0_gc; // P. 180-1
TCD1_PER = 0xFFFF;
TCD1_CNT = 0x8000;
previous_encoder_poke2 = 0x8000;

/* Start timer */
TCD1_CTRLA=TC_CLKSEL_DIV1_gc;
Expand Down Expand Up @@ -1535,10 +1538,18 @@ bool app_write_REG_EN_ENCODERS(void *a)


/************************************************************************/
/* REG_RESERVED1 */
/* REG_CONF_ENCODERS */
/************************************************************************/
void app_read_REG_RESERVED1(void) {}
bool app_write_REG_RESERVED1(void *a) {return true;}
void app_read_REG_CONF_ENCODERS(void) {}
bool app_write_REG_CONF_ENCODERS(void *a)
{
uint8_t reg = *((uint8_t*)a);

if (reg & ~MSK_ENCODERS_MODE) return false;

app_regs.REG_CONF_ENCODERS = reg;
return true;
}
/************************************************************************/
/* REG_RESERVED2 */
/************************************************************************/
Expand Down Expand Up @@ -1759,7 +1770,8 @@ bool app_write_REG_ENCODERS_RESET(void *a)
{
if (_states_.quad_counter.port2)
{
TCD1_CNT = 0x8000;
TCD1_CNT = 0x8000;
previous_encoder_poke2 = 0x8000;
}
}

Expand Down
4 changes: 2 additions & 2 deletions Firmware/Behavior/app_funcs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ void app_read_REG_STOP_CAMERAS(void);
void app_read_REG_EN_SERVOS(void);
void app_read_REG_DIS_SERVOS(void);
void app_read_REG_EN_ENCODERS(void);
void app_read_REG_RESERVED1(void);
void app_read_REG_CONF_ENCODERS(void);
void app_read_REG_RESERVED2(void);
void app_read_REG_RESERVED3(void);
void app_read_REG_RESERVED4(void);
Expand Down Expand Up @@ -164,7 +164,7 @@ bool app_write_REG_STOP_CAMERAS(void *a);
bool app_write_REG_EN_SERVOS(void *a);
bool app_write_REG_DIS_SERVOS(void *a);
bool app_write_REG_EN_ENCODERS(void *a);
bool app_write_REG_RESERVED1(void *a);
bool app_write_REG_CONF_ENCODERS(void *a);
bool app_write_REG_RESERVED2(void *a);
bool app_write_REG_RESERVED3(void *a);
bool app_write_REG_RESERVED4(void *a);
Expand Down
2 changes: 1 addition & 1 deletion Firmware/Behavior/app_ios_and_regs.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -320,7 +320,7 @@ uint8_t *app_regs_pointer[] = {
(uint8_t*)(&app_regs.REG_EN_SERVOS),
(uint8_t*)(&app_regs.REG_DIS_SERVOS),
(uint8_t*)(&app_regs.REG_EN_ENCODERS),
(uint8_t*)(&app_regs.REG_RESERVED1),
(uint8_t*)(&app_regs.REG_CONF_ENCODERS),
(uint8_t*)(&app_regs.REG_RESERVED2),
(uint8_t*)(&app_regs.REG_RESERVED3),
(uint8_t*)(&app_regs.REG_RESERVED4),
Expand Down
9 changes: 6 additions & 3 deletions Firmware/Behavior/app_ios_and_regs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ typedef struct
uint8_t REG_EN_SERVOS;
uint8_t REG_DIS_SERVOS;
uint8_t REG_EN_ENCODERS;
uint8_t REG_RESERVED1;
uint8_t REG_CONF_ENCODERS;
uint8_t REG_RESERVED2;
uint8_t REG_RESERVED3;
uint8_t REG_RESERVED4;
Expand Down Expand Up @@ -277,7 +277,7 @@ typedef struct
#define ADD_REG_EN_SERVOS 80 // U8 Enable servo motors control
#define ADD_REG_DIS_SERVOS 81 // U8 Disable servo motors control
#define ADD_REG_EN_ENCODERS 82 // U8 Enable encoders
#define ADD_REG_RESERVED1 83 // U8 Reserved for future use
#define ADD_REG_CONF_ENCODERS 83 // U8 Configure the operation mode of the quadrature encoders
#define ADD_REG_RESERVED2 84 // U8 Reserved for future use
#define ADD_REG_RESERVED3 85 // U8 Reserved for future use
#define ADD_REG_RESERVED4 86 // U8 Reserved for future use
Expand Down Expand Up @@ -316,7 +316,7 @@ typedef struct
#define ADD_REG_MIMIC_PORT2_VALVE 119 // U8
#define ADD_REG_RESERVED23 120 // U8
#define ADD_REG_RESERVED24 121 // U8
#define ADD_REG_POKE_INPUT_FILTER_MS 122 // U8
#define ADD_REG_POKE_INPUT_FILTER_MS 122 // U8 Set the low pass filter time value for the pokes inputs (ms)

/************************************************************************/
/* PWM Generator registers' memory limits */
Expand Down Expand Up @@ -367,6 +367,9 @@ typedef struct
#define B_EN_SERVO_OUT2 (1<<2) // Servo on digital output 2
#define B_EN_SERVO_OUT3 (1<<3) // Servo on digital output 3
#define B_EN_ENCODER_PORT2 (1<<2) // Encoder on port 2
#define MSK_ENCODERS_MODE 0x01 //
#define GM_POSITION 0x00 //
#define GM_DISPLACEMENT 0x01 //
#define B_CAM_ACQ (1<<0) // Camera frame was triggered
#define B_RST_ENCODER_PORT2 (1<<2) // Reset the encoder counter on Port 2
#define B_EN_SRL_TSTAMP_PORT2 (1<<2) // Enable the serial timestamp TX on Port 2
Expand Down
Binary file modified Firmware/Behavior/registers.xls
View file
Binary file not shown.
17 changes: 12 additions & 5 deletions device.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
# yaml-language-server: $schema=https://raw.githubusercontent.com/harp-tech/reflex-generator/main/schema/device.json
device: Behavior
whoAmI: 1216
firmwareVersion: "3.1"
firmwareVersion: "3.2"
hardwareTargets: "1.1"
registers:
DigitalInputState:
Expand Down Expand Up @@ -275,9 +275,12 @@ registers:
access: Write
maskType: EncoderInputs
description: Specifies the port quadrature counters to enable in the device.
Reserved1:
<<: *reserved
EncoderMode:
address: 83
type: U8
access: Write
maskType: EncoderModeConfig
description: Configures the operation mode of the quadrature encoders.
Reserved2:
<<: *reserved
address: 84
Expand Down Expand Up @@ -504,5 +507,9 @@ groupMasks:
DO0: 4
DO1: 5
DO2: 6
DO3: 7

DO3: 7
EncoderModeConfig:
description: Specifies the type of reading made from the quadrature encoder.
values:
Position: 0
Displacement: 1

0 comments on commit 10df806

Please sign in to comment.