267 lines
9.5 KiB
C++
267 lines
9.5 KiB
C++
|
|
// DRIVE_WITH_SENSORS
|
|
|
|
// 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
|
|
|
|
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
|
|
}
|
|
|
|
// 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
|
|
|
|
}
|
|
|
|
|