89 lines
2.0 KiB
C
89 lines
2.0 KiB
C
#define halBlueLeft 2
|
|
#define halYellowLeft 22
|
|
#define halGreenLeft 19
|
|
|
|
#define halBlueRight 23
|
|
#define halYellowRight 18
|
|
#define halGreenRight 5
|
|
|
|
#define wheelCircumference 26.062
|
|
|
|
TaskHandle_t rpmTaskHandle;
|
|
int rpmRight = 0;
|
|
int rpmLeft = 0;
|
|
|
|
int lastState[2] = {1, 1};
|
|
long lastTime[2] = {0, 0};
|
|
int counter[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
|
|
};
|
|
|
|
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);
|
|
}
|
|
|
|
void measureRpm(void * parameter) {
|
|
while(true) {
|
|
if(millis()-rpmTimer>250) {
|
|
rpmRight = constrain(counter[0], 0, 350)*4;
|
|
rpmLeft = constrain(counter[1], 0, 350)*4;
|
|
kmh = (wheelCircumference/100) * (rpmRight+rpmLeft)/2 * 60/1000;
|
|
|
|
sendSpeed = abs(floor(kmh));
|
|
sendSpeedDecimals = (kmh - sendSpeed) * 100;
|
|
Serial.print("remote "); Serial.println(kmh);
|
|
counter[true] = 0;
|
|
counter[false] = 0;
|
|
rpmTimer = millis();
|
|
}
|
|
|
|
int state = getState(0);
|
|
if(lastState[0] != state) {
|
|
counter[0] = counter[0] + relativeLookup[lastState[0]-1][state-1];
|
|
lastState[0] = state;
|
|
}
|
|
|
|
state = getState(1);
|
|
if(lastState[1] != state) {
|
|
counter[1] = counter[1] + relativeLookup[lastState[1]-1][state-1];
|
|
lastState[1] = state;
|
|
}
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(1));
|
|
}
|
|
} |