diff --git a/config/modbus-mapping.json b/config/modbus-mapping.json index 07a4fe5b9a57adaf524013b133c2d1e955a25cde..193c4afa7cec16b6129f8a829374f7409214d7e1 100644 --- a/config/modbus-mapping.json +++ b/config/modbus-mapping.json @@ -27,7 +27,19 @@ "echo": { "address": 3, "type": "UINT" + }, + "current":{ + "address": 4, + "type":"UINT" + }, + "voltage":{ + "address": 5, + "type": "UINT" + }, + "charge":{ + "address": 6, + "type": "UINT" } } } -} +} \ No newline at end of file diff --git a/etc/arduino.ino b/etc/arduino.ino deleted file mode 100644 index 6f2d9bba2f2bddb2b87a6f2a017d8b3756c19abd..0000000000000000000000000000000000000000 --- a/etc/arduino.ino +++ /dev/null @@ -1,91 +0,0 @@ -#include -#include -#include -#include - -const int thruster_left_pin = 11; -const int thruster_right_pin = 10; -const byte slave_id = 1; - -const int reg_thruster_left = 1; -const int reg_thruster_right = 2; -const int reg_echo = 3; - -#define PWM_LOW 1100 -#define PWM_HIGH 1900 - -#define MySerial Serial // define serial port used, Serial most of the time, or Serial1, Serial2 ... if available - -const unsigned long baudrate = 115200; - -// ModbusSerial object -ModbusSerial mb (MySerial, slave_id, -1); -// Servo object -Servo thruster_left; -Servo thruster_right; - -// echosounder -#define rxPin 9 -#define txPin 6 -SoftwareSerial softSerial = SoftwareSerial(rxPin, txPin); -unsigned long lastPing; -const int timeout = 200; -#define ECHO_PACKAGE_SIZE 4 -int echoPackage[ECHO_PACKAGE_SIZE]; -int depth = 0; -void setup() { - MySerial.begin (baudrate); // works on all boards but the configuration is 8N1 which is incompatible with the MODBUS standard - wdt_disable(); - mb.config (baudrate); - mb.setAdditionalServerData("Waterstrider low level"); // for Report Server ID function (0x11) - - thruster_left.attach(thruster_left_pin); - thruster_right.attach(thruster_right_pin); - mb.addHreg (reg_thruster_left, 1500); - mb.addHreg (reg_thruster_right, 1500); - mb.addHreg (reg_echo, 0); - - pinMode(rxPin, INPUT); - pinMode(txPin, OUTPUT); - - // Set the baud rate for the SoftwareSerial object - softSerial.begin(115200); - lastPing = millis(); -} - -void ping() { - if ((millis() - lastPing)> timeout) - { - digitalWrite(txPin, LOW); - delay(2); - digitalWrite(txPin, HIGH); - lastPing = millis(); - } -} - -void parseEchosounder() -{ - 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(reg_echo, constrain(depth,0,10000)); - softSerial.flush(); - } - } -} - - -void loop() { - ping(); - parseEchosounder(); - mb.task(); - - thruster_left.writeMicroseconds(constrain(mb.Hreg(reg_thruster_left), PWM_LOW, PWM_HIGH)); - thruster_right.writeMicroseconds(constrain(mb.Hreg(reg_thruster_right), PWM_LOW, PWM_HIGH)); - delay (15); -} \ No newline at end of file diff --git a/etc/arduino/arduino.ino b/etc/arduino/arduino.ino new file mode 100644 index 0000000000000000000000000000000000000000..52800fee2be8907ab21ce99bf759f020dc0630f6 --- /dev/null +++ b/etc/arduino/arduino.ino @@ -0,0 +1,113 @@ +#include +#include +#include +#include + +#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 +#define ADC_TO_VOLTAGE 5.0 / 1024 + +// echosounder uart +#define RX_PIN 9 +#define TX_PIN 6 +#define ECHO_PACKAGE_SIZE 4 + +//Modbus registers +#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) * ADC_TO_VOLTAGE) / (V_R2 / (V_R1 + V_R2)); + float current = (analogRead(CURRENT_PIN) * ADC_TO_VOLTAGE) / ((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