AN中にinaを用いてモーターの出力を変化させるコードの実装

Driver.py

@@ -12,29 +12,17 @@
 import sys
 import time
 
+import adafruit_ina260
+import board
 import yaml
 
-from ina226 import ina226
 from Logger import Logger
 from Pid import PositionalPID
 from PwmOut import PwmOut
 from PwmRead import PwmRead
 from Status import Status
 from TimeManager import TimeManager
 
-INA226_ADDRESS = 0x40
-
-ina226_averages_t = dict(
-    INA226_AVERAGES_1=0b000,
-    INA226_AVERAGES_4=0b001,
-    INA226_AVERAGES_16=0b010,
-    INA226_AVERAGES_64=0b011,
-    INA226_AVERAGES_128=0b100,
-    INA226_AVERAGES_256=0b101,
-    INA226_AVERAGES_512=0b110,
-    INA226_AVERAGES_1024=0b111,
-)
-
 
 class Driver:
     def __init__(self, filename):
@@ -78,17 +66,22 @@ def __init__(self, filename):
             params["gpio"]["servo"]["out"], params["gpio"]["thruster"]["out"]
         )
 
-        # Whether experienced OR mode or not
-        self._or_experienced = False
+        optimize_params = params["optimize_thruster"]
+        self.target_voltage = optimize_params["target_voltage"]
+        self.min_current = optimize_params["min_current"]
+        self.lower_coef = optimize_params["lower_coef"]
+        self.raise_val = optimize_params["raise_val"]
+        self.initialize = False
+
+        self.current = 0
+        self.voltage = 0
+        self.power = 0
 
         # setup for ina226
         print("Configuring INA226..")
         try:
-            self.i_sensor = ina226(INA226_ADDRESS, 1)
-            self.i_sensor.configure(avg=ina226_averages_t["INA226_AVERAGES_4"])
-            self.i_sensor.calibrate(rShuntValue=0.002, iMaxExcepted=1)
-            self.i_sensor.print_status()
-            print("Mode is " + str(hex(self.i_sensor.getMode())))
+            i2c = board.I2C()
+            self.i_sensor = adafruit_ina260.INA260(i2c, 0x40)
         except:
             print("Error when configuring INA226")
 
@@ -125,20 +118,24 @@ def do_operation(self):
         while self._time_manager.in_time_limit():
             self._pwm_read.measure_pulse_width()
             self._status.read_gps()
+            self._update_ina()
             self._update_output()
 
             if time.time() - self.log_time > 1:
                 self.log_time = time.time()
                 # for test
                 self._pwm_read.print_pulse_width()
 
-                # ina226
-                if hasattr(self, "i_sensor"):
-                    self.i_sensor.print_status()
                 self._print_log()
             time.sleep(self._sleep_time)
         return
 
+    def _update_ina(self):
+        if hasattr(self, "i_sensor"):
+            self.current = self.i_sensor.current
+            self.voltage = self.i_sensor.voltage
+            self.power = self.i_sensor.power
+
     def _update_output(self):
         if self._status.has_finished:
             mode = "RC"
@@ -192,9 +189,26 @@ def _auto_navigation(self):
             target_bearing_relative, target_distance
         )
         self._pwm_out.servo_pulse_width = servo_pulse_width
-        self._pwm_out.thruster_pulse_width = 1900
+        if hasattr(self, "i_sensor"):
+            self._optimize_thruster()
+        else:
+            self._pwm_out.thruster_pulse_width = 1900
         return
 
+    def _optimize_thruster(self):
+        voltage_delta = self.target_voltage - self.voltage
+        if voltage_delta > 0:
+            self._pwm_out.thruster_pulse_width -= self.lower_coef * voltage_delta
+        elif self.current < self.min_current and not self.initialize:
+            self._pwm_out.thruster_pulse_width = 1100
+            self.initialize = True
+        elif self._pwm_out.thruster_pulse_width <= 1900:
+            self._pwm_out.thruster_pulse_width += self.raise_val
+            self.initialize = False
+        self._pwm_out.thruster_pulse_width = min(
+            max(self._pwm_out.thruster_pulse_width, 1100), 1900
+        )
+
     def _print_log(self):
         timestamp = self._status.timestamp_string
         mode = self._status.mode
@@ -212,17 +226,13 @@ def _print_log(self):
         t_longitude = target[1]
         t_idx = self._status.waypoint._index
         err = self._pid.err_back
-        if hasattr(self, "i_sensor"):
-            current = str(round(self.i_sensor.readShuntCurrent(), 3))
-            voltage = str(round(self.i_sensor.readBusVoltage(), 3))
-            power = str(round(self.i_sensor.readBusPower(), 3))
-        else:
-            current = 0
-            voltage = 0
-            power = 0
 
         # To print logdata
         print(timestamp)
+
+        print(
+            f"Current: {self.current:.3f}mA, Voltage: {self.voltage:.3f}V, Power: {self.power:.3f}mW"
+        )
         print(
             f"[{mode} MODE] LAT={latitude:.7f}, LON={longitude:.7f}, SPEED={speed:.2f} [km/h], HEADING={heading:.2f}"
         )
@@ -252,9 +262,9 @@ def _print_log(self):
             servo_pw,
             thruster_pw,
             err,
-            current,
-            voltage,
-            power,
+            self.current,
+            self.voltage,
+            self.power,
         ]
         self._logger.write(log_list)
         return

README.md

@@ -2,22 +2,20 @@
 
 This is an algorithm by which the solar boat can aim the goal automatically. This codes was made for private educational project.
 # Overview
- 
+
 todo: システムの概要
 2019年のレポート参照
- 
+
 # Requirement
- 
- 
+
+
 * python 3.8.1
   * formatter: black
-*micropyGPS
-  *https://github.com/inmcm/micropyGPS
-*serial
-  `pip install serial`
-
+
+`pip3 install -r requirements.txt`
+
 # Installation
- 
+
 mac OSの場合
 
 ## pythonのinstall
@@ -60,20 +58,20 @@ if which pyenv > /dev/null; then eval "$(pyenv init -)"; fi' >`
 
 これでターミナルから python を実行すれば 3.8.1が使えるようになる。
 
- 
+
 # Usage
 
 ```bash
 python3 main.py parameter_sample.txt
 ```
- 
+
 # Note
- 
- 
+
+
 # Author
- 
- 
+
+
 * YukiSaegusa, Amusac
 * University of Tokyo
- 
+
  [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)

config/example/example.yml

@@ -1,6 +1,6 @@
 
 time_limit: 12600
-sleep_time: 0.2
+sleep_time: 1.0
 wp_radius: 10
 pwm_range: 400 #1500 +/- pwm_range <- MAX 400
 P: 0.1
@@ -32,3 +32,8 @@ waypoints:
     name: "wp3"
     lat: 35.910887
     lon: 139.761674
+optimize_thruster:
+  target_voltage: 10 # [V]
+  min_current: 50 # [mA]
+  lower_coef: 10 # [pwm/V]
+  raise_val: 100 # [pwm]