Slight tweaks to encoder PIO

Author: zhiquanyeo

include/encoder2.pio.h

@@ -20,38 +20,38 @@
 
 static const uint16_t encoder2_program_instructions[] = {
     0x0b10, //  0: jmp    16                     [11]
-    0x0315, //  1: jmp    21                     [3] 
-    0x0318, //  2: jmp    24                     [3] 
+    0x0315, //  1: jmp    21                     [3]
+    0x0318, //  2: jmp    24                     [3]
     0x0b11, //  3: jmp    17                     [11]
-    0x0318, //  4: jmp    24                     [3] 
+    0x0318, //  4: jmp    24                     [3]
     0x0b10, //  5: jmp    16                     [11]
     0x0b11, //  6: jmp    17                     [11]
-    0x0315, //  7: jmp    21                     [3] 
-    0x0315, //  8: jmp    21                     [3] 
+    0x0315, //  7: jmp    21                     [3]
+    0x0315, //  8: jmp    21                     [3]
     0x0b11, //  9: jmp    17                     [11]
     0x0b10, // 10: jmp    16                     [11]
-    0x0318, // 11: jmp    24                     [3] 
+    0x0318, // 11: jmp    24                     [3]
     0x0b11, // 12: jmp    17                     [11]
-    0x0318, // 13: jmp    24                     [3] 
-    0x0315, // 14: jmp    21                     [3] 
+    0x0318, // 13: jmp    24                     [3]
+    0x0315, // 14: jmp    21                     [3]
     0x0b10, // 15: jmp    16                     [11]
             //     .wrap_target
-    0x0052, // 16: jmp    x--, 18                    
-    0xe020, // 17: set    x, 0                       
-    0x4002, // 18: in     pins, 2                    
-    0xa0e6, // 19: mov    osr, isr                   
-    0x60a4, // 20: out    pc, 4                      
-    0x0079, // 21: jmp    !y, 25                     
-    0xa04a, // 22: mov    y, !y                      
-    0x0019, // 23: jmp    25                         
-    0x0076, // 24: jmp    !y, 22                     
-    0xa0e6, // 25: mov    osr, isr                   
-    0xa0c9, // 26: mov    isr, !x                    
-    0x4041, // 27: in     y, 1                       
-    0x8000, // 28: push   noblock                    
-    0x60c2, // 29: out    isr, 2                     
-    0xa0eb, // 30: mov    osr, !null                 
-    0x603f, // 31: out    x, 31                      
+    0x0052, // 16: jmp    x--, 18
+    0xe020, // 17: set    x, 0
+    0x4002, // 18: in     pins, 2
+    0xa0e6, // 19: mov    osr, isr
+    0x60a4, // 20: out    pc, 4
+    0x0079, // 21: jmp    !y, 25
+    0xa04a, // 22: mov    y, !y
+    0x0019, // 23: jmp    25
+    0x0076, // 24: jmp    !y, 22
+    0xa0e6, // 25: mov    osr, isr
+    0xa0c9, // 26: mov    isr, !x
+    0x4041, // 27: in     y, 1
+    0x8000, // 28: push   noblock
+    0x60c2, // 29: out    isr, 2
+    0xa0eb, // 30: mov    osr, !null
+    0x603f, // 31: out    x, 31
             //     .wrap
 };
 
@@ -69,6 +69,9 @@ static inline pio_sm_config encoder2_program_get_default_config(uint offset) {
 }
 
 static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint base_pin) {
+    pio_gpio_init(pio, base_pin);
+    pio_gpio_init(pio, base_pin+1);
+    pio_sm_set_consecutive_pindirs(pio, sm, base_pin, 2, false);
     pio_sm_config c = encoder2_program_get_default_config(offset);
     sm_config_set_in_pins(&c, base_pin);
     sm_config_set_in_shift(&c, false, false, 32);
@@ -79,4 +82,3 @@ static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint bas
 }
 
 #endif
-

platformio.ini

@@ -1,5 +1,5 @@
 [env]
-platform = https://github.com/maxgerhardt/platform-raspberrypi.git#088a074ba98e091f9abc48db91b99bedf908332b
+platform = https://github.com/maxgerhardt/platform-raspberrypi.git#39b90392af50585e429941bd2561a91949a2ba46
 framework = arduino
 board_build.core = earlephilhower
 board_build.filesystem_size = 0.5m

src/encoder.cpp

@@ -40,15 +40,15 @@ static PIOProgram encoderProgram(&encoder2_program);
 *     Initialize PIO program that measures encoder period.
 *
 *  Returns true on sucess.
-*  
+*
 *****************************************************************/
 
 bool Encoder::init(const int pin) {
   last_sample_time = millis();
 
   this->pin = pin;
 
-  if (!encoderProgram.prepare(&PioInstance, &StateMachineIdx, &offset)) {
+  if (!encoderProgram.prepare(&PioInstance, &StateMachineIdx, &offset, pin, 2)) {
     return false;
   }
 
@@ -60,7 +60,7 @@ bool Encoder::init(const int pin) {
 *  Encoder::enable()
 *     Enable the encoder.
 *
-*  
+*
 *****************************************************************/
 
 void Encoder::enable() {
@@ -75,7 +75,7 @@ void Encoder::enable() {
 *  Encoder::disable()
 *     Disable the encoder.
 *
-*  
+*
 *****************************************************************/
 
 void Encoder::disable() {
@@ -90,14 +90,14 @@ void Encoder::disable() {
 *  Encoder::update()
 *     Get the latest encoder period(s) from the PIO if available
 *     store the latest, and updated the tick count.
-*  
+*
 *  Returns number of samples that were retrieved.
-*  
+*
 *****************************************************************/
 
 uint Encoder::getFraction(const uint count) const {
   return count & (samples_to_average() - 1);
-} 
+}
 
 uint Encoder::getWholeNumber(const uint count) const {
   return count >> samples_to_average_shift;
@@ -147,7 +147,7 @@ int Encoder::update() {
       ++period;
       period_fraction = getFraction(period_fraction);
     }
-    period_queue[sample_index++] = raw_rx_fifo; 
+    period_queue[sample_index++] = raw_rx_fifo;
     ++sample_count;
     if(sample_index == samples_to_average())
       sample_index = 0;
@@ -191,7 +191,7 @@ void Encoder::setSamplesToAverage(const int n) {
 *     Return the period calculated by the PIO in the following format:
 *     31                           1   0
 *    |  Period in 16-cycle ticks    | dir |
-*  
+*
 *    This is the same format return by the PIO.
 *    (Note: PIO calculated value may be overidden if a relatively
 *     large amount of time has passed since the last sample from the PIO).
@@ -228,7 +228,7 @@ uint Encoder::getPeriod() {
   //If PIO value is correct, the amount of time that has passed since the last sample
   //should be less than or equal to period calculated by the PIO.
   //Otherwise, motor may have slowed way down; use the time passed for the current period length.
-  
+
   //If time since last sample is significantly longer than the last period returned by the PIO,
   //use the time passed instead.
 

src/encoder2.pio

@@ -29,14 +29,14 @@
 .program encoder2
 .origin 0
 
-.define PUBLIC VERSION 102 
+.define PUBLIC VERSION 102
 .define PUBLIC CYCLES_PER_COUNT 16
 
 ; This PIO program monitors the encoder inputs to measure the period
 ; between encoder pulses.  At the start of a period, a 31-bit counter is
 ; initialized to its maximum value (0x7FFFFFFF) and is decremented every 16 cycles
 ; until the start of the next period.  The period start/end is determined
-; by the transition of the encoder signals from one state to another (e.g., 01 -> 11 or 01 -> 00) 
+; by the transition of the encoder signals from one state to another (e.g., 01 -> 11 or 01 -> 00)
 
 ; Every period, the total count is passed to the robot code through the RX FIFO.
 ; The upper 31 bits is the count.  It represents the number of 16-cycle intervals
@@ -117,11 +117,15 @@ send_period:
    push noblock        ;send period/direction to robot code
    out isr,2           ;restore last state to isr
    mov osr,~null       ;set all bits in osr
-   out x, 31           ;set 31 bits of x register        
+   out x, 31           ;set 31 bits of x register
 .wrap
 
 % c-sdk {
 static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint base_pin) {
+    pio_gpio_init(pio, base_pin);
+    pio_gpio_init(pio, base_pin+1);
+    pio_sm_set_consecutive_pindirs(pio, sm, base_pin, 2, false);
+
     pio_sm_config c = encoder2_program_get_default_config(offset);
 
     sm_config_set_in_pins(&c, base_pin);