
// Collision_Control

// Read sensors

int Sensepinleft = 4 ;                           // Define PIN A4 for left Sensor
int Sensepinright = 3 ;                          // Define PIN A3 for right Sensor
int Sensevalueleft;                              // Define variable for sensorvalue left
int Sensevalueright;                             // Define variable for sensorvalue right
int Sensemaxleft = 4;                            // Define variable for max sensorvalue left and set default
int Sensemaxright = 4;                           // Define variable for max sensorvalue right and set default

// Status LED
int Ledbat = 3;                                  // Define PIN 3 for digital output red LED
int Ledstart = 4;                                // Define PIN 4 for digital output green LED

// Read Battery voltage
int Voltpin = 0;                                 // Define PIN A0 for reading battery voltage
float Volt;                                      // Define variable for voltage
float Voltvalue;                                 // Define variable for avarage voltage calculation
float Voltlow = 10;                              // Define variable and setup for minimum operation voltage
int I;                                           // Define variable for IF-LOOP counter
int I_bat = 20;                                  // Define variable for battery status (low or high)

// Drive motors
int Driveleft = 9;                               // Define PIN 9 for left Motor PWM output               
int IN3 = 6;                                     // Define PIN 6 for left Motor IN3
int IN4 = 5;                                     // Define PIN 5 for left Motor IN4

int Driveright = 10;                             // Define PIN 10 for right Motor PWM output 
int IN1 = 8;                                     // Define PIN 8 for right Motor IN1
int IN2 = 7;                                     // Define PIN 7 for right Motor IN2

int Drivespeedleft = 255;                        // Define variable for left motor speed and set PWM value
int Drivespeedright = 255;                       // Define variable for right motor speed and set PWM value

int Turntime;                                    // Define variable for the time the mower has to turn
int I_Ramp;                                      // Define counter-variable for motor ramp

// Collision control
int Drivepinleft = 1;                            // Define PIN A1 for left motor current
int Drivepinright = 2;                           // Define PIN A2 for right motor current
int Drivesenseleft;                              // Define variable for left motor current
int Drivesenseright;                             // Define variable for right motor current

int Drivemaxleft = 215;                         // Define variable for max motor current left and set default
int Drivemaxright = 215;                        // Define variable for max motor current right and set default

void setup() {                                   // Setup

  // Print
  Serial.begin(9600);                            // Start the serial communication

  // Status LEDs
  pinMode(Ledbat, OUTPUT);                       // Define Ledbat PIN as OUTPUT
  pinMode(Ledstart, OUTPUT);                     // Define Ledstart PIN as OUTPUT 

  // Drive motors
  pinMode(IN1, OUTPUT);                          // Define IN1 PIN as OUTPUT
  pinMode(IN2, OUTPUT);                          // Define IN2 PIN as OUTPUT
  pinMode(IN3, OUTPUT);                          // Define IN3 PIN as OUTPUT
  pinMode(IN4, OUTPUT);                          // Define IN4 PIN as OUTPUT
  
}

void loop() {                                   // Start main programm

digitalWrite(Ledstart, HIGH);                   // Switch green status LED ON

// Read Voltpin for 10 times and calculate average voltvalue
Voltvalue = 0;
for (I = 0;I<10; I++){
 Volt = analogRead(Voltpin);
 Voltvalue = Voltvalue + Volt;
 delay(10);
}
  Voltvalue = Voltvalue / 10;
  Voltvalue = Voltvalue * 0.02765;               // 0.02765 is factor for voltagedevider 22K / 4,7K   
 
if (Voltvalue < Voltlow) {                      // Make dicission: Batteryvoltage O.K or low
  I_bat = 1;                                    // If battery voltage is low set status 1
  Stop();                                       // If battery is low go to “Stop()” function
} 


  //Collision control
  Drivesenseleft = analogRead(Drivepinleft);    // Read left motor current
  Drivesenseright = analogRead(Drivepinright);  // Read right motor current
  Serial.print("Motor current left = ");        // Print
  Serial.println(Drivesenseleft);
  Serial.print("Motor current right = ");
  Serial.println(Drivesenseright);
  if (Drivesenseleft > Drivemaxleft) {          // Compare left motor current 
    backward() ;
    Turnright();
  }
   if (Drivesenseright > Drivemaxright) {       // Compare right motor current 
    backward() ;
    Turnleft();
  }

// Read sensors
Sensevalueleft = analogRead(Sensepinleft);      // Read left sensor 
Sensevalueright = analogRead(Sensepinright);    // Read right sensor
Serial.print("Left = ");
Serial.println(Sensevalueleft);
Serial.print("Right = ");
Serial.println(Sensevalueright);
Serial.println(" ");
 

 if (Sensevalueleft > Sensemaxleft) {           // Compare left sensor
  analogWrite(Driveleft, 0);
  analogWrite(Driveright, 0);
  delay(250);
  backward();
  Turnright();
 }

 if (Sensevalueright > Sensemaxright) {         // Compare right sensor
  analogWrite(Driveleft, 0);
  analogWrite(Driveright, 0);
  delay(250);
  backward();
  Turnleft();
 }

 // Go forward
  forward();   
  delay(200);
 
}


/////////FUNCTIONS///////////

void Stop() {                                   // Stop the ARDUMOWER if battery is low
  
  analogWrite(Driveleft, 0);                    // Switch off left drive motor
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,LOW);   
  analogWrite(Driveright, 0);                   // Switch off right drive motor
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,LOW); 

digitalWrite(Ledstart, LOW);                    // Switch green status LED OFF               

while (I_bat < 10)                              // As long as batterystatus is low, stay here
{
  // flash red status LED
digitalWrite(Ledbat, HIGH);
delay(100);
digitalWrite(Ledbat, LOW); 
delay(500);

Serial.println("Low Battery");                  // Print "Low battery" message        

}
}

void forward()  {                               // Drive forward
    
  digitalWrite(IN3,LOW);                        // Switch PIN IN3 LOW
  digitalWrite(IN4,HIGH);                       // Switch PIN IN4 HIGH
 
  digitalWrite(IN1,LOW);                        // Switch PIN IN1 Low                  
  digitalWrite(IN2,HIGH);                       // Switch PIN IN2 HIGH

 analogWrite(Driveleft, Drivespeedleft);        // Set PWM-value for left motor
 analogWrite(Driveright, Drivespeedright);      // Set PWM-value for right motor
 
}

void backward() {                               // Drive backward

  // Switch IN-PINs for backward drive
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW); 
 
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW); 

 for (I_Ramp = 0; I_Ramp < 255; I_Ramp ++) {    // Counter-loop for motor ramp
 analogWrite(Driveleft, I_Ramp);
 analogWrite(Driveright, I_Ramp);
 delay(5);
  }

 analogWrite(Driveleft, Drivespeedleft);        // After motor ramp use default drivespeed-value
 analogWrite(Driveright, Drivespeedright);      // After motor ramp use default drivespeed-value

  delay(1000);                                  // The time the mower should go backwards
  analogWrite(Driveleft, 0);                    // Stop motor after going backwards
  analogWrite(Driveright, 0);                   // Stop motor after going backwards
  delay(250);                                   // Give the mower some time to stop

  }

  void Turnleft() {                             // Turn left                      
  // Switch IN-PINs for turn left
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW); 
  
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);   

   for (I_Ramp = 0; I_Ramp < 255; I_Ramp ++) {  // Counter-loop for motor ramp
 analogWrite(Driveleft, I_Ramp);
 analogWrite(Driveright, I_Ramp);
 delay(5);
 
  }

 analogWrite(Driveleft, Drivespeedleft);     // After motor ramp use default drivespeed-value
 analogWrite(Driveright, Drivespeedright);   // After motor ramp use default drivespeed-value

 Turntime = random(100, 1500);               // Random for turning time
 delay(Turntime); 

analogWrite(Driveleft, 0);                    // Stop motor after turning
analogWrite(Driveright, 0);                   // Stop motor after turning

Sensevalueleft = 0;                           // Set left sensor-value back to zero
Sensevalueright = 0;                          // Set right sensor-value back to zero
delay(250);

   // Switch IN-PINs for driving forward
  digitalWrite(IN3,LOW);      
  digitalWrite(IN4,HIGH);
 
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);

 for (I_Ramp = 0; I_Ramp < 255; I_Ramp ++) {  // Counter-loop for motor ramp
 analogWrite(Driveleft, I_Ramp);
 analogWrite(Driveright, I_Ramp);
 delay(5); 
  }

 analogWrite(Driveleft, Drivespeedleft);        // After motor ramp use default drivespeed-value
 analogWrite(Driveright, Drivespeedright);     // After motor ramp use default drivespeed-value

}


void Turnright() {                            // Turn right

  // Switch IN-PINs for turn right 
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);  
  
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);  

   for (I_Ramp = 0; I_Ramp < 255; I_Ramp ++) {    // Counter-loop for motor ramp
 analogWrite(Driveleft, I_Ramp);
 analogWrite(Driveright, I_Ramp);
 delay(5); 
  }
  
 analogWrite(Driveleft, Drivespeedleft);          // After motor ramp use default drivespeed-value
 analogWrite(Driveright, Drivespeedright);        // After motor ramp use default drivespeed-value

 Turntime = random(100, 1500);                    // Random for turning time
 delay(Turntime); 
 
analogWrite(Driveleft, 0);                        // Stop motor after turning
analogWrite(Driveright, 0);                       // Stop motor after turning

Sensevalueleft = 0;                           // Set left sensor-value back to zero
Sensevalueright = 0;                          // Set right sensor-value back to zero
delay(250);

// Switch IN-PINs for driving forward
  digitalWrite(IN3,LOW);  
  digitalWrite(IN4,HIGH);
 
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);

 for (I_Ramp = 0; I_Ramp < 255; I_Ramp ++) {      // Counter-loop for motor ramp
 analogWrite(Driveleft, I_Ramp);
 analogWrite(Driveright, I_Ramp);
 delay(5); 
  }

 analogWrite(Driveleft, Drivespeedleft);          // After motor ramp use default drivespeed-value
 analogWrite(Driveright, Drivespeedright);        // After motor ramp use default drivespeed-value
 
}


