You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
128 lines
2.7 KiB
128 lines
2.7 KiB
5 years ago
|
|
||
|
|
#include "Motorz.h";
|
||
|
|
#include "tankdrive.h";
|
||
|
|
#include <NewPing.h>
|
||
|
|
#include <DHT.h>
|
||
|
|
|
||
|
|
#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;
|
||
|
|
|
||
|
|
}
|