From 8dbd4be7df01c471fe2b71cf1dbd6ceb9a863d61 Mon Sep 17 00:00:00 2001
From: Maxim Marshalov <marshalmaxim@gmail.com>
Date: Sat, 13 Apr 2024 17:07:20 +0300
Subject: [PATCH] add opportunity to read battarey level, voltage and current

---
 etc/arduino/arduino.ino | 112 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 112 insertions(+)
 create mode 100644 etc/arduino/arduino.ino

diff --git a/etc/arduino/arduino.ino b/etc/arduino/arduino.ino
new file mode 100644
index 0000000..38c607e
--- /dev/null
+++ b/etc/arduino/arduino.ino
@@ -0,0 +1,112 @@
+#include <ModbusSerial.h>
+#include <Servo.h>  
+#include <SoftwareSerial.h>
+#include <avr/wdt.h>
+
+#define THRUSTER_LEFT_PIN 11
+#define THRUSTER_RIGHT_PIN 10
+#define SLAVE_ID 1
+#define PWM_LOW 1100
+#define PWM_HIGH 1900
+#define MODBUS_SERIAL Serial
+#define BAUDRATE 115200 
+#define TIMEOUT 200
+//Battarey parameters
+#define R_S 0.01
+#define R_L 49900
+#define V_REF 2.5
+#define V_R1 2000
+#define V_R2 1000
+#define VOLTAGE_PIN A1
+#define CURRENT_PIN A0
+
+// echosounder uart
+#define RX_PIN 9
+#define TX_PIN 6
+
+//Modbus registers
+#define ECHO_PACKAGE_SIZE 4
+#define LEFT_THRUSTER_REG 1
+#define RIGHT_THRUSTER_REG 2 
+#define ECHO_REG 3 
+#define CURRENT_REG 4
+#define VOLTAGE_REG 5
+#define BATTAREY_REG 6
+
+// ModbusSerial object
+ModbusSerial mb (MODBUS_SERIAL, SLAVE_ID, -1);
+
+// Motor objects
+Servo thruster_left;
+Servo thruster_right;
+
+SoftwareSerial softSerial =  SoftwareSerial(RX_PIN, TX_PIN);
+int echoPackage[ECHO_PACKAGE_SIZE];
+int depth = 0;
+unsigned long lastPing;
+
+void setup() {
+  thruster_left.attach(THRUSTER_LEFT_PIN);
+  thruster_right.attach(THRUSTER_RIGHT_PIN);
+  softSerial.begin(BAUDRATE);
+  pinMode(RX_PIN, INPUT);
+  pinMode(TX_PIN, OUTPUT);
+  MODBUS_SERIAL.begin (BAUDRATE); // works on all boards but the configuration is 8N1 which is incompatible with the MODBUS standard
+  lastPing = millis();
+  wdt_disable();
+  mb.config (BAUDRATE);
+  mb.setAdditionalServerData("Waterstrider low level");
+  mb.addHreg (LEFT_THRUSTER_REG, 1500);
+  mb.addHreg (RIGHT_THRUSTER_REG, 1500);
+  mb.addHreg (ECHO_REG, 0);
+  mb.addHreg(BATTAREY_REG, 0);
+  mb.addHreg(CURRENT_REG, 0);
+  mb.addHreg(VOLTAGE_REG, 0);
+  pinMode(CURRENT_PIN, INPUT);
+  pinMode(VOLTAGE_PIN, INPUT);
+  
+
+}
+void ping(){
+   if(millis() - lastPing > TIMEOUT){
+     digitalWrite(RX_PIN, LOW);
+     delay(2);
+     digitalWrite(TX_PIN, HIGH);
+     lastPing = millis();
+   }
+     
+}
+void ReadDepth(){
+    if (softSerial.available() > ECHO_PACKAGE_SIZE) {
+          if (softSerial.read() == 0xFF)
+          {
+            for (int i = 1; i < ECHO_PACKAGE_SIZE; i++) {
+              echoPackage[i] = softSerial.read();
+            }
+            depth = echoPackage[1] * 256 + echoPackage[2];
+            mb.setHreg(ECHO_REG, constrain(depth,0,10000));
+            softSerial.flush();
+          }
+    }
+}
+void BattareyRead(){
+  
+  float voltage = (analogRead(VOLTAGE_PIN) * 5.0 / 1024) / (V_R2 / (V_R1 + V_R2));
+  float current = (analogRead(CURRENT_PIN) * 5.0 / 1024) / ((R_S * R_L) / 1000);
+  float battarey = voltage * V_REF;
+    mb.setHreg(VOLTAGE_REG, voltage);
+    mb.setHreg(CURRENT_REG, current);
+    mb.setHreg(BATTAREY_REG, battarey);
+    delay(30);
+}
+
+
+void loop() {
+  ping();
+  ReadDepth();
+  BattareyRead();
+  mb.task();
+  thruster_left.writeMicroseconds(constrain(mb.Hreg(LEFT_THRUSTER_REG), PWM_LOW, PWM_HIGH));
+  thruster_right.writeMicroseconds(constrain(mb.Hreg(RIGHT_THRUSTER_REG), PWM_LOW, PWM_HIGH));
+  delay(15);
+}
\ No newline at end of file
-- 
GitLab