Hi, I am trying to build a little robot to clean the dust on the floor in our house.
Based on an Arduino Uno with a CNC shield, 2 stepper motors to drive the 2 wheels, 2 switches to sense nearby walls and an US sensor (HC-SR05) to measure distances to obstacles, battery operated at 12V.
The random targets (distance and angle) together with the distance to obstacle are fed into a fuzzy logic algorithm with an output of 2 speeds, one for the left motor and one for the right motor. These speeds are used to calculate 2 pulse durations that are fed into OCR1A and OCR1B of Timer1. Timer1 is setup in Normal mode with a prescaler of 8. Timer1 is handling 2 ISR routines on compare match OCR1A/B. These ISRs generate 2 pulse trains with different frequencies to result in 2 different motor speeds...and everything works fine...most of the time.
After a number of random targets (mostly around 20) the software start running around wasting time.
In normal operation Timer1 overflows a number of times (between 40 and 80 times) before moving to the next target, but f.e. at target nr 21 it starts overflowing between 200 to 350 times and I can not find any reason for that.
Any suggestion would be appreciated. Thank you.
// Collision Avoidance Swiffer Robot Fuzzy Logic
//
#include "Configuration.h"
#include <NewPing.h>
NewPing sonar(usTrig, usEcho, usMaxD); //setup sonar
#include "PinChangeInterrupt.h"
#include "MeasDist.h"
#include "switchesHR.h"
#include "Fuzzification.h"
#include "Defuzzification.h"
void setup() {
Serial.begin(115200);
noInterrupts ();
TCCR1A = 0; TCCR1B = 0; TIMSK1 = 0;
bitClear(TCCR1A, WGM10);
bitClear(TCCR1A, WGM11);
bitClear(TCCR1B, WGM12);
bitClear(TCCR1B, WGM13); // Normal mode
bitClear(TCCR1B, CS10);
bitSet(TCCR1B, CS11);
bitClear(TCCR1B, CS12); // Prescaler : 8
bitSet(TIFR1, OCF1A);
bitSet(TIFR1, OCF1B);
bitSet(TIFR1, TOV1); // Clear Interrupt Flags
bitSet(TIMSK1, OCIE1A); // Output Compare match Interrupt Enable 1A
bitSet(TIMSK1, OCIE1B); // Output Compare match Interrupt Enable 1B
bitSet(TIMSK1, TOIE1); // Enable Overflow Interrupt
OCR1A = cntLM; // Count for left motor pulses
OCR1B = cntRM; // Count for right motor pulses
TCNT1 = 0; // Timer1 set count to 0
interrupts ();
pinMode(leSwPin, INPUT_PULLUP);
attachPCINT(digitalPinToPCINT(leSwPin),fncMLeftSw, FALLING);
pinMode(riSwPin, INPUT_PULLUP);
attachPCINT(digitalPinToPCINT(riSwPin),fncMRightSw, FALLING);
pinMode(mEnpin, OUTPUT); // Motors Enable pin
pinMode(mXstepP, OUTPUT); // Left stepper motor
pinMode(mXdirP, OUTPUT); // HIGH = forward or LOW= backward
pinMode(mYstepP, OUTPUT); // Right stepper motor
pinMode(mYdirP, OUTPUT); // LOW = forward or HIGH= backward
digitalWrite(mEnpin, LOW); // enable motors
cntLM = 800; incLM = 800;
cntRM = 800; incRM = 800;
randomSeed(analogRead(0));
}
void loop() {
if (nrSt <= 0){
// fncswitchesHR();
fncMeasDist();
dTarget = random(1000,8000);
tTarget = -pi + (2*pi*random(0,10001))/10000;
bitClear(TIMSK1, OCIE1A);
bitClear(TIMSK1, OCIE1B);
fncFuzzification();
fncDefuzzification();
cntLM = int(stDelay/vL); cntRM = int(stDelay/vR);
incLM = cntLM; incRM = cntRM;
nrSt = int(8 * dTarget); //60000;
bitSet(TIMSK1, OCIE1A);
bitSet(TIMSK1, OCIE1B);
Serial.print(dTarget); Serial.print(" "); Serial.print(tTarget); Serial.print(" ");
Serial.print(cntLM); Serial.print(" "); Serial.print(cntRM); Serial.print(" ");
Serial.print(nrSt); Serial.print(" "); Serial.println(overflowCnt);
overflowCnt = 0;
}
}
void fncMLeftSw(){
// reverse run Rmotor 200 steps
if (rightSw == true){
digitalWrite(mYdirP, HIGH);
for (int steps = 0; steps < 200; steps++) {
digitalWrite(mYstepP, HIGH);
delayMicroseconds(stDelay);
digitalWrite(mYstepP, LOW);
delayMicroseconds(stDelay);}
leftSw = true;
}
}
void fncMRightSw(){
// reverse run Lmotor 200 steps
if (leftSw == true){
digitalWrite(mXdirP, LOW);
for (int steps = 0; steps < 200; steps++) {
digitalWrite(mXstepP, HIGH);
delayMicroseconds(stDelay);
digitalWrite(mXstepP, LOW);
delayMicroseconds(stDelay);}
rightSw = true;
}
}
ISR(TIMER1_COMPA_vect){
digitalWrite(mXdirP, HIGH);
pulseLM = !pulseLM;
digitalWrite(mXstepP, pulseLM ? HIGH : LOW);
cntLM = cntLM + incLM;
OCR1A = cntLM;
nrSt--;
}
ISR(TIMER1_COMPB_vect){
digitalWrite(mYdirP, LOW);
pulseRM = !pulseRM;
digitalWrite(mYstepP, pulseRM ? HIGH : LOW);
cntRM = cntRM + incRM;
OCR1B = cntRM;
nrSt--;
}
ISR(TIMER1_OVF_vect){
overflowCnt++;
}
Problem solved