Gab_S
/
Slam_cars_stuff_cs161083_cs141296_cs131083
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

Update 'Autodriving/motortest_first.ino'

master
Gab_S 5 years ago
parent
15b3760be4
commit
a62584b0df
1 changed files with 150 additions and 127 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 277
      Autodriving/motortest_first.ino

277
Autodriving/motortest_first.ino
View File

@ -1,127 +1,150 @@
#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;
}
//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)
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
}

Write Preview
Loading…
Cancel
Save