For some reason, the right motor (represented by MR in the identifiers) works only up to the first test of the motors within the setup() body. It stops working from when the comment in the setup() mentions it. In the loop() or any of the other functions, it doesn't work at all. And this is strange also given that this just popped up all of a sudden. This code has worked fine before. And it's nothing electronic, because if I upload another program to test out the motors (back, forwards, varying degrees of speed), they work fine. I've tried to look over everything and I can't find what's wrong. Any suggestions?
#include <NewPing.h> //Header file for HC-SR04 sensor
//Speed for the right and left motors, and the number of turns done, as will be seen later in the code.//
byte speedR=0, speedL=0, turns=0;
#define MR1 0
#define MR2 1
#define ML1 2
#define ML2 3
#define MRspeed 10
#define MLspeed 11
#define SMASHd 20 //Distance for robot to go full-force forward and, as the name states, SMASH!
#define FE 5 //Echo pin of HC-SR04 sensor
#define FT 6 //Trigger pin of HC-SR04 sensor
#define Md 200 //Maximum distance sensor can detect in cm
//Converting the reading to distance, which is (1/147)*2.54
#define cmconv 0.01727891156462585034013605442177
int Fread=0; //Reading from the sensor
NewPing sonarF(FT, FE, Md); //Declaring sensor object.
void setup(){
pinMode(MR1, OUTPUT);
pinMode(MR2, OUTPUT);
pinMode(ML1, OUTPUT);
pinMode(ML2, OUTPUT);
pinMode(MRspeed, OUTPUT);
pinMode(MLspeed, OUTPUT);
digitalWrite(MR1, HIGH);
digitalWrite(MR2, LOW);
digitalWrite(ML1, HIGH);
digitalWrite(ML2, LOW);
delay(1000);
analogWrite(MRspeed, 255); //Testing if motors work.
analogWrite(MLspeed, 255);
delay(1000);
Serial.begin(9600);
Serial.println(sonarF.ping()*cmconv); //Testing if sensor works.
delay(1000);
SMASH(); //Starting here, the right motor no longer works.
STOP();
delay(1000);
turnFollow();
delay(1000);
goBack();
delay(1000);
}
void loop(){
//While the reading from the sensor is still greater than the SMASHd or 0 (noise)...
while(readF() > SMASHd || readF()==0){
if(turns < 3){
/*Quick point turn then forward movement with the turnFollow() function. This is basically repositioning the robot for another sensor check. This repeats until it has sensed an object.*/
turnFollow();
turns++;
/*The turns variable is a measure to ensure the robot doesn't go out of bounds. It rotates and moves forward 3 times, before going back to initial position (not necessarily orientation), which is implemented with the goBack() function.*/
}
else{
goBack();
turns=0;
}
}
SMASH(); //Once SMASHd is met, well... SMASH!
STOP(); //Brakes.
goBack(); //Go back to initial position.
}
int readF(){ //Reading value from sensor.
Fread = sonarF.ping()*cmconv;
Serial.println(Fread); //Debugging stuff.
delay(30);
return(Fread);
}
void turnFollow(){
/*Setting the right motor to go backwards, while the left will continue forwards, and then setting them both off to max speed, causing the robot to rotate about its center quickly. */
digitalWrite(MR1, LOW);
digitalWrite(MR2, HIGH);
delay(20);
analogWrite(MRspeed, 255);
analogWrite(MLspeed, 255);
delay(100);
//Setting the right motor once more forwards, then moving the robot forward a bit.
digitalWrite(MR1, HIGH);
digitalWrite(MR2, LOW);
delay(20);
analogWrite(MRspeed, 255);
analogWrite(MLspeed, 255);
delay(300);
}
void goBack(){
digitalWrite(MR1, LOW); //Setting both motors to go backwards.
digitalWrite(MR2, HIGH);
digitalWrite(ML1, LOW);
digitalWrite(ML2, HIGH);
delay(20);
analogWrite(MRspeed, 255);
analogWrite(MLspeed, 255);
delay(500);
STOP();
digitalWrite(MR1, HIGH);
digitalWrite(MR2, LOW);
digitalWrite(ML1, HIGH);
digitalWrite(ML2, LOW);
delay(20);
}
void SMASH(){
digitalWrite(MR1, HIGH); //Ensuring all motors are set to forward.
digitalWrite(MR2, LOW);
digitalWrite(ML1, HIGH);
digitalWrite(ML2, LOW);
delay(20);
analogWrite(MRspeed, 255);
analogWrite(MLspeed, 255);
delay(1000);
}
void STOP(){
analogWrite(MRspeed, 0);
analogWrite(MLspeed, 0);
}
