157 lines
3.7 KiB
C
157 lines
3.7 KiB
C
#define halBlueLeft 2
|
|
#define halYellowLeft 22
|
|
#define halGreenLeft 19
|
|
|
|
#define halBlueRight 23
|
|
#define halYellowRight 18
|
|
#define halGreenRight 5
|
|
|
|
#define FORWARD 0
|
|
#define BACKWARD 1
|
|
|
|
#define wheelCircumference 26.062
|
|
#define pulsesPerRotation 22
|
|
|
|
#include "driver/pcnt.h"
|
|
|
|
TaskHandle_t rpmTaskHandle;
|
|
float rpmRight = 0;
|
|
float rpmLeft = 0;
|
|
|
|
int lastState[2] = {1, 1};
|
|
int16_t counter[2] = {0, 0};
|
|
int direction[2] = {0, 0};
|
|
|
|
long rpmTimer = 0;
|
|
|
|
// Zero states are non existant
|
|
int motorPosition[2][2][2] {
|
|
{
|
|
{0, 1},
|
|
{3, 2}
|
|
},
|
|
{
|
|
{5, 6},
|
|
{4, 0}
|
|
}
|
|
};
|
|
// old state, new state
|
|
int relativeLookup[6][6] {
|
|
{ 0, 1, 2, 3,-2,-1}, // 1
|
|
{-1, 0, 1, 2, 3,-1}, // 2
|
|
{-2,-1, 0, 1, 2, 3}, // 3
|
|
{-3,-2,-1, 0, 1, 2}, // 4
|
|
{ 1,-3,-2,-1, 0, 1}, // 5
|
|
{ 1, 2,-3,-2,-1, 0} // 6
|
|
};
|
|
|
|
#define PCNT_UNIT_LEFT PCNT_UNIT_0
|
|
#define PCNT_UNIT_RIGHT PCNT_UNIT_1
|
|
|
|
|
|
int getState(bool isLeft) {
|
|
if(isLeft) {
|
|
return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)];
|
|
} else {
|
|
return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)];
|
|
}
|
|
}
|
|
|
|
void initRPMPins() {
|
|
pinMode(halBlueLeft, INPUT_PULLUP);
|
|
pinMode(halYellowLeft, INPUT_PULLUP);
|
|
pinMode(halGreenLeft, INPUT_PULLUP);
|
|
|
|
pinMode(halBlueRight, INPUT_PULLUP);
|
|
pinMode(halYellowRight, INPUT_PULLUP);
|
|
pinMode(halGreenRight, INPUT_PULLUP);
|
|
|
|
// init counter
|
|
pcnt_config_t pcnt_config_left = {
|
|
.pulse_gpio_num = halBlueLeft,
|
|
.ctrl_gpio_num = PCNT_PIN_NOT_USED,
|
|
.lctrl_mode = PCNT_MODE_KEEP,
|
|
.hctrl_mode = PCNT_MODE_KEEP,
|
|
.pos_mode = PCNT_COUNT_INC, // count both rising and falling edges
|
|
.neg_mode = PCNT_COUNT_INC,
|
|
.counter_h_lim = 32767,
|
|
.counter_l_lim = -1,
|
|
.unit = PCNT_UNIT_LEFT,
|
|
.channel = PCNT_CHANNEL_0
|
|
};
|
|
|
|
pcnt_config_t pcnt_config_right = {
|
|
.pulse_gpio_num = halBlueRight,
|
|
.ctrl_gpio_num = PCNT_PIN_NOT_USED,
|
|
.lctrl_mode = PCNT_MODE_KEEP,
|
|
.hctrl_mode = PCNT_MODE_KEEP,
|
|
.pos_mode = PCNT_COUNT_INC, // count both rising and falling edges
|
|
.neg_mode = PCNT_COUNT_INC,
|
|
.counter_h_lim = 32767,
|
|
.counter_l_lim = -1,
|
|
.unit = PCNT_UNIT_RIGHT,
|
|
.channel = PCNT_CHANNEL_0
|
|
};
|
|
|
|
pcnt_unit_config(&pcnt_config_left);
|
|
pcnt_unit_config(&pcnt_config_right);
|
|
|
|
|
|
pcnt_counter_pause(PCNT_UNIT_LEFT);
|
|
pcnt_counter_pause(PCNT_UNIT_RIGHT);
|
|
|
|
pcnt_counter_clear(PCNT_UNIT_LEFT);
|
|
pcnt_counter_clear(PCNT_UNIT_RIGHT);
|
|
|
|
pcnt_counter_resume(PCNT_UNIT_LEFT);
|
|
pcnt_counter_resume(PCNT_UNIT_RIGHT);
|
|
|
|
}
|
|
|
|
void measureRpm(void * parameter) {
|
|
|
|
while(true) {
|
|
if(millis()-rpmTimer>250) {
|
|
// read counters
|
|
pcnt_get_counter_value(PCNT_UNIT_LEFT, &counter[1]);
|
|
pcnt_get_counter_value(PCNT_UNIT_RIGHT, &counter[0]);
|
|
|
|
pcnt_counter_clear(PCNT_UNIT_LEFT);
|
|
pcnt_counter_clear(PCNT_UNIT_RIGHT);
|
|
|
|
|
|
rpmRight = (counter[0]/pulsesPerRotation)*4;
|
|
rpmLeft = (counter[1]/pulsesPerRotation)*4;
|
|
kmh = (wheelCircumference/100) * (rpmRight+rpmLeft)/2 * 3600/1000;
|
|
//Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
|
|
sendSpeed = abs(floor(kmh));
|
|
sendSpeedDecimals = (kmh - sendSpeed) * 100;
|
|
counter[true] = 0;
|
|
counter[false] = 0;
|
|
rpmTimer = millis();
|
|
}
|
|
|
|
int state = getState(0);
|
|
if(lastState[0] != state) {
|
|
if(relativeLookup[lastState[0]-1][state-1] > 0) {
|
|
direction[0] = FORWARD;
|
|
} else {
|
|
direction[0] = BACKWARD;
|
|
}
|
|
lastState[0] = state;
|
|
}
|
|
|
|
state = getState(1);
|
|
if(lastState[1] != state) {
|
|
if(relativeLookup[lastState[1]-1][state-1] > 0) {
|
|
direction[1] = FORWARD;
|
|
} else {
|
|
direction[1] = BACKWARD;
|
|
}
|
|
|
|
lastState[1] = state;
|
|
}
|
|
|
|
vTaskDelay(1 / portTICK_PERIOD_MS);
|
|
}
|
|
} |