Go Down

Topic: Autonomous Robot Sketch!  (Read 780 times) previous topic - next topic

yashtag

This  is an autonomous robot sketch!! It is not scanning correctly.

Code: [Select]
#include <NewPing.h>

#include <Servo.h>
//Declare Servos

Servo scanservo;     //Ping Sensor Servo
const int scanservopin = 4;   // Pin number for scan servo

const int TRIGGER_PIN = 8;     //Pin that the Ping sensor is attached to
const int ECHO_PIN = 9; //Pin that echo returns to
const int distancelimit = 20;   //If something gets this many inches from
                                 // the robot it stops and looks for where to go.
int scantime = 0;
int lastscantime = 0;
char sensorpos = 'L';
long oldtime = 0;
long timesinceturnedleft = 0;
long timesinceturnedright = 0;

const int A1A = 5; //(pwm) pin 5 to pin A-1A
const int A1B = 6; //(pwm) pin 6 to pin A-1B
const int B1A = 10; //(pwm) pin 10 to pin B-1A
const int B1B = 11; //(pwm) pin 11 to pin B-1B

byte speed = 240; //change this (0-255) to control motor speed
                                 
//Setup function. Runs once when Arduino is turned on or restarted
void setup()
{
 
  scanservo.attach(scanservopin); // Attach the scan servo
  Serial.begin(9600);
  delay(2000);        // wait two seconds
   pinMode(A1A, OUTPUT); // set pins to output or input
  pinMode(A1B, OUTPUT);
  pinMode(B1A, OUTPUT);
  pinMode(B1B, OUTPUT);
  pinMode(9, INPUT_PULLUP);
  pinMode(8, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIGGER_PIN, OUTPUT);
  }
void loop(){
    int leftdistance = 180;
  int rightdistance = 180;
  go();  // if nothing is wrong then go forward using go() function below.
  if(millis()>oldtime+300){
  if(sensorpos == 'L'){
      leftdistance = ping();
      sensorpos = 'R';
      sensorpos = 'C';
  }
  else{
      rightdistance = ping();
      sensorpos = 'L';
  }
  oldtime = millis();
  }
  switch (sensorpos){
  case 'L':
      scanservo.write(20);
      break;
  case 'R':
      scanservo.write(180);
      break;
  }
 
  if(leftdistance<distancelimit){
  if (millis()<timesinceturnedleft + 500){
      backward(1000);
      turnright(200);
  }
  else{
      turnright(50);
  }
    timesinceturnedright = millis();
  }
  if(rightdistance<distancelimit){
    if(millis()<timesinceturnedright + 500){
      backward(1000);
      turnleft(200);
  }
  else{
      turnleft(50);
  }
    timesinceturnedleft = millis();
  }
}
  if(centerdistance<distancelimit){
    moveBackward
  }
  else{
    moveForward
  }
 
 
 
int ping(){
  long duration, inches, cm;
  //Send Pulse
  pinMode(TRIGGER_PIN, OUTPUT);
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  //Read Echo
  pinMode(ECHO_PIN, INPUT);
  duration = pulseIn(ECHO_PIN, HIGH);
 
  // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
  Serial.print("Ping:  ");
  Serial.println(inches);
  return round(inches);
}
void go(){
  analogWrite(A1A, speed);
  analogWrite(A1B, 0);
  analogWrite(B1A, speed);
  analogWrite(B1B, 0);
}
void turnleft(float t){
  analogWrite(A1A, speed);
  analogWrite(A1B, 0);
  analogWrite(B1A, 0);
  analogWrite(B1B, speed);
  t=(t/120)*1000;

  Serial.print("Turning left for this many seconds: ");
  Serial.println(t);
  delay(t);
}
   
void turnright(float t){
  analogWrite(A1A, 0);
  analogWrite(A1B, speed);
  analogWrite(B1A, speed);
  analogWrite(B1B, 0);
   t=(t/120)*1000;
 
  Serial.print("Turning right for this many seconds: ");
  Serial.println(t);
  delay(t);
}
void forward(int t){
  analogWrite(A1A, speed);
  analogWrite(A1B, 0);
  analogWrite(B1A, speed);
  analogWrite(B1B, 0);
  delay(t); 
}
void backward(int t){
  analogWrite(A1A, 0);
  analogWrite(A1B,speed);
  analogWrite(B1A, 0);
  analogWrite(B1B, speed);
  delay(t); 
}
void stopmotors(){
  analogWrite(A1A, 0);
  analogWrite(A1B, 0);
  analogWrite(B1A, 0);
  analogWrite(B1B, 0);
}     
 
char scan(){
  int lval, lcval, cval, rcval, rval;
  int maximum = 0;
  int choice;
 
  scanservo.write(20); //Look left
  delay(300);
  lval = ping();
  if(lval>maximum){
  maximum = lval;
  choice = 1;
}
 scanservo.write(45); //Look left center
  delay(300);
  lcval = ping();
  if(lcval>maximum){
  maximum = lcval;
  choice = 2;
}
 
  scanservo.write(90); //Look center
  delay(300);
  cval = ping();
  if(cval>maximum){
  maximum = cval;
  choice = 3;
}
 
  scanservo.write(135); //Look right center
  delay(300);
  rcval = ping();
  if(rcval>maximum){
  maximum = rcval;
  choice = 4;
}
 
  scanservo.write(170);  //Look right
  delay(300);
  rval = ping();
  if(rval>maximum){
  maximum = rval;
  choice = 5;
}
 
if(maximum<=distancelimit){choice = 6;}
 
  scanservo.write(88);  //center scan servo
  delay(300);
 
  Serial.print("Choice:  ");
  Serial.println(choice);
  return choice;
}

long microsecondsToInches(long microseconds){
  return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds){
  return microseconds / 29 / 2;

PaulS

Code: [Select]
      sensorpos = 'R';
      sensorpos = 'C';

Well, that's certainly useful.

Code: [Select]
  if(centerdistance<distancelimit){
    moveBackward
  }
  else{
    moveForward
  }

Parentheses are NOT optional.
The art of getting good answers lies in asking good questions.

Go Up