Ghetto Robot Code. Any Improvements needed?

[blackheart]

I just finished my second robot. It's shitty and ugly, but functional. It's a modified RC car, keep that in mind as the code is different than differential drive. It doesn't run very well as the code sucks. I programmed it myself with no help. The only thing I used from somebody else is the read_cm function which is great!

Code: [Select]

float read_cm(byte pin) {
int tmp;

tmp = analogRead(pin);
if (tmp < 3)
return -1; // invalid value

return (6787.0 /((float)tmp - 3.0)) - 4.0;
}

 
int pwma, pwmb, ain1, ain2, stby, bin1, bin2;
float val_ir1, val_ir2;
byte pin_ir1 = 5;
byte pin_ir2 = 2;
void drive_forward(int speed) {
  digitalWrite(ain1, HIGH);
  digitalWrite(ain2, LOW);
  analogWrite(pwma, speed);
}

void drive_back(int speed) {
  digitalWrite(ain1, LOW);
  digitalWrite(ain2, HIGH);
  analogWrite(pwma, speed);
}

void turn_right() {
      digitalWrite(bin1, LOW);
      digitalWrite(bin2, HIGH);
      analogWrite(pwmb, 255);
}

void turn_left() {
      digitalWrite(bin1, HIGH);
      digitalWrite(bin2, LOW);
      analogWrite(pwmb, 255);
}

void straight() {
  digitalWrite(bin1, LOW);
  digitalWrite(bin2, LOW);
  analogWrite(pwmb, 0);
}
void setup() {
      Serial.begin(9600); 
      pwma = 5;
      pwmb = 10;
      ain2 = 12;
      ain1 = 2;
      stby = 4;     
      bin1 = 7;
      bin2 = 8;
      pinMode(ain1, OUTPUT);
      pinMode(ain2, OUTPUT);
      pinMode(bin1, OUTPUT);
      pinMode(bin2, OUTPUT);
      pinMode(stby, OUTPUT);
      digitalWrite(stby, HIGH);
}
int speed_v = 0;
void loop() {
 
  val_ir1 = read_cm(pin_ir1);
  val_ir2 = read_cm(pin_ir2);
  speed_v = 255;
  if((val_ir1 < 20) || (val_ir2 < 20)) {
    drive_back(speed_v);
    delay(1000);
  }
 
  if(val_ir1 > val_ir2) {
    turn_left();
    drive_forward(speed_v);
    delay(1000);
  } 
  else if(val_ir2 > val_ir1) {
    turn_right();
    drive_forward(speed_v);
    delay(1000);
  }
   
 
}

If you can hep me improve the code so it runs smoother and avoids obstacles better, it would be great. I have 1 sharp IR on each corner of the front bumper of the car. So that would be 2 sharp ir rf's. It's rear wheel drive.

Again, I need the code improved and better optimised for obstacle avoidance.

Thank You and sorry for any grammar errors as I am posting from my eee pc and the keyboard sucks!