Skip to content

Commit 618db67

Browse files
beardedone55beardedone55
authored
Add Feature to Get Encoder Rate (Period) (#40)
* Add PIO Program for Encoder Period * Update Robot Code to Get Encoder Period * Added code to get Period from pio * Refactor EncoderPeriod Class Into Its Own Files * Added Code to Send Encoder Period Data Over Network. * Uggg * Combined Encoder Period and Count PIO Programs The servo is using one of the PIOs, so there isn't enough room to have 4 separate state machines running for both the Encoder tick counter and the encoder period counter. This change replaces the encoder PIO program with one that will output the time between encoder pulses to its RX FIFO. Instead of having the PIO program keep track of the encoder tick count, the robot code will count the number of items it pulls from the RX FIFO (there will be one for each encoder tick) and keep track of the count itself. Only the encoder period (time between pulses) will be calculated in the PIO. * Added Copyright Notice and License to New Files * Fixed Up Comment * Fix Up Comments and Error Logging * Add Encoder Enable/Disable and Other Cleanup * Add Code to Average Consecutive Periods Encoder Channel A and B are not 90 degrees out of phase; therefore, at minimum, 2 consecutive periods have to be averaged together to obtain the current encoder speed. After some experimentation, I got the best results if I averaged 8 consecutive periods together, so I've set that as the default and added a function to change the number of samples to average in case someone wants to make it configurable in the future. * Remove Unused Experimental Code
1 parent a5ad9a9 commit 618db67

9 files changed

+673
-103
lines changed

‎include/encoder.h

+148
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
/* The Encoder class reads encoder periods calculated by the
2+
encoder2 pio program, stores the calculated encoder periods
3+
and keeps track of encoder tick count.
4+
5+
Copyright (C) 2024 Brian LePage
6+
7+
Redistribution and use in source and binary forms, with or without
8+
modification, are permitted provided that the following conditions
9+
are met:
10+
1. Redistributions of source code must retain the above copyright
11+
notice, this list of conditions and the following disclaimer.
12+
2. Redistributions in binary form must reproduce the above copyright
13+
notice, this list of conditions and the following disclaimer in the
14+
documentation and/or other materials provided with the distribution.
15+
3. The name of the author may not be used to endorse or promote products
16+
derived from this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
19+
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
20+
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
21+
IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
22+
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
23+
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24+
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25+
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26+
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
27+
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
*/
29+
30+
#pragma once
31+
#include <Arduino.h>
32+
#include <limits>
33+
#include <vector>
34+
#include "encoder2.pio.h"
35+
36+
namespace xrp {
37+
38+
class Encoder {
39+
public:
40+
41+
Encoder() : period_queue(samples_to_average()) {}
42+
43+
/****************************************************************
44+
*
45+
* Encoder::init()
46+
* Initialize PIO program that measures encoder period.
47+
*
48+
* Returns true on sucess.
49+
*
50+
*****************************************************************/
51+
bool init(const int pin);
52+
53+
/****************************************************************
54+
*
55+
* Encoder::enable()
56+
* Enable the encoder.
57+
*
58+
*
59+
*****************************************************************/
60+
void enable();
61+
62+
/****************************************************************
63+
*
64+
* Encoder::disable()
65+
* Disable the encoder.
66+
*
67+
*
68+
*****************************************************************/
69+
void disable();
70+
71+
/****************************************************************
72+
*
73+
* Encoder::update()
74+
* Get the latest encoder period(s) from the PIO if available
75+
* store the latest, and updated the tick count.
76+
*
77+
* Returns number of samples that were retrieved.
78+
*
79+
*****************************************************************/
80+
int update();
81+
82+
/****************************************************************
83+
*
84+
* Encoder::setSamplesToAverage()
85+
* Set the number of Encoder period samples to average together to
86+
* calculate period.
87+
*
88+
*****************************************************************/
89+
90+
void setSamplesToAverage(const int n);
91+
92+
/****************************************************************
93+
*
94+
* Encoder::getPeriod()
95+
* Return the period calculated by the PIO in the following format:
96+
* 31 1 0
97+
* | Period in 16-cycle ticks | dir |
98+
*
99+
* This is the same format return by the PIO.
100+
*
101+
*****************************************************************/
102+
uint getPeriod();
103+
104+
/****************************************************************
105+
*
106+
* Encoder::getCount()
107+
* Return the number of encoder ticks since the robot started.
108+
*
109+
*****************************************************************/
110+
int getCount() const;
111+
112+
/****************************************************************
113+
*
114+
* Encoder::getDivisor()
115+
* Get divisor for encoder period in ticks per second.
116+
*
117+
*****************************************************************/
118+
119+
static constexpr uint getDivisor() {
120+
return F_CPU / encoder2_CYCLES_PER_COUNT;
121+
}
122+
123+
private:
124+
uint period = 0;
125+
int period_fraction = 0;
126+
uint saved_period = UINT_MAX;
127+
bool direction = true;
128+
bool saved_direction = true;
129+
int samples_to_average_shift = 3;
130+
int sample_count = 0;
131+
int sample_index = 0;
132+
std::vector<uint> period_queue;
133+
uint count = 0;
134+
int StateMachineIdx = -1;
135+
unsigned long last_sample_time = 0;
136+
PIO PioInstance = nullptr;
137+
int pin = 0;
138+
int offset = -1;
139+
bool enabled = false;
140+
void clearPeriodQueue();
141+
uint getFraction(const uint count) const;
142+
uint getWholeNumber(const uint count) const;
143+
constexpr uint samples_to_average() const {
144+
return 1 << samples_to_average_shift;
145+
}
146+
};
147+
148+
}

‎include/encoder2.pio.h

+82
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
// -------------------------------------------------- //
2+
// This file is autogenerated by pioasm; do not edit! //
3+
// -------------------------------------------------- //
4+
5+
#pragma once
6+
7+
#if !PICO_NO_HARDWARE
8+
#include "hardware/pio.h"
9+
#endif
10+
11+
// -------- //
12+
// encoder2 //
13+
// -------- //
14+
15+
#define encoder2_wrap_target 16
16+
#define encoder2_wrap 31
17+
18+
#define encoder2_VERSION 102
19+
#define encoder2_CYCLES_PER_COUNT 16
20+
21+
static const uint16_t encoder2_program_instructions[] = {
22+
0x0b10, // 0: jmp 16 [11]
23+
0x0315, // 1: jmp 21 [3]
24+
0x0318, // 2: jmp 24 [3]
25+
0x0b11, // 3: jmp 17 [11]
26+
0x0318, // 4: jmp 24 [3]
27+
0x0b10, // 5: jmp 16 [11]
28+
0x0b11, // 6: jmp 17 [11]
29+
0x0315, // 7: jmp 21 [3]
30+
0x0315, // 8: jmp 21 [3]
31+
0x0b11, // 9: jmp 17 [11]
32+
0x0b10, // 10: jmp 16 [11]
33+
0x0318, // 11: jmp 24 [3]
34+
0x0b11, // 12: jmp 17 [11]
35+
0x0318, // 13: jmp 24 [3]
36+
0x0315, // 14: jmp 21 [3]
37+
0x0b10, // 15: jmp 16 [11]
38+
// .wrap_target
39+
0x0052, // 16: jmp x--, 18
40+
0xe020, // 17: set x, 0
41+
0x4002, // 18: in pins, 2
42+
0xa0e6, // 19: mov osr, isr
43+
0x60a4, // 20: out pc, 4
44+
0x0079, // 21: jmp !y, 25
45+
0xa04a, // 22: mov y, !y
46+
0x0019, // 23: jmp 25
47+
0x0076, // 24: jmp !y, 22
48+
0xa0e6, // 25: mov osr, isr
49+
0xa0c9, // 26: mov isr, !x
50+
0x4041, // 27: in y, 1
51+
0x8000, // 28: push noblock
52+
0x60c2, // 29: out isr, 2
53+
0xa0eb, // 30: mov osr, !null
54+
0x603f, // 31: out x, 31
55+
// .wrap
56+
};
57+
58+
#if !PICO_NO_HARDWARE
59+
static const struct pio_program encoder2_program = {
60+
.instructions = encoder2_program_instructions,
61+
.length = 32,
62+
.origin = 0,
63+
};
64+
65+
static inline pio_sm_config encoder2_program_get_default_config(uint offset) {
66+
pio_sm_config c = pio_get_default_sm_config();
67+
sm_config_set_wrap(&c, offset + encoder2_wrap_target, offset + encoder2_wrap);
68+
return c;
69+
}
70+
71+
static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint base_pin) {
72+
pio_sm_config c = encoder2_program_get_default_config(offset);
73+
sm_config_set_in_pins(&c, base_pin);
74+
sm_config_set_in_shift(&c, false, false, 32);
75+
sm_config_set_out_shift(&c, true, false, 32);
76+
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX);
77+
pio_sm_init(pio, sm, offset, &c);
78+
pio_sm_set_enabled(pio, sm, true);
79+
}
80+
81+
#endif
82+

‎include/robot.h

+1-3
Original file line numberDiff line numberDiff line change
@@ -62,10 +62,8 @@ void robotSetEnabled(bool enabled);
6262

6363
// Encoder Related
6464
void configureEncoder(int deviceId, int chA, int chB);
65-
int readEncoder(int deviceId);
6665
int readEncoderRaw(int rawDeviceId);
67-
void resetEncoder(int deviceId);
68-
std::vector<std::pair<int,int> > getActiveEncoderValues();
66+
uint readEncoderPeriod(int rawDeviceId);
6967

7068
// PWM Related
7169
void setPwmValue(int wpilibChannel, double value);

‎include/wpilibudp.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ bool dsWatchdogActive();
1515
bool processPacket(char* buffer, int size);
1616
void resetState();
1717

18-
int writeEncoderData(int deviceId, int count, char* buffer, int offset = 0);
18+
int writeEncoderData(int deviceId, int count, unsigned period, unsigned divisor, char* buffer, int offset = 0);
1919
int writeDIOData(int deviceId, bool value, char* buffer, int offset = 0);
2020
int writeGyroData(float rates[3], float angles[3], char* buffer, int offset = 0);
2121
int writeAccelData(float accels[3], char* buffer, int offset = 0);

0 commit comments

Comments
 (0)