You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

157 lines
3.7 KiB

#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);
}
}