123 lines
5.0 KiB
C++
123 lines
5.0 KiB
C++
|
|
// 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
|
|
|
|
}
|
|
|