// ---------------------------------------------------------------------------------- // // The SEVI Servant robot charge software v3.1 // by Jaakko Muhonen (Robolab Fin) 30.9.2018 // Internet: www.robolab.fi // Arduino Nano ATmega328 // // ---------------------------------------------------------------------------------- #include const int BatteryPinArduino=A2; // A2 = Arduino measuring voltage in (12V) const int BatteryPinServo=A1; // A1 = Servo measuring voltage in (6V) const int BatteryPinMotor=A0; // A0 = Motor measuring voltage in (12V) const int ForceChargePin=A3; // A3 = All battery forced charging button (manual) pin const byte RelayPinArduino=2; // D2 = Arduino relay pin on/off (Red led 1) const byte RelayPinServo=3; // D3 = Servo relay pin on/off (Red led 2) const byte RelayPinMotor=4; // D4 = Motor relay pin on/off (Red led 3) const byte RelayPinCharge=5; // D5 = Charge relay pin on/off (Yellow led) const byte TestSelectorPin=6; // D6 = Test selector switch (manual) pin const byte VoltageInfoInPin=7; // D7 = Logic Unit voltage info In pin (Write) const byte VoltageInfoOutPin=8; // D8 = Logic Unit voltage info Out pin (Read) const byte ChargeLedPinArduino=9; // D9 = Charge Led Arduino pin on/off (Green led 1) const byte ChargeLedPinServo=10; // D10 = Charge Led Servo pin on/off (Green led 2) const byte ChargeLedPinMotor=11; // D11 = Charge Led Motor pin on/off (Green led 3) const byte RelayPinAtmega=12; // D12 = Atmega battery relay pin on/off int gTestSelectorButton=LOW; // Test selector switch (manual) button status; HIGH/LOW float gVoltageArduino=0; // Arduino battery voltage input float gVoltageServo=0; // Servo battery voltage input float gVoltageMotor=0; // Motor battery voltage input int gArduinoLow=false; // Arduino battery is low: true/false int gServoLow=false; // Servo battery is low: true/false int gMotorLow=false; // Motor battery is low: true/false int gForceCharging=false; // Start forced charging: true/false int gForceChargingTime=4800; // Forced charging time per battery. 1200 = 60 min. 4800 = 4 hours. int gForceChargingTimeCount=0; // Forced charging time counter int gForceLoadArduino=false; // Forced Arduino battery loading: true/false int gForceLoadServo=false; // Forced Servo battery loading: true/false int gForceLoadMotor=false; // Forced Motor battery loading: true/false float gVol[5]; // Voltage status auxiliary variable int gPin=0; // Auxiliary pin variable int Count=0; // Auxiliary count variable /* ------------------------------------------------------------------------------------------- */ void setup() { Serial.begin(9600); pinMode(RelayPinArduino, OUTPUT); // Arduino relay pin setup pinMode(RelayPinServo, OUTPUT); // Servo relay pin setup pinMode(RelayPinMotor, OUTPUT); // Motor relay pin setup pinMode(RelayPinCharge, OUTPUT); // Charge relay pin setup pinMode(RelayPinAtmega, OUTPUT); // Atmega relay pin setup pinMode(ChargeLedPinArduino, OUTPUT); // Arduno charge led pin setup pinMode(ChargeLedPinServo, OUTPUT); // Servo charge led pin setup pinMode(ChargeLedPinMotor, OUTPUT); // Motor charge led pin setup pinMode(TestSelectorPin, INPUT); // Test selector switch (manual) pin setup pinMode(VoltageInfoInPin, OUTPUT); // Logic Unit voltage info In pin (Write) setup pinMode(VoltageInfoOutPin, INPUT); // Logic Unit voltage info Out pin (Read) setup pinMode(ForceChargePin, INPUT); // All battery forced charging button (manual) pin setup digitalWrite(RelayPinArduino, HIGH); // Arduino relay: Turn OFF digitalWrite(RelayPinServo, HIGH); // Servo relay: Turn OFF digitalWrite(RelayPinMotor, HIGH); // Motor relay: Turn OFF digitalWrite(RelayPinCharge, HIGH); // Charge relay: Turn OFF digitalWrite(RelayPinAtmega, LOW); // Atmega relay: Turn ON digitalWrite(ChargeLedPinArduino, LOW); // Arduino charge led: Turn OFF digitalWrite(ChargeLedPinServo, LOW); // Servo charge led: Turn OFF digitalWrite(ChargeLedPinMotor, LOW); // Motor charge led: Turn OFF digitalWrite(VoltageInfoInPin, LOW); // Logic Unit voltage info In pin (Write): Voltage LOW } /* ------------------------------------------------------------------------------------------- */ /* ------------------------------------------------------------------------------------------- */ void loop() { // Read battery status CheckBattery(); // Check Arduino voltage limits if (gVoltageArduino>3 and gVoltageArduino<11) { digitalWrite(ChargeLedPinArduino, HIGH); gArduinoLow=true; } else { if (gForceCharging==false) digitalWrite(ChargeLedPinArduino, LOW); gArduinoLow=false; } // Check Servo voltage limits if (gVoltageServo>3 and gVoltageServo<5) { digitalWrite(ChargeLedPinServo, HIGH); gServoLow=true; } else { if (gForceCharging==false) digitalWrite(ChargeLedPinServo, LOW); gServoLow=false; } // Check Motor voltage limits if (gVoltageMotor>3 and gVoltageMotor<11) { digitalWrite(ChargeLedPinMotor, HIGH); digitalWrite(VoltageInfoInPin, HIGH); gMotorLow=true; } else { if (gForceCharging==false) digitalWrite(ChargeLedPinMotor, LOW); gMotorLow=false; } // Send info to the Logic Unit if any battery is low if (gArduinoLow==true or gServoLow==true or gMotorLow==true) digitalWrite(VoltageInfoInPin, HIGH); else digitalWrite(VoltageInfoInPin, LOW); // Read the test switch status gTestSelectorButton=digitalRead(TestSelectorPin); // Start forced loading if the Test Selector button is On and the Forced Charging button is On if (gTestSelectorButton==true and digitalRead(ForceChargePin)==true) { if (gForceCharging==false) { gForceCharging=true; gForceLoadArduino=true; gForceChargingTimeCount=0; Serial.println(F("Forced charging BEGINS.")); for (Count=0; Count<5; Count++) { digitalWrite(ChargeLedPinArduino, HIGH); digitalWrite(ChargeLedPinServo, HIGH); digitalWrite(ChargeLedPinMotor, HIGH); delay(1500); digitalWrite(ChargeLedPinArduino, LOW); digitalWrite(ChargeLedPinServo, LOW); digitalWrite(ChargeLedPinMotor, LOW); delay(500); } digitalWrite(RelayPinAtmega, HIGH); // Atmega relay: Turn ON delay(4000); } else { // Move next forced loading step gForceChargingTimeCount=gForceChargingTime+1; delay(2000); } } // if - ForceChargePin==true // ARDUINO forced charging if (gForceCharging==true and gForceLoadArduino==true) { if (gForceChargingTimeCount==0) { Serial.println(F("ARDUINO forced charging has been started.")); Serial.println(""); digitalWrite(ChargeLedPinArduino, HIGH); // Arduino charge led: Turn ON delay(3000); digitalWrite(RelayPinArduino, LOW); // Arduino relay: Turn ON delay(3000); digitalWrite(RelayPinCharge, LOW); // Charge relay: Turn ON delay(3000); } else if (gForceChargingTimeCount>gForceChargingTime) { Serial.println(F("ARDUINO forced charging is stopped.")); digitalWrite(RelayPinArduino, HIGH); // Arduino relay: Turn OFF digitalWrite(RelayPinCharge, HIGH); // Charge relay: Turn OFF digitalWrite(ChargeLedPinArduino, LOW); // Arduino charge led: Turn OFF gForceLoadArduino=false; gForceLoadServo=true; gForceChargingTimeCount=0; delay(4000); } } // If - ARDUINO forced charging // SERVO forced charging if (gForceCharging==true and gForceLoadServo==true) { if (gForceChargingTimeCount==0) { Serial.println(F("SERVO forced charging has been started.")); Serial.println(""); digitalWrite(ChargeLedPinServo, HIGH); // Servo charge led: Turn ON delay(3000); digitalWrite(RelayPinServo, LOW); // Servo relay: Turn ON delay(3000); digitalWrite(RelayPinCharge, LOW); // Charge relay: Turn ON delay(3000); } else if (gForceChargingTimeCount>gForceChargingTime) { Serial.println(F("SERVO forced charging is stopped.")); digitalWrite(RelayPinServo, HIGH); // Servo relay: Turn OFF digitalWrite(RelayPinCharge, HIGH); // Charge relay: Turn OFF digitalWrite(ChargeLedPinServo, LOW); // Servo charge led: Turn OFF gForceLoadServo=false; gForceLoadMotor=true; gForceChargingTimeCount=0; delay(4000); } } // If - SERVO forced charging // MOTOR forced charging if (gForceCharging==true and gForceLoadMotor==true) { if (gForceChargingTimeCount==0) { Serial.println(F("MOTOR forced charging has been started.")); Serial.println(""); digitalWrite(ChargeLedPinMotor, HIGH); // Motor charge led: Turn ON delay(3000); digitalWrite(RelayPinMotor, LOW); // Motor relay: Turn ON delay(3000); digitalWrite(RelayPinCharge, LOW); // Charge relay: Turn ON delay(3000); } else if (gForceChargingTimeCount>gForceChargingTime) { Serial.println(F("MOTOR forced charging is stopped.")); digitalWrite(RelayPinMotor, HIGH); // Motor relay: Turn OFF digitalWrite(RelayPinCharge, HIGH); // Charge relay: Turn OFF digitalWrite(ChargeLedPinMotor, LOW); // Motor charge led: Turn OFF gForceLoadMotor=false; gForceCharging=false; gForceChargingTimeCount=0; Serial.println(F("All the batteries are charged.")); Serial.println(F("Forced charging is OVER.")); Serial.println(""); delay(1000); for (Count=0; Count<5; Count++) { digitalWrite(ChargeLedPinArduino, HIGH); digitalWrite(ChargeLedPinServo, HIGH); digitalWrite(ChargeLedPinMotor, HIGH); delay(1500); digitalWrite(ChargeLedPinArduino, LOW); digitalWrite(ChargeLedPinServo, LOW); digitalWrite(ChargeLedPinMotor, LOW); delay(500); } delay(4000); digitalWrite(RelayPinAtmega, LOW); } } // If - MOTOR forced charging // Increase the gForceCharging counter if (gForceCharging==true) gForceChargingTimeCount++; Serial.print(F("gVoltageArduino=")); Serial.println(gVoltageArduino); Serial.print(F("gVoltageServo=")); Serial.println(gVoltageServo); Serial.print(F("gVoltageMotor=")); Serial.println(gVoltageMotor); Serial.println(""); delay(3000); // Show all battery status if the Test Selector button is Off and the Forced Charging button is On if (gTestSelectorButton==false and digitalRead(ForceChargePin)==true) { Serial.println(F("Show all battery status in green leds.")); Serial.println(""); // Loop green leds 1-3: D9, D10 and D11 pins for (gPin=9; gPin<12; gPin++) { // Turn off all green leds 1-3 digitalWrite(ChargeLedPinArduino, LOW); digitalWrite(ChargeLedPinServo, LOW); digitalWrite(ChargeLedPinMotor, LOW); delay(2000); // Set the pin voltage values if (gPin==9) { gVol[0]=gVoltageArduino; gVol[1]=12.0; gVol[2]=11.5; gVol[3]=11.0; } else if (gPin==10) { gVol[0]=gVoltageServo; gVol[1]=6.0; gVol[2]=5.5; gVol[3]=5.0; } else { gVol[0]=gVoltageMotor; gVol[1]=12.0; gVol[2]=11.5; gVol[3]=11.0; } for (Count=0; Count<3; Count++) { digitalWrite(gPin, HIGH); delay(500); digitalWrite(gPin, LOW); delay(1000); } // Show battery status in green leds if (gVol[0]>=gVol[1]) { digitalWrite(ChargeLedPinArduino, HIGH); digitalWrite(ChargeLedPinServo, HIGH); digitalWrite(ChargeLedPinMotor, HIGH); } else if (gVol[0]>=gVol[2]) { digitalWrite(ChargeLedPinArduino, HIGH); digitalWrite(ChargeLedPinServo, HIGH); digitalWrite(ChargeLedPinMotor, LOW); } else if (gVol[0]>=gVol[3]) { digitalWrite(ChargeLedPinArduino, HIGH); digitalWrite(ChargeLedPinServo, LOW); digitalWrite(ChargeLedPinMotor, LOW); } else { digitalWrite(ChargeLedPinArduino, LOW); digitalWrite(ChargeLedPinServo, LOW); digitalWrite(ChargeLedPinMotor, LOW); } delay(4000); digitalWrite(ChargeLedPinArduino, LOW); digitalWrite(ChargeLedPinServo, LOW); digitalWrite(ChargeLedPinMotor, LOW); } // For - gPin } // If - Battery Status } /* ------------------------------------------------------------------------------------------- */ /* ------------------------------------------------------------------------------------------- */ /* Check the Motor, Servo and Arduino battery status. */ /* ------------------------------------------------------------------------------------------- */ void CheckBattery() { float Vol, nVoltageArduino, nVoltageServo, nVoltageMotor; float r1=100000; // 100 Kohm resistor float r2=10000; // 10 Kohm resistor // Check Arduino voltage Vol = (analogRead(BatteryPinArduino) * 5.00) / 1024.0; gVoltageArduino = Vol / (r2 / (r1 + r2)); // Check Servo voltage Vol = (analogRead(BatteryPinServo) * 5.00) / 1024.0; gVoltageServo = Vol / (r2 / (r1 + r2)); // Check Motor voltage Vol = (analogRead(BatteryPinMotor) * 5.00) / 1024.0; gVoltageMotor = Vol / (r2 / (r1 + r2)); } /* ------------------------------------------------------------------------------------------- */