parent
a13936c35e
commit
a17572fa1a
@ -1,7 +1,7 @@
|
||||
//mac addresses
|
||||
|
||||
// reciever mac:
|
||||
uint8_t mac_receiver[] = {0x30, 0xae, 0xa4, 0x05, 0x82, 0x15};
|
||||
uint8_t mac_receiver[] = {0xB4, 0xE6, 0x2D, 0x8C, 0x34, 0xAE};
|
||||
|
||||
// remote macStr
|
||||
uint8_t mac_remote[] = {0x24, 0x0a, 0xc4, 0x82, 0x51, 0x70};
|
||||
|
@ -0,0 +1,90 @@
|
||||
#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));
|
||||
}
|
||||
}
|
Loading…
Reference in new issue