Skip to content

Commit 2688c15

Browse files
yqiu2018yqiu2018
authored
Merge pull request #15 from sogwms/master
update encoder
2 parents a2755f8 + 3ab83a6 commit 2688c15

File tree

10 files changed

+256
-49
lines changed

10 files changed

+256
-49
lines changed

‎encoder/ab_phase_encoder.c‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
#include "ab_phase_encoder.h"
2+
3+
#define DBG_SECTION_NAME "ab_phase_encoder"
4+
#define DBG_LEVEL DBG_LOG
5+
#include <rtdbg.h>
6+
7+
struct ab_phase_encoder
8+
{
9+
struct encoder enc;
10+
rt_base_t pin_a; /* interrupt pin */
11+
rt_base_t pin_b;
12+
rt_int16_t last_value;
13+
};
14+
15+
static const uint8_t AB_PHASE_ENCODER_TABLE[4] = {2, 0, 3, 1};
16+
17+
static void encoder_isr(void *args)
18+
{
19+
ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)args;
20+
int val = rt_pin_read(enc_sub->pin_a) * 2 + rt_pin_read(enc_sub->pin_b);
21+
// rt_kprintf("val:%d\n", val);
22+
if (AB_PHASE_ENCODER_TABLE[enc_sub->last_value] == val)
23+
{
24+
enc_sub->enc.dir = ENCODER_DIR_FORWARD;
25+
enc_sub->enc.pulse_count++;
26+
}
27+
else
28+
{
29+
enc_sub->enc.dir = ENCODER_DIR_BACKWARD;
30+
enc_sub->enc.pulse_count--;
31+
}
32+
enc_sub->last_value = val;
33+
}
34+
35+
static rt_err_t ab_phase_encoder_enable(void *enc)
36+
{
37+
RT_ASSERT(enc != RT_NULL);
38+
39+
ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)enc;
40+
41+
// Attach interrupts
42+
rt_pin_mode(enc_sub->pin_a, PIN_MODE_INPUT);
43+
rt_pin_mode(enc_sub->pin_b, PIN_MODE_INPUT);
44+
rt_pin_attach_irq(enc_sub->pin_a, PIN_IRQ_MODE_RISING_FALLING, encoder_isr, enc_sub);
45+
rt_pin_attach_irq(enc_sub->pin_b, PIN_IRQ_MODE_RISING_FALLING, encoder_isr, enc_sub);
46+
enc_sub->last_value = rt_pin_read(enc_sub->pin_a) * 2 + rt_pin_read(enc_sub->pin_b);
47+
enc_sub->enc.last_time = rt_tick_get();
48+
rt_pin_irq_enable(enc_sub->pin_a, PIN_IRQ_ENABLE);
49+
rt_pin_irq_enable(enc_sub->pin_b, PIN_IRQ_ENABLE);
50+
51+
return RT_EOK;
52+
}
53+
54+
static rt_err_t ab_phase_encoder_disable(void *enc)
55+
{
56+
RT_ASSERT(enc != RT_NULL);
57+
58+
ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)enc;
59+
60+
rt_pin_irq_enable(enc_sub->pin_a, PIN_IRQ_DISABLE);
61+
rt_pin_irq_enable(enc_sub->pin_b, PIN_IRQ_DISABLE);
62+
63+
return RT_EOK;
64+
}
65+
66+
static rt_err_t ab_phase_encoder_destroy(void *enc)
67+
{
68+
RT_ASSERT(enc != RT_NULL);
69+
70+
ab_phase_encoder_disable(enc);
71+
ab_phase_encoder_t enc_sub = (ab_phase_encoder_t)enc;
72+
rt_pin_detach_irq(enc_sub->pin_a);
73+
rt_pin_detach_irq(enc_sub->pin_b);
74+
75+
rt_free(enc_sub);
76+
77+
return RT_EOK;
78+
}
79+
80+
ab_phase_encoder_t ab_phase_encoder_create(rt_base_t pin_a, rt_base_t pin_b, rt_uint16_t pulse_revol)
81+
{
82+
// Malloc memory and initialize pulse_count and pin
83+
ab_phase_encoder_t new_encoder = (ab_phase_encoder_t)encoder_create(sizeof(struct ab_phase_encoder));
84+
if(new_encoder == RT_NULL)
85+
{
86+
return RT_NULL;
87+
}
88+
89+
new_encoder->pin_a = pin_a;
90+
new_encoder->pin_b = pin_b;
91+
new_encoder->last_value = 0;
92+
new_encoder->enc.pulse_revol = pulse_revol;
93+
new_encoder->enc.enable = ab_phase_encoder_enable;
94+
new_encoder->enc.disable = ab_phase_encoder_disable;
95+
new_encoder->enc.destroy = ab_phase_encoder_destroy;
96+
97+
return new_encoder;
98+
}

‎encoder/ab_phase_encoder.h‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
#ifndef __AB_PHASE_ENCODER_H__
2+
#define __AB_PHASE_ENCODER_H__
3+
4+
#include "encoder.h"
5+
6+
struct ab_phase_encoder;
7+
typedef struct ab_phase_encoder *ab_phase_encoder_t;
8+
9+
ab_phase_encoder_t ab_phase_encoder_create(rt_base_t pin_a, rt_base_t pin_b, rt_uint16_t pulse_revol);
10+
11+
#endif // __AB_PHASE_ENCODER_H__

‎encoder/encoder.c‎

Lines changed: 38 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -5,98 +5,106 @@
55
#define DBG_LEVEL DBG_LOG
66
#include <rtdbg.h>
77

8-
void encoder_isr(void *args)
8+
encoder_t encoder_create(rt_size_t size)
99
{
10-
rt_uint16_t* pulse_count = (rt_uint16_t*)args;
11-
(*pulse_count)++;
12-
// LOG_D("Count %d", *pulse_count);
13-
}
14-
15-
encoder_t encoder_create(rt_base_t pin, rt_uint16_t pulse_revol)
16-
{
17-
// Malloc memory and initialize pulse_count and pin
18-
encoder_t new_encoder = (encoder_t)rt_malloc(sizeof(struct encoder));
10+
// Malloc memory and initialize
11+
encoder_t new_encoder = (encoder_t)rt_malloc(size);
1912
if(new_encoder == RT_NULL)
2013
{
2114
LOG_E("Failed to malloc memory for new encoder\n");
2215
return RT_NULL;
2316
}
2417

2518
new_encoder->pulse_count = 0;
26-
new_encoder->pin = pin;
27-
new_encoder->pulse_revol = pulse_revol;
2819
new_encoder->last_count = 0;
2920

3021
return new_encoder;
3122
}
3223

33-
void encoder_destroy(encoder_t enc)
24+
rt_err_t encoder_destroy(encoder_t enc)
3425
{
3526
LOG_D("Free Encoder");
27+
RT_ASSERT(enc != RT_NULL);
3628

37-
rt_free(enc);
29+
return enc->destroy(enc);
3830
}
3931

4032
rt_err_t encoder_enable(encoder_t enc)
4133
{
4234
LOG_D("Enabling encoder");
35+
RT_ASSERT(enc != RT_NULL);
4336

44-
// Attach interrupts
45-
rt_pin_mode(enc->pin, PIN_MODE_INPUT_PULLUP);
46-
rt_pin_attach_irq(enc->pin, PIN_IRQ_MODE_FALLING, encoder_isr, &(enc->pulse_count));
47-
enc->last_time = rt_tick_get();
48-
49-
return rt_pin_irq_enable(enc->pin, PIN_IRQ_ENABLE);
37+
return enc->enable(enc);
5038
}
5139

5240
rt_err_t encoder_disable(encoder_t enc)
5341
{
5442
LOG_D("Diabling encoder");
43+
RT_ASSERT(enc != RT_NULL);
5544

56-
return rt_pin_detach_irq(enc->pin);
45+
return enc->disable(enc);
5746
}
5847

5948
rt_int16_t encoder_read(encoder_t enc)
6049
{
50+
RT_ASSERT(enc != RT_NULL);
51+
6152
return enc->pulse_count;
6253
}
6354

6455
rt_err_t encoder_reset(encoder_t enc)
6556
{
66-
enc -> pulse_count = 0;
57+
RT_ASSERT(enc != RT_NULL);
58+
59+
enc->pulse_count = 0;
60+
6761
return RT_EOK;
6862
}
6963

7064
rt_int16_t encoder_measure_cps(encoder_t enc)
7165
{
66+
RT_ASSERT(enc != RT_NULL);
7267
// TODO
7368
// return count per second
7469
if((rt_tick_get() - enc->last_time) < rt_tick_from_millisecond(enc->sample_time))
7570
{
76-
// LOG_D("Encoder waiting ... ");
77-
// LOG_D("Current count per second %d", enc->cps);
71+
LOG_D("Encoder waiting ... ");
7872
return enc->cps;
7973
}
8074

81-
enc->cps = (enc->pulse_count - enc->last_count) * 1000 / enc->sample_time;
75+
rt_int32_t diff_count = enc->pulse_count - enc->last_count;
76+
77+
// if (diff_count >= INT32_MAX)
78+
// {
79+
// diff_count = -(enc->pulse_count + enc->last_count);
80+
// }
81+
// if (diff_count <= INT32_MIN)
82+
// {
83+
// diff_count = enc->pulse_count + enc->last_count;
84+
// }
85+
86+
enc->cps = diff_count * 1000 / enc->sample_time;
8287
enc->last_count = enc->pulse_count;
8388
enc->last_time = rt_tick_get();
84-
// LOG_D("Current count per second %d", enc->cps);
8589

8690
return enc->cps;
8791
}
8892

8993
rt_int16_t encoder_measure_rpm(encoder_t enc)
9094
{
95+
RT_ASSERT(enc != RT_NULL);
96+
9197
// return resolution per minute
92-
// LOG_D("Current count per revolution %d", enc->pulse_revol);
93-
9498
rt_int16_t res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol;
95-
// LOG_D("Current revol per minute %d", res_rpm);
99+
96100
return res_rpm;
97101
}
98102

99-
void encoder_set_sample_time(encoder_t enc, rt_uint16_t sample_time)
103+
rt_err_t encoder_set_sample_time(encoder_t enc, rt_uint16_t sample_time)
100104
{
105+
RT_ASSERT(enc != RT_NULL);
106+
101107
enc->sample_time = sample_time;
108+
109+
return RT_EOK;
102110
}

‎encoder/encoder.h‎

Lines changed: 26 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -2,34 +2,41 @@
22
#define __ENCODER_H__
33

44
#include <rtthread.h>
5+
#include <rtdevice.h>
56

6-
struct encoder
7+
enum encoder_direction
78
{
8-
rt_base_t pin; /* interrupt pin */
9-
rt_int16_t pulse_count; /* absolute pulse number in total */
10-
rt_uint16_t pulse_revol; /* pulse number per revolution */
11-
rt_uint16_t sample_time; /* sample time in microseconds for speed measurement */
12-
rt_tick_t last_time;
13-
rt_int16_t last_count;
14-
rt_int16_t cps;
9+
ENCODER_DIR_FORWARD,
10+
ENCODER_DIR_BACKWARD,
1511
};
1612

1713
typedef struct encoder *encoder_t;
1814

19-
encoder_t encoder_create(rt_base_t pin, rt_uint16_t pulse_revol);
20-
void encoder_destroy(encoder_t enc);
15+
struct encoder
16+
{
17+
rt_int32_t pulse_count; /* absolute pulse number in total */
18+
rt_uint16_t pulse_revol; /* pulse number per revolution */
19+
rt_uint16_t sample_time; /* sample time in microseconds for speed measurement */
20+
rt_tick_t last_time;
21+
rt_int32_t last_count;
22+
rt_int16_t cps;
23+
enum encoder_direction dir;
24+
rt_err_t (*enable)(void *enc);
25+
rt_err_t (*disable)(void *enc);
26+
rt_err_t (*destroy)(void *enc);
27+
};
2128

22-
rt_err_t encoder_enable(encoder_t enc); /* start measurement */
23-
rt_err_t encoder_disable(encoder_t enc); /* stop measurement */
24-
rt_err_t encoder_reset(encoder_t enc); /* set pulse_count to 0 */
29+
encoder_t encoder_create(rt_size_t size);
30+
rt_err_t encoder_destroy(encoder_t enc);
31+
rt_err_t encoder_enable(encoder_t enc); /* start measurement */
32+
rt_err_t encoder_disable(encoder_t enc); /* stop measurement */
33+
rt_err_t encoder_reset(encoder_t enc); /* set pulse_count to 0 */
2534

26-
void encoder_set_sample_time(encoder_t enc, rt_uint16_t sample_time); /* set sample time */
35+
rt_err_t encoder_set_sample_time(encoder_t enc, rt_uint16_t sample_time); /* set sample time */
2736

2837
/** rpm = pulse_count / sample_time(ms) / pulse_revol * 1000 * 60 **/
29-
rt_int16_t encoder_read(encoder_t enc); /* return pulse_count */
30-
rt_int16_t encoder_measure_cps(encoder_t enc); /* pulse_count per second */
31-
rt_int16_t encoder_measure_rpm(encoder_t enc); /* revolutions per minute */
32-
33-
// Encoder Thread
38+
rt_int16_t encoder_read(encoder_t enc); /* return pulse_count */
39+
rt_int16_t encoder_measure_cps(encoder_t enc); /* pulse_count per second */
40+
rt_int16_t encoder_measure_rpm(encoder_t enc); /* revolutions per minute */
3441

3542
#endif

‎encoder/encoder_AB.c‎

Whitespace-only changes.

‎encoder/encoder_AB.h‎

Whitespace-only changes.

‎encoder/encoder_single.c‎

Whitespace-only changes.

‎encoder/encoder_single.h‎

Whitespace-only changes.

‎encoder/single_phase_encoder.c‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
#include "single_phase_encoder.h"
2+
3+
#define DBG_SECTION_NAME "single_phase_encoder"
4+
#define DBG_LEVEL DBG_LOG
5+
#include <rtdbg.h>
6+
7+
struct single_phase_encoder
8+
{
9+
struct encoder enc;
10+
rt_base_t pin; /* interrupt pin */
11+
};
12+
13+
static void encoder_isr(void *args)
14+
{
15+
rt_int32_t* pulse_count = (rt_int32_t*)args;
16+
(*pulse_count)++;
17+
// LOG_D("Count %d", *pulse_count);
18+
}
19+
20+
static rt_err_t single_phase_encoder_enable(void *enc)
21+
{
22+
RT_ASSERT(enc != RT_NULL);
23+
24+
single_phase_encoder_t enc_sub = (single_phase_encoder_t)enc;
25+
26+
// Attach interrupts
27+
rt_pin_mode(enc_sub->pin, PIN_MODE_INPUT_PULLUP);
28+
rt_pin_attach_irq(enc_sub->pin, PIN_IRQ_MODE_FALLING, encoder_isr, &(enc_sub->enc.pulse_count));
29+
enc_sub->enc.last_time = rt_tick_get();
30+
31+
return rt_pin_irq_enable(enc_sub->pin, PIN_IRQ_ENABLE);
32+
}
33+
34+
static rt_err_t single_phase_encoder_disable(void *enc)
35+
{
36+
RT_ASSERT(enc != RT_NULL);
37+
38+
single_phase_encoder_t enc_sub = (single_phase_encoder_t)enc;
39+
40+
return rt_pin_irq_enable(enc_sub->pin, PIN_IRQ_DISABLE);;
41+
}
42+
43+
static rt_err_t single_phase_encoder_destroy(void *enc)
44+
{
45+
RT_ASSERT(enc != RT_NULL);
46+
47+
single_phase_encoder_disable(enc);
48+
single_phase_encoder_t enc_sub = (single_phase_encoder_t)enc;
49+
rt_pin_detach_irq(enc_sub->pin);
50+
51+
rt_free(enc);
52+
53+
return RT_EOK;
54+
}
55+
56+
single_phase_encoder_t single_phase_encoder_create(rt_base_t pin,rt_uint16_t pulse_revol)
57+
{
58+
// Malloc memory and initialize pulse_count and pin
59+
single_phase_encoder_t new_encoder = (single_phase_encoder_t)encoder_create(sizeof(struct single_phase_encoder));
60+
if(new_encoder == RT_NULL)
61+
{
62+
return RT_NULL;
63+
}
64+
65+
new_encoder->pin = pin;
66+
new_encoder->enc.pulse_revol = pulse_revol;
67+
new_encoder->enc.enable = single_phase_encoder_enable;
68+
new_encoder->enc.disable = single_phase_encoder_disable;
69+
new_encoder->enc.destroy = single_phase_encoder_destroy;
70+
71+
return new_encoder;
72+
}

‎encoder/single_phase_encoder.h‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
#ifndef __SINGLE_PHASE_ENCODER_H__
2+
#define __SINGLE_PHASE_ENCODER_H__
3+
4+
#include "encoder.h"
5+
6+
struct single_phase_encoder;
7+
typedef struct single_phase_encoder *single_phase_encoder_t;
8+
9+
single_phase_encoder_t single_phase_encoder_create(rt_base_t pin, rt_uint16_t pulse_revol);
10+
11+
#endif // __SINGLE_PHASE_ENCODER_H__

0 commit comments

Comments
 (0)