stuff.ino

gistfile1.c

//Declare Pin Numbers here.
int turnType = 0; //Right Turn
void setup(){
   //set up!
}
void loop(){

   if(distanceGetCm() > 15 && maskCheckLightSensor(0,0,0)){
      //go forward
   }else if(distanceGetCm() < 15 && maskCheckLightSensor(0,0,0)){
      if(turnType == 0){
         //Go right. Use a delay.
         turnType = 1; //Return to a left turn next time.
      }else if(turnType == 1){
         //Go left. Use a delay.
         turnType = 0; //Return to a right turn next time.
      }
      //turn whatever direction. you're near hitting something.
   }
}

int distanceGetCm(){
   digitalWrite(trigPin,LOW);
   delayMicroseconds(2);
   digitalWrite(trigPin,HIGH);
   delayMicroseconds(2);
   digitalWrite(trigPin, LOW);
   duration = pulseIn(echoPin, HIGH);
   cm = duration / 29 / 2;
   return cm;
}

boolean maskCheckLightSensor(int s1, int s2, int s3){
   //Read in L1-L3 here.
	return (L1 == s1 && L2 == s2 && L3 == s3);
}
void setMotorPowerPercentage(int percentage, int pinFwd, int pinRev){
   if(percentage < 0){
      analogWrite(pinFwd, 0);
      percentage = percentage*-1;
      int mappedPercentage = map(percentage, 0, 100, 0, 255);
      analogWrite(pinRev, mappedPercentage);
   }else if(percentage > 0){
      analogWrite(pinRev, 0);
      int mappedPercentage = map(percentage, 0, 100, 0, 255);
      analogWrite(pinFwd, mappedPercentage);
   }else if(percentage == 0){
      analogWrite(pinRev, 0);
      analogWrite(pinFwd, 0);
   }
}
void leftTurn(int percentage){
   setMotorPowerPercentage(-1*percentage, leftFwd, leftRev);
   setMotorPowerPercentage(percentage, rightFwd, rightRev);
}
void rightTurn(int percentage){
   setMotorPowerPercentage(percentage, leftFwd, leftRev);
   setMotorPowerPercentage(-1*percentage, rightFwd, rightRev);
}