Autoduino

/**
* Ryhmän nimi :D
*
*/

#include <Servo.h>

Servo servo;

// define input pins
int echoPin =     8;
int fotoPin =     A2;
int benderPin =   A1;
int tempPin =     A0;

// define output pins
int ledPin =      2;
int servoPin =    3;

int speedPin =    5;  // PWM
int motorPin1 =   12;
int motorPin2 =   13;

int echoTrigPin =  10;


/****************
*     SETUP   *
****************/

void setup() {

  servo.attach(servoPin);

  // define pinmodes for INPUT
  pinMode(echoPin, INPUT);
  pinMode(fotoPin, INPUT);
  pinMode(benderPin, INPUT);
  pinMode(tempPin, INPUT);

  // define pinmodes for OUTPUT
  pinMode(ledPin, OUTPUT);
  pinMode(servoPin, OUTPUT);
  
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  
  pinMode(speedPin, OUTPUT);
  pinMode(echoTrigPin, OUTPUT);

  Serial.begin(9600);
}

bool wallAhead = false;
bool ohCrash = false;
bool tempTooHigh = false;
bool darknessAhead = false;

int steering = 28;


/****************
*   MAIN LOOP   *
****************/

void loop() {
  setSpeed(170);
  // refresh sensor values
  wallAhead = isWallAhead(30);
  ohCrash = isBarrierAhead(16);
  tempTooHigh = isTempTooHigh(28);
  darknessAhead = isGettingDark(650);
  
  // Handle EVENTS

  // darkness gets too overhelming --> turn on light
  if (darknessAhead) useLight();

  // HI-SPEED LOOP (tm)
  
  // these will be called more often than others
  int i = 0;
  while(i < 10) {

    // wall ahead --> turn left
    if (wallAhead) {

       // obstacle in 30cm
       turnCar(-steering);
     
    } else if(ohCrash) {

      // reverse a bit
      // turn 35 & go!
      
      // crashed to wall
      turnCar(steering);
      
    } else {

      // all clear
      turnCar(0);
      
    }

    // temperature gets too high --> stop
    if (tempTooHigh) {
      setSpeed(0);
    } else {
      setSpeed(170);
    }
    
    delay(10);
    i++;
  }
}

 
/****************
*   FUNCTIONS   *
****************/

// Sensor checks

/*
 * Check ULTRASOUND sensor input
 * If wall is closer than MINDISTANCE, return TRUE
 * Otherwise FALSE
 */
bool isWallAhead(float minDistance) {
  digitalWrite(echoTrigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(echoTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(echoTrigPin, LOW);

  // read the value
  long dur = pulseIn(echoPin, HIGH);
  
  return (dur / 58) <= minDistance;
}

/**
 * Check BENDER sensor input
 * if it's bent, return TRUE
 * otherwise FALSE
 */
bool isBarrierAhead(int tolerance) {

  // this is the input when the bender is in stable position
  int stablePosition = 833;
  int bending = analogRead(benderPin);

  // Serial.println(bending);
  // Serial.println(bending <= stablePosition - tolerance);

  // if bender is bent more than tolerance to either direction
  return bending <= stablePosition - tolerance;
  // return bending >= stablePosition + tolerance || bending <= stablePosition - tolerance;
}

/** 
 * Check TEMPERATURE sensor input 
 * If the temp is more than TEMPERATURE, return TRUE
 * Otherwise FALSE
 */
bool isTempTooHigh(float minTemp) {
  float rawTemp = analogRead(tempPin);
  float voltage = rawTemp * 5.0;
  voltage /= 1024;
  
  return (voltage - 0.5) * 100 >= minTemp;
}

/*
 * Check FOTORECEPTOR inpur
 * If darker than something, return TRUE
 * Otherwise FALSE
 */
bool isGettingDark(int limit) {
  int lightLevel = analogRead(fotoPin);
  return lightLevel <= limit;
}



// Actions

/** 
 * Turn the car to given direction 
 * negative --> left
 * positive --> right
 */
void turnCar(int direction) {
  int defaultPos = 90;

  Serial.println("Steering angle: \t" + String(defaultPos-direction));
  
  servo.write(defaultPos-direction);

/*
  int timer = 0;
  while {
    servo.write(defaultPos-direction);
    Reverse(60);
    timer += 1;
    if(timer <= 1000){
      break;
    }
  }
  */
}

void setSpeed(int speed) {
  bool forward = true;
  
  analogWrite(speedPin, speed);
  digitalWrite(motorPin1, forward);
  digitalWrite(motorPin2, !forward);

  // Serial.println("Spid is bout: \t" + String(speed));
}

void useLight() {
  digitalWrite(ledPin, HIGH);
}