diff --git a/Driver.py b/Driver.py index 9b4af51..7305f86 100644 --- a/Driver.py +++ b/Driver.py @@ -12,9 +12,10 @@ 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 @@ -22,19 +23,6 @@ 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,6 +118,7 @@ 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: @@ -132,13 +126,16 @@ def do_operation(self): # 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 diff --git a/README.md b/README.md index 6730381..1b45fe8 100644 --- a/README.md +++ b/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) diff --git a/config/example/example.yml b/config/example/example.yml index a30fac2..904e837 100644 --- a/config/example/example.yml +++ b/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] diff --git a/ina226.py b/ina226.py deleted file mode 100644 index efbe86e..0000000 --- a/ina226.py +++ /dev/null @@ -1,399 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding: utf-8 -*- -""" -Created on Wed May 17 23:45:12 2017 -Decription: INA226 I2C based Bi-directional Current/Power Sensor Driver for Python -@author: Srinivas Nistala -@https://github.com/neutronstriker -Ported from Arduino-INA226 -https://github.com/jarzebski/Arduino-INA226 -Original Author:Korneliusz Jarzebski -Contributor: Srinivas Nistala (aka neutronstriker) -This program is free software: you can redistribute it and/or modify -it under the terms of the version 3 GNU General Public License as -published by the Free Software Foundation. -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. -You should have received a copy of the GNU General Public License -along with this program. If not, see . -""" - - -import ctypes -import math -import sys -import time - -PYTHON_SMBUS_LIB_PRESENT = True - -try: - import smbus -except ImportError as e: - PYTHON_SMBUS_LIB_PRESENT = False - - -INA226_ADDRESS = 0x40 - -INA226_REG_CONFIG = 0x00 -INA226_REG_SHUNTVOLTAGE = 0x01 -INA226_REG_BUSVOLTAGE = 0x02 -INA226_REG_POWER = 0x03 -INA226_REG_CURRENT = 0x04 -INA226_REG_CALIBRATION = 0x05 -INA226_REG_MASKENABLE = 0x06 -INA226_REG_ALERTLIMIT = 0x07 - -INA226_BIT_SOL = 0x8000 -INA226_BIT_SUL = 0x4000 -INA226_BIT_BOL = 0x2000 -INA226_BIT_BUL = 0x1000 -INA226_BIT_POL = 0x0800 -INA226_BIT_CNVR = 0x0400 -INA226_BIT_AFF = 0x0010 -INA226_BIT_CVRF = 0x0008 -INA226_BIT_OVF = 0x0004 -INA226_BIT_APOL = 0x0002 -INA226_BIT_LEN = 0x0001 - -# enum replacement, but not truly -# now replaced class by dict because it can give me back keys -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, -) - -ina226_busConvTime_t = dict( - INA226_BUS_CONV_TIME_140US=0b000, - INA226_BUS_CONV_TIME_204US=0b001, - INA226_BUS_CONV_TIME_332US=0b010, - INA226_BUS_CONV_TIME_588US=0b011, - INA226_BUS_CONV_TIME_1100US=0b100, - INA226_BUS_CONV_TIME_2116US=0b101, - INA226_BUS_CONV_TIME_4156US=0b110, - INA226_BUS_CONV_TIME_8244US=0b111, -) - -ina226_shuntConvTime_t = dict( - INA226_SHUNT_CONV_TIME_140US=0b000, - INA226_SHUNT_CONV_TIME_204US=0b001, - INA226_SHUNT_CONV_TIME_332US=0b010, - INA226_SHUNT_CONV_TIME_588US=0b011, - INA226_SHUNT_CONV_TIME_1100US=0b100, - INA226_SHUNT_CONV_TIME_2116US=0b101, - INA226_SHUNT_CONV_TIME_4156US=0b110, - INA226_SHUNT_CONV_TIME_8244US=0b111, -) - -ina226_mode_t = dict( - INA226_MODE_POWER_DOWN=0b000, - INA226_MODE_SHUNT_TRIG=0b001, - INA226_MODE_BUS_TRIG=0b010, - INA226_MODE_SHUNT_BUS_TRIG=0b011, - INA226_MODE_ADC_OFF=0b100, - INA226_MODE_SHUNT_CONT=0b101, - INA226_MODE_BUS_CONT=0b110, - INA226_MODE_SHUNT_BUS_CONT=0b111, -) - -I2C_DRIVER = "SBC_LINUX_SMBUS" -# other I2C options -I2C_DEFAULT_CLK_KHZ = 100 -I2C_DEFAULT_BUS_NUMBER = 1 - - -class ina226: - def __init__( - self, - ina226_addr=INA226_ADDRESS, - i2c_bus_number=I2C_DEFAULT_BUS_NUMBER, - i2c_clk_Khz=I2C_DEFAULT_CLK_KHZ, - i2c_driver_type=I2C_DRIVER, - ): - - if PYTHON_SMBUS_LIB_PRESENT is False: - print( - "SMBUS lib is not installed, Please install an appropriate one and try again." - ) - sys.exit(0) - - elif i2c_driver_type == "SBC_LINUX_SMBUS": - if PYTHON_SMBUS_LIB_PRESENT is False: - print( - "PYTHON SMBUS Driver is not installed, Please install and Try again." - ) - sys.exit(0) - self.i2c_bus = smbus.SMBus(i2c_bus_number) - self.readRegister16 = self.readRegister16_SMBUS - self.writeRegister16 = self.writeRegister16_SMBUS - if i2c_clk_Khz != I2C_DEFAULT_CLK_KHZ: - print( - "Python SMBUS linux driver doesn't provide I2C CLK Freq Manipulation support yet," - ) - print("So Ignoring i2c_clk_khz param and using default.") - else: - print( - "Unknown I2C DRIVER Specified, available Options are : AARDVARK, SBC_LINUX_SMBUS" - ) - - self.ina226_address = ina226_addr - self.vBusMax = 36 - self.vShuntMax = 0.08192 - self.rShunt = 0.1 - self.currentLSB = 0 - self.powerLSB = 0 - self.iMaxPossible = 0 - - # not using with statement related code yet - - # this causes some issue may be because when exception occurs I am manually calling - # self.close() and even this function tries to call the same. Need to check. - # def __del__(self): - # self.close() - - def close(self): - self.i2c_bus.close() - - def readRegister16_SMBUS(self, register): - # higher_byte = self.i2c_bus.read_byte_data(self.ina226_address,register) - # lower_byte = self.i2c_bus.read_byte_data(self.ina226_address,register+1) - data = self.i2c_bus.read_i2c_block_data(self.ina226_address, register, 2) - higher_byte = data[0] - lower_byte = data[1] - # there is still some issue in read which we need to fix, we are not able to print negative current--done--fixed using ctypes int16 return - word_data = higher_byte << 8 | lower_byte - # return word_data - return ctypes.c_int16(word_data).value - - def writeRegister16_SMBUS(self, register, dataWord): - higher_byte = (dataWord >> 8) & 0xFF - lower_byte = dataWord & 0xFF # truncating the dataword to byte - self.i2c_bus.write_i2c_block_data( - self.ina226_address, register, [higher_byte, lower_byte] - ) - - def configure( - self, - avg=ina226_averages_t["INA226_AVERAGES_1"], - busConvTime=ina226_busConvTime_t["INA226_BUS_CONV_TIME_1100US"], - shuntConvTime=ina226_shuntConvTime_t["INA226_SHUNT_CONV_TIME_1100US"], - mode=ina226_mode_t["INA226_MODE_SHUNT_BUS_CONT"], - ): - config = 0 - config |= avg << 9 | busConvTime << 6 | shuntConvTime << 3 | mode - self.writeRegister16(INA226_REG_CONFIG, config) - return True - - def calibrate(self, rShuntValue=0.1, iMaxExcepted=2): - self.rShunt = rShuntValue - - self.iMaxPossible = self.vShuntMax / self.rShunt - - minimumLSB = float(iMaxExcepted) / 32767 - - print("minimumLSB:" + str(minimumLSB)) - - self.currentLSB = int((minimumLSB * 100000000)) - print("currentLSB:" + str(self.currentLSB)) - self.currentLSB /= 100000000.0 - self.currentLSB /= 0.0001 - self.currentLSB = math.ceil(self.currentLSB) - self.currentLSB *= 0.0001 - - self.powerLSB = self.currentLSB * 25 - - print("powerLSB:" + str(self.powerLSB)) - print("rshunt:" + str(self.rShunt)) - - calibrationValue = int( - ((0.00512) / (self.currentLSB * self.rShunt)) - ) # if we get error need to convert this to unsigned int 16 bit instead - - self.writeRegister16(INA226_REG_CALIBRATION, calibrationValue) - - return True - - def getAverages(self): - value = self.readRegister16(INA226_REG_CONFIG) - value &= 0b0000111000000000 - value >>= 9 - return value - - def getMaxPossibleCurrent(self): - return self.vShuntMax / self.rShunt - - def getMaxCurrent(self): - maxCurrent = self.currentLSB * 32767 - maxPossible = self.getMaxPossibleCurrent() - - if maxCurrent > maxPossible: - return maxPossible - else: - return maxCurrent - - def getMaxShuntVoltage(self): - maxVoltage = self.getMaxCurrent() * self.rShunt - if maxVoltage >= self.vShuntMax: - return self.vShuntMax - else: - return maxVoltage - - def getMaxPower(self): - return self.getMaxCurrent() * self.vBusMax - - def readBusPower(self): - return self.readRegister16(INA226_REG_POWER) * self.powerLSB - - def readShuntCurrent(self): - return self.readRegister16(INA226_REG_CURRENT) * self.currentLSB - - def readShuntVoltage(self): - voltage = self.readRegister16(INA226_REG_SHUNTVOLTAGE) - return voltage * 0.0000025 - - def readBusVoltage(self): - voltage = self.readRegister16(INA226_REG_BUSVOLTAGE) - return voltage * 0.00125 - - def getBusConversionTime(self): - value = self.readRegister16(INA226_REG_CONFIG) - value &= 0b0000000111000000 - value >>= 6 - return value - - def getShuntConversionTime(self): - value = self.readRegister16(INA226_REG_CONFIG) - value &= 0b0000000000111000 - value >>= 3 - return value - - def getMode(self): - value = self.readRegister16(INA226_REG_CONFIG) - value &= 0b0000000000000111 - return value - - def setMaskEnable(self, mask): - self.writeRegister16(INA226_REG_MASKENABLE, mask) - - def getMaskEnable(self): - return self.readRegister16(INA226_REG_MASKENABLE) - - def enableShuntOverLimitAlert(self): - self.writeRegister16(INA226_REG_MASKENABLE, INA226_BIT_SOL) - - def enableBusOverLimitAlert(self): - self.writeRegister16(INA226_REG_MASKENABLE, INA226_BIT_BOL) - - def enableBusUnderLimitAlert(self): - self.writeRegister16(INA226_REG_MASKENABLE, INA226_BIT_BUL) - - def enableOverPowerLimitAlert(self): - self.writeRegister16(INA226_REG_MASKENABLE, INA226_BIT_POL) - - def enableConversionReadyAlert(self): - self.writeRegister16(INA226_REG_MASKENABLE, INA226_BIT_CNVR) - - def setBusVoltageLimit(self, voltage): - value = voltage / 0.00125 - self.writeRegister16(INA226_REG_ALERTLIMIT, value) - - def setShuntVoltageLimit(self, voltage): - value = voltage * 25000 - self.writeRegister16(INA226_REG_ALERTLIMIT, value) - - def setPowerLimit(self, watts): - value = watts / self.powerLSB - self.writeRegister16(INA226_REG_ALERTLIMIT, value) - - def setAlertInvertedPolarity(self, inverted): - temp = self.getMaskEnable() - - if inverted: - temp |= INA226_BIT_APOL - else: - temp &= ~INA226_BIT_APOL - self.setMaskEnable(temp) - - def setAlertLatch(self, latch): - temp = self.getMaskEnable() - if latch: - temp |= INA226_BIT_LEN - else: - temp &= ~INA226_BIT_LEN - self.setMaskEnable(temp) - - def isMathOverflow(self): - return (self.getMaskEnable() & INA226_BIT_OVF) == INA226_BIT_OVF - - def isAlert(self): - return (self.getMaskEnable() & INA226_BIT_AFF) == INA226_BIT_AFF - - def print_status(self): - print( - "Current: " - + str(round(self.readShuntCurrent(), 3)) - + "A" - + ", Voltage: " - + str(round(self.readBusVoltage(), 3)) - + "V" - + ", Power:" - + str(round(self.readBusPower(), 3)) - + "W" - ) - return - - -# -----------------------Demo Program-------------------------------------- - - -def demo(): - print("Configuring INA226..") - iSensor = ina226(INA226_ADDRESS, 0) - try: - iSensor.configure(avg=ina226_averages_t["INA226_AVERAGES_4"]) - iSensor.calibrate(rShuntValue=0.002, iMaxExcepted=1) - - time.sleep(1) - - print("Configuration Done") - - current = iSensor.readShuntCurrent() - - print("Current Value is " + str(current) + "A") - - print("Mode is " + str(hex(iSensor.getMode()))) - - while True: - print( - "Current: " - + str(round(iSensor.readShuntCurrent(), 3)) - + "A" - + ", Voltage: " - + str(round(iSensor.readBusVoltage(), 3)) - + "V" - + ", Power:" - + str(round(iSensor.readBusPower(), 3)) - + "W" - ) - # print "ShuntBus_Voltage: "+str(iSensor.readShuntVoltage()) - time.sleep(0.2) - - except KeyboardInterrupt as e: - print("\nCTRL^C received, Terminating..") - iSensor.close() - - except Exception as e: - print("There has been an exception, Find details below:") - print(str(e)) - iSensor.close() - - -if __name__ == "__main__": - demo() diff --git a/ina260.py b/ina260.py new file mode 100644 index 0000000..cf03b4c --- /dev/null +++ b/ina260.py @@ -0,0 +1,32 @@ +# -*- coding: utf-8 -*- +import signal +import time +from datetime import datetime + +import adafruit_ina260 +import board + +REC_START_HOUR = 8 +REC_END_HOUR = 18 + + +def task(arg1, arg2): + # 電流・電圧・電力の取得 + cur = ina260.current + vol = ina260.voltage + po = ina260.power + # 日時の取得 + _now = datetime.now() + today = _now.strftime("%Y-%m-%d") + nowtime = _now.strftime("%H:%M:%S") + print( + "日付 %s 時刻 %s 電流 %.2f mA 電圧 %.2f V 電力 %.2f mW" % (today, nowtime, cur, vol, po) + ) + + +i2c = board.I2C() +ina260 = adafruit_ina260.INA260(i2c) +signal.signal(signal.SIGALRM, task) +signal.setitimer(signal.ITIMER_REAL, 0.1, 1) # コマンドラインでCtrl+Cで止められるように +while True: + time.sleep(1) diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..7c8730c --- /dev/null +++ b/requirements.txt @@ -0,0 +1,4 @@ +adafruit-circuitpython-ina260 +board +serial +-e git+https://github.com/inmcm/micropyGPS.git \ No newline at end of file