Help making my code a little faster and more efficent

johnbrodowski · June 28, 2015, 12:01am

I know I could be going about this all wrong but I'm trying to make and antonymous rover and its not working great, I need some ideas on how to make the code a little more efficient. I have be programming in vb.net for years but I am new to c++.

#include <NewPing.h>
#include <AFMotor.h>
#include <Servo.h> 
AF_Stepper RightMotor(48, 2);
AF_Stepper LeftMotor(48, 1);
Servo myservo;
unsigned long currentMillis = 0;
#define TRIGGER_PIN  15         
#define ECHO_PIN     14         
#define MAX_DISTANCE 200     

unsigned long previousMillisScan = 0;
unsigned long previousMillisPing = 0;
unsigned long previousMillisDrive = 0;
unsigned long intervalScan = 10;
unsigned long intervalPing = 250;
unsigned long intervalDrive = 40;
unsigned long Distance = 6;
bool turning = false;
int pos = 90;
bool ScanningLeft = false;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {
	myservo.attach(9);
	RightMotor.setSpeed(150);
	LeftMotor.setSpeed(150);
}


void loop(){
currentMillis = millis();
DoPing();
	if (turning == false){
		scan();
	}
	if (turning == false){
		drive();
	}
 }


 void scan(){
	 if (currentMillis - previousMillisScan > intervalScan) {
		 previousMillisScan = currentMillis;
		 if (ScanningLeft == true)
		 {
			 pos--;
		 }
		 else
		 {
			 pos++;
		 }

		 if (pos <= 40)
		 {
			 ScanningLeft = false;
			 pos = 40;
		 }
		 else
		 if (pos >= 140){
			 ScanningLeft = true;
			 pos = 140;
		 }
		 myservo.write(pos);
	 }
 }


 void DoPing(){
	 int dis = sonar.ping_cm();
	 if (currentMillis - previousMillisPing > intervalPing) {
		 previousMillisPing = currentMillis;
		 if (dis > 0){
			 Distance = dis;
		 }
	 }
 }


 void drive(){
	 if (currentMillis - previousMillisDrive > intervalDrive) {
		 previousMillisDrive = currentMillis;
		 if (turning == false){
			 if (Distance >= 30){
				 GoForward();
			 }
			 else
			 {
				 turn();
			 }
		 }
	 }
 }


 void GoForward(){
	 RightMotor.step(1, FORWARD, DOUBLE);
	 LeftMotor.step(1, FORWARD, DOUBLE);
 }


 void GoBackward(){
	 RightMotor.step(1, BACKWARD, DOUBLE);
	 LeftMotor.step(1, BACKWARD, DOUBLE);
 }


 void turn(){
	 if (ScanningLeft == true){
		 RightMotor.step(11, FORWARD, DOUBLE);
		 LeftMotor.step(11, BACKWARD, DOUBLE);
	 }
	 else
	 {
		 RightMotor.step(11, BACKWARD, SINGLE);
		 LeftMotor.step(11, FORWARD, SINGLE);
	 }
	 turning = false;
 }

moderator: cleaned up post

MorganS · June 28, 2015, 5:24am

That's definitely not great in the photo. It's all blurry! (Do you have daylight where you are? Put it near a window and get a better photo that way.)

void loop() {
	currentMillis = millis();
		DoPing();
		if (turning == false){
			scan();
		}
		if (turning == false){
			drive();
		}
 }

How does turning become false? If it somehow accidentally gets set to true, then this does not drive and does not scan. DoPing() doesn't seem to be updating that variable.

Robin2 · June 28, 2015, 6:45am

You have not told us what the code actually does and what you would like it to do.

"It's not working great" suggests to me that it is generally doing ALL that it is supposed to do, but not as well as you would like. What improvement in behaviour do you want ?

All variables associated with millis() are best defined as unsigned long. You have some of them as long which can have negative values that may confuse things.

...R

johnbrodowski · June 28, 2015, 4:47pm

making a autonomous rover, right now im working on obstacle avoidance. turning right or left if it needs to avoid something. also kind of curious is my code is well written or if its sloppy .etc

Robin2 · June 28, 2015, 6:37pm

johnbrodowski:
making a autonomous rover, right now im working on obstacle avoidance. turning right or left if it needs to avoid something. also kind of curious is my code is well written or if its sloppy .etc

Your code looks reasonable - but the real test is whether it does what you want. And you have not told us, nor have you told us what you would like it to do better.

Please edit your original post to remove all the irrelevant (and confusing) bold text.

...R

johnbrodowski · June 28, 2015, 7:05pm

Ok basically i just want to get it to turn left or right if it cant go straight. its doing what I want to an extent its just not doing it well.

in the end my goal it to have it roam around on its own. ie turn until it can go forward, remember and retrace a path stuff like that, but for now I'm just trying to get it to do the basics

Robin2 · June 29, 2015, 8:16am

johnbrodowski:
Ok basically i just want to get it to turn left or right if it cant go straight. its doing what I want to an extent its just not doing it well.

Computer programming, and especially debugging, requires much more specific information than "doing what I want to an extent its just not doing it well"

If you can't explain the problem I can't help with a solution.

...R

system · June 29, 2015, 9:06am

Increasing the speed of sound could make your code run faster.

You're dealing with a mechanical system - making your code run faster may make no difference whatsoever to how your device behaves.

johnbrodowski · June 29, 2015, 10:22am

Well currently the main issue I am having is getting it to turn the right way when it approaches an object. Sometimes it turns towards an obstacle and other times it turns away.

system · June 29, 2015, 10:28am

johnbrodowski:
Well currently the main issue I am having is getting it to turn the right way when it approaches an object. Sometimes it turns towards an obstacle and other times it turns away.

And you've got proof that that behaviour is related to the speed and efficiency of the code?

johnbrodowski · June 29, 2015, 10:35am

no not exactly but when i turn it on and set it free the servo for the sonar tends to stop and the stepper will do the same thing and they kind of do it at the same time. It's buggy

system · June 29, 2015, 10:37am

So, fix the bugginess, then make it faster if it needs to be faster.

johnbrodowski · June 29, 2015, 10:46am

Correct

johnbrodowski · June 29, 2015, 10:48am

I'll make a video of it when I get home in a couple hours

Robin2 · June 29, 2015, 5:15pm

The usual way to debug a computer program is to simplify it so there is only one piece being tested - then it will be obvious where the problem is.

...R

johnbrodowski · June 29, 2015, 6:12pm

Yeah, I just went it did it all at once and it got to be a real pain pretty fast

Robin2 · June 29, 2015, 7:45pm

johnbrodowski:
Yeah, I just went it did it all at once and it got to be a real pain pretty fast

I can't figure whether you are expecting a response to that - I don't see a question in it.

If you have made a simplified version of the code please post it and explain what it does and what you would like it to do.

...R

johnbrodowski · June 29, 2015, 10:48pm

Ok i got it working, its not pretty and its still a little buggy but its working none the less.

#include <NewPing.h>
#include <AFMotor.h>
#include <Servo.h> 
AF_Stepper RightMotor(48, 2);
AF_Stepper LeftMotor(48, 1);
Servo myservo;
unsigned long currentMillis = 0;
#define TRIGGER_PIN  15         
#define ECHO_PIN     14         
#define MAX_DISTANCE 200     

unsigned long previousMillisScan = 0;
unsigned long previousMillisPing = 0;
unsigned long previousMillisDrive = 0;
unsigned long intervalScan = 10;
unsigned long intervalPing = 250;
unsigned long intervalDrive = 40;
unsigned long Distance = 6;
bool turning = false;
bool ForwardClear = false;
bool LeftClear = false;
bool RightClear = false;
int pos = 90;
bool ScanningLeft = false;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {
	myservo.attach(9);
	RightMotor.setSpeed(150);
	LeftMotor.setSpeed(150);
}



void loop(){
currentMillis = millis();
DoPing();
	if (turning == false){
		scan();
	}
		 drive(); 
 }

 void scan(){
	 if (currentMillis - previousMillisScan > intervalScan) {
		 previousMillisScan = currentMillis;
		 if (ScanningLeft == false)
		 {
			 pos--;
		 }
		 else
		 {
			 pos++;
		 }
		 if (pos <= 40)
		 {
			 ScanningLeft = true;
			 pos = 40;
		 }
		 else
		 if (pos >= 140){
			 ScanningLeft = false;
			 pos = 140;
		 }
		 else{
		 GoBackward();
		 }
		 myservo.write(pos);
	 }
	 if (pos == 90 )
	 {
		 if (Distance > 20){
		 	 ForwardClear = true;
		  }
		  else{
		 ForwardClear = false;
		  }
	 }
	 else
	 if (pos == 45){
		 if (Distance > 20){
		  LeftClear = true;
			  }
			  else
			  {
			 	 LeftClear = false;
			   GoLeft();
		 }
	 }
	 else
	 if (pos == 140){

		 if (Distance > 20){
			 	 RightClear = true;
			 }
			 else{
			 	 RightClear = false;
		  GoRight();
		 }
	 }
 }

 void DoPing(){
	 int dis = sonar.ping_cm();
	 if (currentMillis - previousMillisPing > intervalPing) {
		 previousMillisPing = currentMillis;
		 if (dis > 0){
			 Distance = dis;
		 }
	 }
 }


 void drive(){
	 if (currentMillis - previousMillisDrive > intervalDrive) {
		 previousMillisDrive = currentMillis;
		 if (ForwardClear == true){
			 GoForward();
		 }
		 else
		 if (LeftClear == true){
			 GoLeft();
		 }
		 else
		 if (RightClear == true){
			 GoRight();
		 }
	 }
}



 void GoForward(){
	 RightMotor.step(1, FORWARD, DOUBLE);
	 LeftMotor.step(1, FORWARD, DOUBLE);
 }

 void GoBackward(){
	 RightMotor.step(1, BACKWARD, DOUBLE);
	 LeftMotor.step(1, BACKWARD, DOUBLE);
 }

 void GoLeft(){
	 RightMotor.step(1, BACKWARD, DOUBLE);
	 LeftMotor.step(1, FORWARD, DOUBLE);
 }

 void GoRight(){
	 RightMotor.step(1, FORWARD, DOUBLE);
	 LeftMotor.step(1, BACKWARD, DOUBLE);
 }

Robin2 · June 30, 2015, 7:25am

Glad to hear you have been successful.

...R

system · June 30, 2015, 7:30am

void DoPing()
{
	int dis = sonar.ping_cm();
	if (currentMillis - previousMillisPing > intervalPing) {
		previousMillisPing = currentMillis;
		if (dis > 0){
			Distance = dis;
		}
  }
}

This doesn't look right - are you sure that's what you intended?

Topic		Replies	Views
CODING HELP General Guidance	4	1220	May 5, 2021
speeding up my code Programming	12	1109	May 5, 2021
Help with Code for Loop Programming	3	1013	May 5, 2021
Someone please check this out and tell me what can I do to perfect it. Robotics	4	2454	May 6, 2021
Compiling problem Programming	9	5102	May 6, 2021

Help making my code a little faster and more efficent

Related topics