what exactly did you do? can you post the code here - before and after your change? would be great, thanksFor me, Just disabling the super Spike eleminator wasnt enough to solve the drunken Problem. Bernards solution did it, but I left the If in the Code, that would only Count high Pin Interrupts and Not Count Low Pin Interrupts too.
attachInterrupt(pinOdometryLeft, OdometryLeftISR, CHANGE);
attachInterrupt(pinOdometryRight, OdometryRightISR, CHANGE);
attachInterrupt(pinMotorMowRpm, OdometryMowISR, CHANGE);
attachInterrupt(pinOdometryLeft, OdometryLeftISR, RISING);
attachInterrupt(pinOdometryRight, OdometryRightISR, RISING);
attachInterrupt(pinMotorMowRpm, OdometryMowISR, CHANGE);
Hello, so today I have tested my Mower the 2nd day after all my changes above.Now I change all what Bernard proposed and add on also:
- disabled SUPER_SPIKE_ELIMINATOR
- AGCM4 stack memory waste workaround: https://github.com/Ardumower/Sunray/commit/42822f658f0279d4c7d1511362c7d23ee086d346
Hope it was correct also to do upper both changes also?
Then I did also the AT+E test and I have to change TICKS_PER_REVOLUTION from 696 / 2 to 2056 / 2 - WOW!!
Interessting has been that right whell moved exaclty 10 turns, but left whell only 9,9 turns.
Nevertheless now Mower is mowing and driving straight on without being drunken, also with speed 0,35!
Now let him do his job and then charge on docking and will see tomorrow if its still ok
... ;-)
So you mean to change this original Code:Dont forget to make the mowmotor Interrupt rising too and Change it to simple Counting code, it wasnt changed in Bernards Last Post.
void OdometryMowISR(){ if (digitalRead(pinMotorMowRpm) == LOW) return; if (millis() < motorMowTicksTimeout) return; // eliminate spikes #ifdef SUPER_SPIKE_ELIMINATOR unsigned long duration = millis() - motorMowTransitionTime; if (duration > 5) duration = 0; motorMowTransitionTime = millis(); motorMowDurationMax = 0.7 * max(motorMowDurationMax, ((float)duration)); motorMowTicksTimeout = millis() + motorMowDurationMax; #else motorMowTicksTimeout = millis() + 1; #endif odomTicksMow++; } |
void OdometryMowISR(){ odomTicksMow++; asm("dsb"); } |
For asm("dsb") i need to say that i don't know if it's mandatory for AGCM4 and the exact functionality , but in fast TEENSY i have made a lot of test and the result is that interrupt work better with this line of code.Bernard knows probably.
void AmBumperDriver::begin(){
pinMode(pinBumperLeft, INPUT_PULLUP);
pinMode(pinBumperRight, INPUT_PULLUP);
attachInterrupt(pinBumperLeft, BumperLeftInterruptRoutine, CHANGE);
attachInterrupt(pinBumperRight, BumperRightInterruptRoutine, CHANGE);
}
// ------------------------------------------------------------------------------------
void BumperLeftInterruptRoutine(){
leftPressed = (digitalRead(pinBumperLeft) == LOW);
}
void BumperRightInterruptRoutine(){
rightPressed = (digitalRead(pinBumperRight) == LOW);
}
void BumperLeftInterruptRoutine(){
leftPressed = true;
}
void BumperRightInterruptRoutine(){
rightPressed = true;