90 lines
2.0 KiB
C
90 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 = counter[false]*4;
|
||
|
rpmLeft = counter[true]*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(false);
|
||
|
if(lastState[false] != state) {
|
||
|
counter[false] = counter[false] + relativeLookup[lastState[false]-1][state-1];
|
||
|
lastState[false] = state;
|
||
|
}
|
||
|
|
||
|
state = getState(true);
|
||
|
if(lastState[true] != state) {
|
||
|
counter[true] = counter[true] + relativeLookup[lastState[true]-1][state-1];
|
||
|
lastState[true] = state;
|
||
|
}
|
||
|
|
||
|
vTaskDelay(pdMS_TO_TICKS(1));
|
||
|
}
|
||
|
}
|