diff --git a/Autodriving/motortest_first.ino b/Autodriving/motortest_first.ino index 1ab2944..e87f469 100644 --- a/Autodriving/motortest_first.ino +++ b/Autodriving/motortest_first.ino @@ -1,127 +1,150 @@ - -#include "Motorz.h"; -#include "tankdrive.h"; -#include -#include - -#define DHTPIN 2 -#define iterations 3 -#define SONAR_L_PIN 3 -#define SONAR_M_PIN 4 -#define SONAR_R_PIN 5 -#define MAX_DISTANCE 400 - -NewPing sonarL(SONAR_L_PIN,SONAR_L_PIN, MAX_DISTANCE); -NewPing sonarM(SONAR_M_PIN,SONAR_M_PIN, MAX_DISTANCE); -NewPing sonarR(SONAR_R_PIN,SONAR_R_PIN, MAX_DISTANCE); -DHT dht(DHTPIN, DHT11); - -float soundcm; -float temp=20.0; -float hum=50.0; -float duration; - -unsigned long temp_startcnt; -unsigned long temp_currcnt; -int i; -int dist[3]; -int enr=10; -int in1=11; -int in2=12; -int directr=1; -int enl=9; -int in3=7; -int in4=8; -int directl=0; - -Motorz motorR(enr,in1,in2,directr); -Motorz motorL(enl,in3,in4,directl); -TankDrive mytank(&motorR,directr,&motorL,directl); - -void setup(){ - temp_startcnt=millis(); - dht.begin(); - Serial.begin(115200); - Serial.println("Setup"); - motorR.SetSpeed(200); - motorL.SetSpeed(60); - soundcm = 331.4 + (0.606 * temp) + (0.0124 * hum) ; - soundcm=soundcm/10000; - - } - - -void loop() -{ - temp_currcnt=millis(); - if((temp_currcnt-temp_startcnt)>=30000){ - GetTemps(); - Serial.println(); - Serial.println(); - Serial.print("temps aquaried "); - Serial.print(millis()); - Serial.println(); - Serial.println(); - } - - GetDistances(); - if(dist[1]>5){ - Serial.print("Moving forward\n"); - mytank.MoveForward(); - } - else{ - Serial.print("Object detected\n"); - mytank.Stop(); - } - - /* - mytank.MoveForward(); - delay(5000); - mytank.Stop(); - delay(2000); - mytank.TurnRight(); - delay(5000); - mytank.Stop(); - delay(2000); - mytank.MoveBack(); - delay(5000); - mytank.Stop(); - delay(2000); - */ -} - -void GetDistances(){ - /*duration = sonarL.ping_median(iterations); - dist[0]=(duration / 2) * soundcm; - duration = sonarM.ping_median(iterations); - dist[1]=(duration / 2) * soundcm; - duration = sonarR.ping_median(iterations); - dist[2]=(duration / 2) * soundcm; */ - duration = sonarL.ping(); - dist[0]=(duration / 2) * soundcm; - delay(100); - duration = sonarM.ping(); - dist[1]=(duration / 2) * soundcm; - delay(100); - duration = sonarR.ping(); - dist[2]=(duration / 2) * soundcm; - delay(100); - Serial.print("Distance L:"); - Serial.print(dist[0]); - Serial.print(" M:"); - Serial.print(dist[1]); - Serial.print(" R:"); - Serial.print(dist[2]); - Serial.print(" Temp:"); - Serial.print(temp); - Serial.print(" Humidity:"); - Serial.print(hum); - Serial.println(); -} - -void GetTemps(){ - hum = dht.readHumidity(); // Get Humidity value - temp= dht.readTemperature(); // Get Temp value - temp_startcnt=temp_currcnt; - -} + +//very crude libraries with objects for motor and tandrive system (2 motors no steering) +//tandrive is an object that you attach the motor objects you make bellow made some functions +//in hope it will be more easier to read etc. + +#include "Motorz.h"; +#include "tankdrive.h"; +#include +#include + +#define DHTPIN 2 // temp humidity sensor pin (why? sound speed changes according to temp/humidity) +#define iterations 3 // can be used with function ping_median from new ping (takes median value ,unused) +#define SONAR_L_PIN 3 //left sonar sensor 3 pin configuration (trig&echo) are bound to 1 pin +#define SONAR_M_PIN 4 // middle sonar sensor +#define SONAR_R_PIN 5 // right sonar sensor +#define MAX_DISTANCE 400 //max sonar dinstance according to the type of sensor i use Hc-SR04 + +NewPing sonarL(SONAR_L_PIN,SONAR_L_PIN, MAX_DISTANCE); +NewPing sonarM(SONAR_M_PIN,SONAR_M_PIN, MAX_DISTANCE); +NewPing sonarR(SONAR_R_PIN,SONAR_R_PIN, MAX_DISTANCE); + +DHT dht(DHTPIN, DHT11); + +float soundcm; +float temp=20.0; +float hum=50.0; +float duration; + +//calculating speed of sound uses floats and multiplications we don't want to bog down +// our already slow and weak cpu so i use a the variables bellow along with function millis() +// to take measurments every half minute (still too often in my opinion) + +unsigned long temp_startcnt; +unsigned long temp_currcnt; + +int i; +int dist[3]; //table that holds the distances the sensors measured + + //using an L298N motor driver module + +int enr=10; // enable pin for motor r (must be pwm) +int in1=11; // pin for one of the two connections of the mottor +int in2=12;// other connection (these two are digital pins) +// depends on how you end up wiring and placing the motors it will spin a certain way +// and another way when you reverse the voltage , direct holds (0,1) depending on how you +// wired the motor to go "forwards" what ever that means for your project +int directr=1; +int enl=9; +int in3=7; +int in4=8; +int directl=0; + +Motorz motorR(enr,in1,in2,directr); +Motorz motorL(enl,in3,in4,directl); + +TankDrive mytank(&motorR,directr,&motorL,directl); + +void setup(){ + temp_startcnt=millis(); //temp&humidity counter + dht.begin(); //it needs this + Serial.begin(115200); //serial monitor for debugging + Serial.println("Setup"); + motorR.SetSpeed(200); //im using diffrent motors (trying to simulate wheels so i figure things out ) + motorL.SetSpeed(60); // so that explains the diffrent speed in each motor + soundcm = 331.4 + (0.606 * temp) + (0.0124 * hum) ; + soundcm=soundcm/10000; //speed of sound per cm + + } + + +void loop() +{ + temp_currcnt=millis(); + if((temp_currcnt-temp_startcnt)>=30000){ + GetTemps(); + Serial.println(); + Serial.println(); + Serial.print("temps aquaried "); + Serial.print(millis()); + Serial.println(); + Serial.println(); + } + //for now it only detects things from the front sensor but + // with only few lines and ifs you can make it better + // moves forward until there's something 5cm in front of it + // carefull this sonar sensors aren't that accurate in < 6 cm distances in my opinion + + GetDistances(); + if(dist[1]>5){ + Serial.print("Moving forward\n"); + mytank.MoveForward(); + } + else{ + Serial.print("Object detected\n"); + mytank.Stop(); + } + + /* + mytank.MoveForward(); + delay(5000); + mytank.Stop(); + delay(2000); + mytank.TurnRight(); + delay(5000); + mytank.Stop(); + delay(2000); + mytank.MoveBack(); + delay(5000); + mytank.Stop(); + delay(2000); + */ +} + +void GetDistances(){ + /*duration = sonarL.ping_median(iterations); + dist[0]=(duration / 2) * soundcm; + duration = sonarM.ping_median(iterations); + dist[1]=(duration / 2) * soundcm; + duration = sonarR.ping_median(iterations); + dist[2]=(duration / 2) * soundcm; */ + + duration = sonarL.ping(); //sending supersonic sound wave and measuring the time it takes to come back + dist[0]=(duration / 2) * soundcm; // devide by 2 (hit object came back ) and multiply by speed of sound + delay(100); //the delay is crusial since we're firing one sensor after another + duration = sonarM.ping(); //we don't want noise from one sonar waver of one sensors affecting another + dist[1]=(duration / 2) * soundcm; + delay(100); + duration = sonarR.ping(); / + dist[2]=(duration / 2) * soundcm; + delay(100); + Serial.print("Distance L:"); + Serial.print(dist[0]); + Serial.print(" M:"); + Serial.print(dist[1]); + Serial.print(" R:"); + Serial.print(dist[2]); + Serial.print(" Temp:"); + Serial.print(temp); + Serial.print(" Humidity:"); + Serial.print(hum); + Serial.println(); +} + +void GetTemps(){ + hum = dht.readHumidity(); // Get Humidity value + temp= dht.readTemperature(); // Get Temp value + temp_startcnt=temp_currcnt; + soundcm = 331.4 + (0.606 * temp) + (0.0124 * hum) ; + soundcm=soundcm/10000; //speed of sound per cm +}