// DRIVE_MOTOR_TEST

// 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

// 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


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);
 Serial.println(I);                             // Print IF-LOOP counter
}
  Voltvalue = Voltvalue / 10;
  Voltvalue = Voltvalue * 0.02765;               // 0.02765 is factor for voltagedevider 22K / 4,7K

 Serial.print(" Voltvalue = ");                 // Print text without linefeed
 Serial.println(Voltvalue);                     // Print avarage voltvalue with linefeed                    
 Serial.println(" ");                           // Print nothing as a linefeed
  
 
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
} 

forward();                                      // Go to "forward()" function

}


/////////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
 
}