Gab_S
5 years ago
1 changed files with 0 additions and 218 deletions
@ -1,218 +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 <Servo.h> |
|
||||
|
|
||||
#define SONAR_PIN 10 |
|
||||
#define SERVO_PIN 9 |
|
||||
#define MAX_DISTANCE 200 //max sonar dinstance according to the type of sensor i use Hc-SR04 |
|
||||
|
|
||||
NewPing sonar(SONAR_PIN,SONAR_PIN, MAX_DISTANCE); |
|
||||
Servo myservo; |
|
||||
|
|
||||
|
|
||||
|
|
||||
int GetDistances(); |
|
||||
|
|
||||
int i; |
|
||||
int enr=5; // enable pin for motor r (must be pwm) |
|
||||
int in1=4; // pin for one of the two connections of the mottor |
|
||||
int in2=3;// 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=0; |
|
||||
int enl=6; |
|
||||
int in3=7; |
|
||||
int in4=8; |
|
||||
int directl=0; |
|
||||
|
|
||||
char buffer1[(sizeof(char)+sizeof(unsigned long))*sizeof(char)+1]; |
|
||||
|
|
||||
char a; |
|
||||
unsigned long runforsmth; |
|
||||
Motorz motorR(enr,in1,in2,directr); |
|
||||
Motorz motorL(enl,in3,in4,directl); |
|
||||
|
|
||||
TankDrive mytank(&motorR,directr,&motorL,directl); |
|
||||
|
|
||||
void setup(){ |
|
||||
Serial.begin(9600); |
|
||||
motorR.SetSpeed(80); //im using diffrent motors (trying to simulate wheels so i figure things out ) |
|
||||
motorL.SetSpeed(80); // so that explains the diffrent speed in each motor |
|
||||
pinMode(LED_BUILTIN, OUTPUT); |
|
||||
} |
|
||||
|
|
||||
|
|
||||
void loop(){ |
|
||||
|
|
||||
digitalWrite(LED_BUILTIN,LOW); |
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
/* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* if(a=='1'){ |
|
||||
digitalWrite(LED_BUILTIN, HIGH); |
|
||||
// turn the LED on (HIGH is the voltage level) |
|
||||
} |
|
||||
else if(a=='0') |
|
||||
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW |
|
||||
|
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
mytank.TurnRight(); |
|
||||
delay(10000); |
|
||||
mytank.TurnLeft(); |
|
||||
delay(10000); |
|
||||
mytank.MoveForward(); |
|
||||
delay(10000); |
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
motorR.Start(); |
|
||||
Serial.println(motorR.GetDirection()); |
|
||||
delay(1000); |
|
||||
motorR.ChangeDir(); |
|
||||
Serial.print("Changing Dir: "); |
|
||||
Serial.println(motorR.GetDirection()); |
|
||||
|
|
||||
Serial.print("F:"); |
|
||||
Serial.print(mytank.getFDirR()); |
|
||||
Serial.print(" B:"); |
|
||||
Serial.print(mytank.getBDirR()); |
|
||||
Serial.print(" "); |
|
||||
Serial.print("F:"); |
|
||||
Serial.print(mytank.getFDirL()); |
|
||||
Serial.print(" B:"); |
|
||||
Serial.println(mytank.getBDirL()); |
|
||||
mytank.MoveForward(); |
|
||||
delay(5000); |
|
||||
mytank.MoveBack(); |
|
||||
delay(5000); |
|
||||
|
|
||||
* delay(1000); |
|
||||
mytank.TurnLeft(); |
|
||||
delay(5000); |
|
||||
mytank.TurnRight(); |
|
||||
delay(5000); |
|
||||
mytank.MoveForward(); |
|
||||
delay(5000); |
|
||||
|
|
||||
|
|
||||
Serial.println("Getting distances..."); |
|
||||
i=GetDistances(); |
|
||||
Serial.println("Done"); |
|
||||
if(i==-1) |
|
||||
{ |
|
||||
Serial.println("Turning Left..."); |
|
||||
run_for_millis(&mytank,'L',200); |
|
||||
} |
|
||||
else if(i==0) |
|
||||
{ |
|
||||
Serial.println("Going Forward..."); |
|
||||
run_for_millis(&mytank,'M',200); |
|
||||
} |
|
||||
else{ |
|
||||
Serial.println("Turning Right..."); |
|
||||
run_for_millis(&mytank,'R',200); |
|
||||
} */ |
|
||||
} |
|
||||
void serialEvent() { |
|
||||
while (Serial.available()) { |
|
||||
Serial.readBytes(buffer1,sizeof(buffer1)); |
|
||||
sscanf( buffer1, "%c%lu",&a, &runforsmth); |
|
||||
run_for_millis(&mytank,a,runforsmth); |
|
||||
/*if(a=='L'){ |
|
||||
digitalWrite(LED_BUILTIN, HIGH); |
|
||||
delay(runforsmth); |
|
||||
}*/ |
|
||||
} |
|
||||
} |
|
||||
int GetDistances(){ |
|
||||
int pos; |
|
||||
int maxdist[2]; |
|
||||
int temp; |
|
||||
myservo.attach(SERVO_PIN); |
|
||||
maxdist[0]=-1; |
|
||||
|
|
||||
for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees |
|
||||
// in steps of 1 degree |
|
||||
myservo.write(pos); |
|
||||
temp=sonar.ping_cm(); |
|
||||
Serial.print("Distance:"); |
|
||||
Serial.print(temp); |
|
||||
Serial.print(" Angle:"); |
|
||||
Serial.println(pos); |
|
||||
|
|
||||
if(temp>maxdist[0]){ |
|
||||
maxdist[0]=temp; |
|
||||
maxdist[1]=pos; |
|
||||
} |
|
||||
delay(55); |
|
||||
} |
|
||||
for (pos = 160; pos >=20; pos -= 1) { // goes from 0 degrees to 180 degrees |
|
||||
myservo.write(pos); |
|
||||
delay(5); |
|
||||
} |
|
||||
myservo.detach(); |
|
||||
|
|
||||
if(maxdist[1]<70){ |
|
||||
return -1; |
|
||||
} |
|
||||
|
|
||||
else if(maxdist[1]>110){ |
|
||||
return 1; |
|
||||
} |
|
||||
|
|
||||
else return 0; |
|
||||
|
|
||||
} |
|
||||
void run_for_millis(TankDrive *tank,char ch,unsigned long timetorun){ |
|
||||
|
|
||||
unsigned long count; |
|
||||
switch(ch){ |
|
||||
case 'L': |
|
||||
count=millis(); |
|
||||
do{ |
|
||||
tank->TurnLeft();} |
|
||||
while(millis()-count<=timetorun); |
|
||||
tank->Stop(); |
|
||||
break; |
|
||||
|
|
||||
case 'F': |
|
||||
count=millis(); |
|
||||
do{ |
|
||||
tank->MoveForward();} |
|
||||
while(millis()-count<=timetorun); |
|
||||
tank->Stop(); |
|
||||
break; |
|
||||
case 'R': |
|
||||
count=millis(); |
|
||||
do{ |
|
||||
tank->TurnRight();} |
|
||||
while(millis()-count<=timetorun); |
|
||||
tank->Stop(); |
|
||||
break; |
|
||||
} |
|
||||
|
|
||||
|
|
||||
|
|
||||
} |
|
Loading…
Reference in new issue