71Views3Replies

Author Options:

I think i made a mistake in code? Answered

I don't know why but my servo stops after the fulscan is done. I tryed to find out what's wrong, but it works fine, except the servo.
Here is the code. I'm new in this and the timing for the rotation of the robot must be recalibrated, but now i'm trying to make it run.
Using arduino uno and sharp IR sensor. the pin 11 is PWM(for both motors, just to slow it down a bit).

#include <Servo.h>

Servo mainservo;
float dist[16];
int sensor = 0;
int rightB=13;
int rightF=12;
int leftF=8;
int leftB=7;
int pwm=150;
int pwmp=11;

///====================Functions====================\\

int turnR(){
  digitalWrite(rightB,HIGH);
  digitalWrite(leftF,HIGH);
  Serial.println("turnR");
}
int turnL(){
  digitalWrite(rightF,HIGH);
  digitalWrite(leftB,HIGH);
  Serial.println("turnL");
}
int turnback(){
  digitalWrite(rightB,HIGH);
  digitalWrite(leftF,HIGH);
  delay(2000);
  stopp();
  Serial.println("turnback");
}
int goforward(){
  digitalWrite(rightF,HIGH);
  digitalWrite(leftF,HIGH);
  Serial.println("goforward");
}
int stopp(){
  digitalWrite(rightF,LOW);
  digitalWrite(leftF,LOW);
  digitalWrite(rightB,LOW);
  digitalWrite(leftB,LOW);
  Serial.println("Stopp");
}
int fullscan(){
  Serial.println("fullscan");
  float volts;
  mainservo.write(0);
  delay(1000);
  for (int i=0;i<=16;i++){
    mainservo.write(i*10);
    delay(150);
    volts=analogRead(sensor)*0.0048828125;
    dist[i] = 27*pow(volts, -1.1);
    delay(50);
    Serial.print("Dist:  ");
    Serial.println(dist[i]);
    //////// 
  }
  mainservo.write(80);
  Serial.println("dir");
  while (boolean ang=true){
    int d=0;d<=12;d++;
    ang=false;
    if (dist[d]>20 && dist[d+1]>20 && dist[d+2]>20 && dist[d+3]>20 && dist[d+4]>20){
      float dir_ang =(d+2)*10;
      ang=!ang;
      Serial.print("Angle:   ");
      Serial.println(dir_ang);
      if (dir_ang<80){
        turnR();
        delay((80-dir_ang)*10);
        stopp();
      }
      else if(dir_ang>80){
        turnL();
        delay((dir_ang-80)*10);
        stopp();
      }
      else{
        stopp();
      }
    }
  }
  Serial.println("FullScan done");
}


void setup() {
  pinMode(11,OUTPUT);
  pinMode(rightF, OUTPUT);
  pinMode(rightB, OUTPUT);
  pinMode(leftF, OUTPUT);
  pinMode(leftB, OUTPUT);
  analogWrite(11,150);
  mainservo.attach(9);
  Serial.begin(9600);
  fullscan();
}
void loop(){
  Serial.println("scanF");
  float volts;
  float dist;
  mainservo.write(50);
  delay(200); 
  volts=analogRead(sensor)*0.0048828125;
  dist = 27*pow(volts, -1.1);
  delay(50);
  if (dist>25){
    goforward();
    Serial.print("go forward  ");
    Serial.println(dist);
  }
  else{
    stopp();
    fullscan();
  } 
  ////////////
  mainservo.write(80);
  delay(200); 
  volts=analogRead(sensor)*0.0048828125;
  dist = 27*pow(volts, -1.1);
  delay(50);
  if (dist>25){
    goforward();
    Serial.print("go forward");
    Serial.println(dist);
  }
  else{
    stopp();
    fullscan();
  } 
  ////////////
  mainservo.write(110);
  delay(200); 
  volts=analogRead(sensor)*0.0048828125;
  dist = 27*pow(volts, -1.1);
  delay(50);
  if (dist>25){
    goforward();
    Serial.print("go forward");
    Serial.println(dist);
  }
  else{
    stopp();
    fullscan();
  } 
  ////////////
  mainservo.write(80);
  delay(200); 
  volts=analogRead(sensor)*0.0048828125;
  dist = 27*pow(volts, -1.1);
  delay(50);
  if (dist>25){
    goforward();
    Serial.print("go forward");
    Serial.println(dist);
  }
  else{
    stopp();
    fullscan();
  } 
  ////////////
}

Any help is welcome.

Discussions

0
frollard
frollard

7 years ago

Why are you using int() for functions when they don't accept or return any values?

pwmp should be a const because you don't change pins after designtime.

As steve says, its BRUTAL formatting.

Break down what your loop() function does and make each distinct thing be a different function/prodecure;

loop should look like

{
ping(); //look ahead
move(); //act on ping
delay(x); //if needed
}


much like how you broke out each different direction as a function, EVERYTHING should be down to the simplest functions you can make, when one goes wrong its easier to find because the same problem happens at each step.

0
steveastrouk
steveastrouk

Answer 7 years ago

Break it down AND COMMENT IT.

0
steveastrouk
steveastrouk

7 years ago

donotexpectpeopletodeconvolveyourcompletelycommentless codepresentedwithnoinformationaboutitsstructureandwithhardlyanyformatting. 

Unformatted code is like prose with no capitalisation and no punctuation. Steve