Gab_S
5 years ago
1 changed files with 0 additions and 151 deletions
@ -1,151 +0,0 @@ |
|||
|
|||
//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 <NewPing.h> |
|||
#include <DHT.h> |
|||
|
|||
#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)
|
|||
// (also you can't get readings more often than 2 seconds from this sensor)
|
|||
|
|||
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
|
|||
} |
Loading…
Reference in new issue