diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/EV3way_MonoBrick_sample.sln b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/EV3way_MonoBrick_sample.sln deleted file mode 100644 index 65fe7f8..0000000 --- a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/EV3way_MonoBrick_sample.sln +++ /dev/null @@ -1,173 +0,0 @@ - -Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 2012 -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ev3way_monobrick_sample", "ev3way_monobrick_sample\ev3way_monobrick_sample.csproj", "{D143CFF0-F2F5-474A-B631-698EC2E98A12}" -EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "AOTCompileApp", "AOTCompileApp\AOTCompileApp.csproj", "{FCEE4FA1-741D-41EA-A24A-804F2BC2A6D4}" -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|x86 = Debug|x86 - Release|x86 = Release|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {D143CFF0-F2F5-474A-B631-698EC2E98A12}.Debug|x86.ActiveCfg = Debug|x86 - {D143CFF0-F2F5-474A-B631-698EC2E98A12}.Debug|x86.Build.0 = Debug|x86 - {D143CFF0-F2F5-474A-B631-698EC2E98A12}.Release|x86.ActiveCfg = Release|x86 - {D143CFF0-F2F5-474A-B631-698EC2E98A12}.Release|x86.Build.0 = Release|x86 - {FCEE4FA1-741D-41EA-A24A-804F2BC2A6D4}.Debug|x86.ActiveCfg = Debug|x86 - {FCEE4FA1-741D-41EA-A24A-804F2BC2A6D4}.Debug|x86.Build.0 = Debug|x86 - {FCEE4FA1-741D-41EA-A24A-804F2BC2A6D4}.Release|x86.ActiveCfg = Release|x86 - {FCEE4FA1-741D-41EA-A24A-804F2BC2A6D4}.Release|x86.Build.0 = Release|x86 - EndGlobalSection - GlobalSection(MonoDevelopProperties) = preSolution - Policies = $0 - $0.DotNetNamingPolicy = $1 - $1.DirectoryNamespaceAssociation = None - $1.ResourceNamePolicy = FileFormatDefault - $0.TextStylePolicy = $2 - $2.inheritsSet = null - $2.scope = text/x-csharp - $0.CSharpFormattingPolicy = $3 - $3.AfterDelegateDeclarationParameterComma = True - $3.inheritsSet = Mono - $3.inheritsScope = text/x-csharp - $3.scope = text/x-csharp - $0.TextStylePolicy = $4 - $4.FileWidth = 120 - $4.TabsToSpaces = False - $4.EolMarker = Windows - $4.inheritsSet = VisualStudio - $4.inheritsScope = text/plain - $4.scope = text/plain - $0.StandardHeader = $5 - $5.Text = - $5.IncludeInNewFiles = True - $0.NameConventionPolicy = $6 - $6.Rules = $7 - $7.NamingRule = $8 - $8.Name = Namespaces - $8.AffectedEntity = Namespace - $8.VisibilityMask = VisibilityMask - $8.NamingStyle = PascalCase - $8.IncludeInstanceMembers = True - $8.IncludeStaticEntities = True - $7.NamingRule = $9 - $9.Name = Types - $9.AffectedEntity = Class, Struct, Enum, Delegate - $9.VisibilityMask = Public - $9.NamingStyle = PascalCase - $9.IncludeInstanceMembers = True - $9.IncludeStaticEntities = True - $7.NamingRule = $10 - $10.Name = Interfaces - $10.RequiredPrefixes = $11 - $11.String = I - $10.AffectedEntity = Interface - $10.VisibilityMask = Public - $10.NamingStyle = PascalCase - $10.IncludeInstanceMembers = True - $10.IncludeStaticEntities = True - $7.NamingRule = $12 - $12.Name = Attributes - $12.RequiredSuffixes = $13 - $13.String = Attribute - $12.AffectedEntity = CustomAttributes - $12.VisibilityMask = Public - $12.NamingStyle = PascalCase - $12.IncludeInstanceMembers = True - $12.IncludeStaticEntities = True - $7.NamingRule = $14 - $14.Name = Event Arguments - $14.RequiredSuffixes = $15 - $15.String = EventArgs - $14.AffectedEntity = CustomEventArgs - $14.VisibilityMask = Public - $14.NamingStyle = PascalCase - $14.IncludeInstanceMembers = True - $14.IncludeStaticEntities = True - $7.NamingRule = $16 - $16.Name = Exceptions - $16.RequiredSuffixes = $17 - $17.String = Exception - $16.AffectedEntity = CustomExceptions - $16.VisibilityMask = VisibilityMask - $16.NamingStyle = PascalCase - $16.IncludeInstanceMembers = True - $16.IncludeStaticEntities = True - $7.NamingRule = $18 - $18.Name = Methods - $18.AffectedEntity = Methods - $18.VisibilityMask = Protected, Public - $18.NamingStyle = PascalCase - $18.IncludeInstanceMembers = True - $18.IncludeStaticEntities = True - $7.NamingRule = $19 - $19.Name = Static Readonly Fields - $19.AffectedEntity = ReadonlyField - $19.VisibilityMask = Protected, Public - $19.NamingStyle = PascalCase - $19.IncludeInstanceMembers = False - $19.IncludeStaticEntities = True - $7.NamingRule = $20 - $20.Name = Fields - $20.AffectedEntity = Field - $20.VisibilityMask = Protected, Public - $20.NamingStyle = PascalCase - $20.IncludeInstanceMembers = True - $20.IncludeStaticEntities = True - $7.NamingRule = $21 - $21.Name = ReadOnly Fields - $21.AffectedEntity = ReadonlyField - $21.VisibilityMask = Protected, Public - $21.NamingStyle = PascalCase - $21.IncludeInstanceMembers = True - $21.IncludeStaticEntities = False - $7.NamingRule = $22 - $22.Name = Constant Fields - $22.AffectedEntity = ConstantField - $22.VisibilityMask = Protected, Public - $22.NamingStyle = PascalCase - $22.IncludeInstanceMembers = True - $22.IncludeStaticEntities = True - $7.NamingRule = $23 - $23.Name = Properties - $23.AffectedEntity = Property - $23.VisibilityMask = Protected, Public - $23.NamingStyle = PascalCase - $23.IncludeInstanceMembers = True - $23.IncludeStaticEntities = True - $7.NamingRule = $24 - $24.Name = Events - $24.AffectedEntity = Event - $24.VisibilityMask = Protected, Public - $24.NamingStyle = PascalCase - $24.IncludeInstanceMembers = True - $24.IncludeStaticEntities = True - $7.NamingRule = $25 - $25.Name = Enum Members - $25.AffectedEntity = EnumMember - $25.VisibilityMask = VisibilityMask - $25.NamingStyle = PascalCase - $25.IncludeInstanceMembers = True - $25.IncludeStaticEntities = True - $7.NamingRule = $26 - $26.Name = Parameters - $26.AffectedEntity = Parameter - $26.VisibilityMask = VisibilityMask - $26.NamingStyle = CamelCase - $26.IncludeInstanceMembers = True - $26.IncludeStaticEntities = True - $7.NamingRule = $27 - $27.Name = Type Parameters - $27.RequiredPrefixes = $28 - $28.String = T - $27.AffectedEntity = TypeParameter - $27.VisibilityMask = VisibilityMask - $27.NamingStyle = PascalCase - $27.IncludeInstanceMembers = True - $27.IncludeStaticEntities = True - $0.VersionControlPolicy = $29 - $29.inheritsSet = Mono - EndGlobalSection -EndGlobal diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/EV3way_MonoBrick_sample.userprefs b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/EV3way_MonoBrick_sample.userprefs deleted file mode 100644 index 59dfcaa..0000000 --- a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/EV3way_MonoBrick_sample.userprefs +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3/Balancer.cs b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3/Balancer.cs deleted file mode 100644 index f44e767..0000000 --- a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3/Balancer.cs +++ /dev/null @@ -1,304 +0,0 @@ -/** -****************************************************************************** -** FILE NAME : Balancer.cs -** -** ABSTRUCT : Two wheeled self-balancing robot "NXTway-GS" balance control program. -** NXTway-GS balance control algorithum is based on modern control theory -** which is servo control (state + integral feedback). -** To develop the controller and indentify the plant, The MathWorks -** MATLAB&Simulink had been used and this design methodology is -** called MBD (Model-Based Design/Development). This C source code -** is automatically generated from a Simulink model by using standard features -** of Real-Time Workshop Embedded Coder. All control parameters need to be defined -** by user and the sample parameters are defined in nxtOSEK\samples\nxtway_gs\balancer_param.c. -** For more detailed information about the controller alogorithum, please check: -** English : http://www.mathworks.com/matlabcentral/fileexchange/19147-nxtway-gs--self-balancing-two-wheeled-robot--controller-design -** Japanese: http://www.cybernet.co.jp/matlab/library/library/detail.php?id=TA060 -** -** MODEL INFO: -** MODEL NAME : balancer.mdl -** VERSION : 1.893 -** HISTORY : y_yama - Tue Sep 25 11:37:09 2007 -** takashic - Sun Sep 28 17:50:53 2008 -** Tadashi Huruya - Mon Feb 09 21:36:00 2015 -** ported from balancer.c -*** -** Copyright (c) 2009-2016 MathWorks, Inc. -** All rights reserved. -****************************************************************************** -**/ -/** -****************************************************************************** -** ファイル名 : Balancer.cs -** -** 概要 : 2輪型倒立振子ロボット「NXTway-GS」バランス制御プログラム -** NXTway-GSのバランス制御には、サーボ制御(状態 + 積分フィードバック) -** という現代制御を適用しています。制御対象の同定および制御器の開発 -** にはThe MathWorks社のMATLAB&Simulinkという製品を使用した、 -** MBD(モデルベースデザイン/開発)開発手法を用いています。このCプログラムは -** SimulinkモデルからReal-Time Workshop Embedded Coderコード生成標準機能を -** 使用して自動生成されたものです。バランス制御器の制御パラメータについては -** ユーザーハンドコード側で定義する必要があります。定義例として、 -** nxtOSEK\samples\nxtway_gs\balancer_param.cを参照してください。 -** バランス制御アルゴリズムの詳細情報につきましては -** 日本語: http://www.cybernet.co.jp/matlab/library/library/detail.php?id=TA060 -** 英語 : http://www.mathworks.com/matlabcentral/fileexchange/19147-nxtway-gs--self-balancing-two-wheeled-robot--controller-design -** を参照してください。 -** -** モデル関連情報: -** モデル名 : balancer.mdl -** バージョン : 1.893 -** 履歴 : y_yama - Tue Sep 25 11:37:09 2007 -** takashic - Sun Sep 28 17:50:53 2008 -** Tadashi Huruya - Mon Feb 09 21:36:00 2015 -** balancer.cを移植 -** -** Copyright (c) 2009-2015 MathWorks, Inc. -** All rights reserved. -****************************************************************************** -**/ - -using System; -using System.Threading; - -using MonoBrickFirmware; -using MonoBrickFirmware.Display.Dialogs; -using MonoBrickFirmware.Display; -using MonoBrickFirmware.Movement; -using MonoBrickFirmware.Sensors; - -namespace ETRobocon.EV3 -{ - class Balancer - { - const float CMD_MAX = 100.0F; /* 前進/旋回命令絶対最大値 */ - const float DEG2RAD = 0.01745329238F; /* 角度単位変換係数(=pi/180) */ - const float EXEC_PERIOD = 0.00400000019F; /* バランス制御実行周期(秒) */ - - const float A_D = 0.8F; /* low pass filter gain for motors average count */ - const float A_R = 0.996F; /* low pass filter gain for motors target count */ - /* servo control state feedback gain for NXT standard tire */ - static float[] K_F = new float[4] {-0.870303F, -31.9978F, -1.1566F * 0.6F, -2.78873F}; - const float K_I = -0.44721F; /* servo control integral gain */ - const float K_PHIDOT = 25.0F * 2.5F; /* turn target speed gain */ - const float K_THETADOT = 7.5F; /* forward target speed gain */ - - const float BATTERY_GAIN = 0.001089F; /* battery voltage gain for motor PWM outputs */ - const float BATTERY_OFFSET = 0.625F; /* battery voltage offset for motor PWM outputs */ - - static float ud_err_theta; /* 左右車輪の平均回転角度(θ)目標誤差状態値 */ - static float ud_psi; /* 車体ピッチ角度(ψ)状態値 */ - static float ud_theta_lpf; /* 左右車輪の平均回転角度(θ)状態値 */ - static float ud_theta_ref; /* 左右車輪の目標平均回転角度(θ)状態値 */ - static float ud_thetadot_cmd_lpf; /* 左右車輪の目標平均回転角速度(dθ/dt)状態値 */ - - public static void control(float args_cmd_forward, float args_cmd_turn, float - args_gyro, float args_gyro_offset, float - args_theta_m_l, float args_theta_m_r, float - args_battery, out sbyte ret_pwm_l, out sbyte ret_pwm_r) - { - float tmp_theta; - float tmp_theta_lpf; - float tmp_pwm_r_limiter; - float tmp_psidot; - float tmp_pwm_turn; - float tmp_pwm_l_limiter; - float tmp_thetadot_cmd_lpf; - float[] tmp = new float[4]; - float[] tmp_theta_0 = new float[4]; - int tmp_0; - - /* Sum: '/Sum' incorporates: - * Constant: '/Constant6' - * Constant: '/Constant' - * Constant: '/Constant1' - * Gain: '/Gain1' - * Gain: '/Gain2' - * Inport: '/cmd_forward' - * Product: '/Divide' - * Product: '/Product' - * Sum: '/Sum1' - * UnitDelay: '/Unit Delay' - */ - tmp_thetadot_cmd_lpf = (((args_cmd_forward / CMD_MAX) * K_THETADOT) * (1.0F - - A_R)) + (A_R * ud_thetadot_cmd_lpf); - - /* Gain: '/Gain' incorporates: - * Gain: '/deg2rad' - * Gain: '/deg2rad1' - * Inport: '/theta_m_l' - * Inport: '/theta_m_r' - * Sum: '/Sum1' - * Sum: '/Sum4' - * Sum: '/Sum6' - * UnitDelay: '/Unit Delay' - */ - tmp_theta = (((DEG2RAD * args_theta_m_l) + ud_psi) + ((DEG2RAD * - args_theta_m_r) + ud_psi)) * 0.5F; - - /* Sum: '/Sum' incorporates: - * Constant: '/Constant' - * Constant: '/Constant1' - * Gain: '/Gain2' - * Product: '/Product' - * Sum: '/Sum1' - * UnitDelay: '/Unit Delay' - */ - tmp_theta_lpf = ((1.0F - A_D) * tmp_theta) + (A_D * ud_theta_lpf); - - /* Gain: '/deg2rad2' incorporates: - * Inport: '/gyro' - * Inport: '/gyro_offset' - * Sum: '/Sum2' - */ - tmp_psidot = (args_gyro - args_gyro_offset) * DEG2RAD; - - /* Gain: '/Gain' incorporates: - * Constant: '/Constant2' - * Constant: '/Constant3' - * Constant: '/Constant' - * Constant: '/Constant' - * Gain: '/FeedbackGain' - * Gain: '/IntegralGain' - * Gain: '/Gain3' - * Inport: '/battery' - * Product: '/Product' - * Product: '/Product' - * Sum: '/Sum2' - * Sum: '/sum_err' - * Sum: '/Sum2' - * Sum: '/Sum' - * UnitDelay: '/Unit Delay' - * UnitDelay: '/Unit Delay' - * UnitDelay: '/Unit Delay' - * UnitDelay: '/Unit Delay' - */ - tmp[0] = ud_theta_ref; - tmp[1] = 0.0F; - tmp[2] = tmp_thetadot_cmd_lpf; - tmp[3] = 0.0F; - tmp_theta_0[0] = tmp_theta; - tmp_theta_0[1] = ud_psi; - tmp_theta_0[2] = (tmp_theta_lpf - ud_theta_lpf) / EXEC_PERIOD; - tmp_theta_0[3] = tmp_psidot; - tmp_pwm_r_limiter = 0.0F; - for (tmp_0 = 0; tmp_0 < 4; tmp_0++) { - tmp_pwm_r_limiter += (tmp[tmp_0] - tmp_theta_0[tmp_0]) * K_F[(tmp_0)]; - } - - tmp_pwm_r_limiter = (((K_I * ud_err_theta) + tmp_pwm_r_limiter) / - ((BATTERY_GAIN * args_battery) - BATTERY_OFFSET)) * - 100.0F; - - /* Gain: '/Gain2' incorporates: - * Constant: '/Constant1' - * Inport: '/cmd_turn' - * Product: '/Divide1' - */ - tmp_pwm_turn = (args_cmd_turn / CMD_MAX) * K_PHIDOT; - - /* Sum: '/Sum' */ - tmp_pwm_l_limiter = tmp_pwm_r_limiter + tmp_pwm_turn; - - /* Saturate: '/pwm_l_limiter' */ - tmp_pwm_l_limiter = rt_SATURATE(tmp_pwm_l_limiter, -100.0F, 100.0F); - - /* Outport: '/pwm_l' incorporates: - * DataTypeConversion: '/Data Type Conversion' - */ - ret_pwm_l = (sbyte)tmp_pwm_l_limiter; - - /* Sum: '/Sum1' */ - tmp_pwm_r_limiter -= tmp_pwm_turn; - - /* Saturate: '/pwm_r_limiter' */ - tmp_pwm_r_limiter = rt_SATURATE(tmp_pwm_r_limiter, -100.0F, 100.0F); - - /* Outport: '/pwm_r' incorporates: - * DataTypeConversion: '/Data Type Conversion6' - */ - ret_pwm_r = (sbyte)tmp_pwm_r_limiter; - - /* Sum: '/Sum' incorporates: - * Gain: '/Gain' - * UnitDelay: '/Unit Delay' - */ - tmp_pwm_l_limiter = (EXEC_PERIOD * tmp_thetadot_cmd_lpf) + ud_theta_ref; - - /* Sum: '/Sum' incorporates: - * Gain: '/Gain' - * UnitDelay: '/Unit Delay' - */ - tmp_pwm_turn = (EXEC_PERIOD * tmp_psidot) + ud_psi; - - /* Sum: '/Sum' incorporates: - * Gain: '/Gain' - * Sum: '/Sum1' - * UnitDelay: '/Unit Delay' - * UnitDelay: '/Unit Delay' - */ - tmp_pwm_r_limiter = ((ud_theta_ref - tmp_theta) * EXEC_PERIOD) + - ud_err_theta; - - /* user code (Update function Body) */ - /* System '' */ - /* 次回演算用状態量保存処理 */ - - /* Update for UnitDelay: '/Unit Delay' */ - ud_err_theta = tmp_pwm_r_limiter; - - /* Update for UnitDelay: '/Unit Delay' */ - ud_theta_ref = tmp_pwm_l_limiter; - - /* Update for UnitDelay: '/Unit Delay' */ - ud_thetadot_cmd_lpf = tmp_thetadot_cmd_lpf; - - /* Update for UnitDelay: '/Unit Delay' */ - ud_psi = tmp_pwm_turn; - - /* Update for UnitDelay: '/Unit Delay' */ - ud_theta_lpf = tmp_theta_lpf; - } - - /* Model initialize function */ - public static void init() - { - /* Registration code */ - - /* states (dwork) */ - - /* custom states */ - ud_err_theta = 0.0F; - ud_theta_ref = 0.0F; - ud_thetadot_cmd_lpf = 0.0F; - ud_psi = 0.0F; - ud_theta_lpf = 0.0F; - } - - private static float rt_SATURATE(float sig, float ll, float ul) - { - return (((sig) >= (ul)) ? (ul) : (((sig) <= (ll)) ? (ll) : (sig)) ); - } - - //***************************************************************************** - // 関数名 : backlash_cancel - // 引数 : lpwm (左モーターPWM値 ※前回の出力値) - // rpwm (右モーターPWM値 ※前回の出力値) - // lenc (左モーターエンコーダー値) - // renc (右モーターエンコーダー値) - // 返り値 : なし - // 概要 : 直近のPWM値に応じてエンコーダー値にバックラッシュ分の値を追加します。 - //***************************************************************************** - public static void backlash_cancel(sbyte lpwm, sbyte rpwm, ref int lenc, ref int renc) - { - const int BACKLASHHALF = 4; // バックラッシュの半分[deg] - - if (lpwm < 0) lenc += BACKLASHHALF; - else if (lpwm > 0) lenc -= BACKLASHHALF; - - if (rpwm < 0) renc += BACKLASHHALF; - else if (rpwm > 0) renc -= BACKLASHHALF; - } - } -} - diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3/Brick.cs b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3/Brick.cs deleted file mode 100644 index 8b616c0..0000000 --- a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3/Brick.cs +++ /dev/null @@ -1,189 +0,0 @@ -//Brick class (get battery voltage, power off ,reboot, exit and change hostname ) -// by jtFuruhata 2014 - -using System; -using System.IO; -using MonoBrickFirmware.Native; - -namespace ETRobocon.EV3 -{ - public class Brick - { - protected const int ANALOG_SIZE = 32; - protected const int ANALOG_BAT_CUR_OFF = 28; - protected const int ANALOG_BAT_V_OFF = 30; - - protected const float AMP_CIN = 22.0f; - protected const float VCE = 0.05f; - protected const float AMP_VIN = 0.5f; - - protected const float ADC_REF = 5.0f; // 5.0 Volts - protected const int ADC_RES = 4095; - - protected const string HOME = "/home/root"; - protected const string LEJOS_HOME = HOME + "/lejos/bin"; - protected const string START_NETWORK = LEJOS_HOME + "/startnetwork"; - protected const string START_NETWORK_ORG = START_NETWORK + ".org"; - protected const string START_BT = LEJOS_HOME + "/startbt"; - protected const string START_BT_ORG = START_BT + ".org"; - protected const string MONO_HOME = "/usr/local/bin"; - protected const string HOSTNAME = "/hostname"; - protected const string SYSTEM_HOSTNAME = "/etc" + HOSTNAME; - protected const string LEJOS_HOSTNAME = LEJOS_HOME + HOSTNAME; - protected const string BTPIN = "/btpin"; - protected const string SYSTEM_BTPIN = "/etc/bluetooth" + BTPIN; - protected const string LEJOS_BTPIN = LEJOS_HOME + BTPIN; - - protected static Brick Instance; - protected UnixDevice dev; - protected MemoryArea batteryMem; - - static Brick () - { - Brick.Instance = new Brick (); - } - - protected Brick () - { - this.dev = new UnixDevice ("/dev/lms_analog"); - this.batteryMem = this.dev.MMap (ANALOG_SIZE, 0); - } - - protected static short getBatteryCurrentRaw () - { - return System.BitConverter.ToInt16 (Brick.Instance.batteryMem.Read (ANALOG_BAT_CUR_OFF, 2), 0); - } - - protected static short getBatteryVoltageRaw () - { - return System.BitConverter.ToInt16 (Brick.Instance.batteryMem.Read (ANALOG_BAT_V_OFF, 2), 0); - } - - protected static float convert (int val) - { - return (float)val * ADC_REF / ADC_RES; - } - - protected static int shell (string fileName, string arguments = "") - { - return MonoBrickFirmware.Native.ProcessHelper.RunAndWaitForProcess (fileName, arguments); - } - - /// - /// バッテリの電源電圧をボルト単位で取得する - /// - /// 電圧(V) - public static float GetVoltage () - { - float CinV = convert(Brick.getBatteryCurrentRaw())/AMP_CIN; - return convert(Brick.getBatteryVoltageRaw())/AMP_VIN + CinV + VCE; - } - - /// - /// バッテリの電源電圧をミリボルト単位で取得する - /// - /// 電圧(mV) - public static int GetVoltageMilliVolt () - { - return (int)(Brick.GetVoltage()*1000f); - } - - /// - /// EV3インテリジェントブリックの電源を切る - /// - public static void Poweroff () - { - MonoBrickFirmware.Native.ProcessHelper.StartProcess ("halt", "-p"); - } - - /// - /// EV3インテリジェントブリックを再起動する - /// - public static void Reboot () - { - MonoBrickFirmware.Native.ProcessHelper.StartProcess ("reboot"); - } - - /// - /// ユーザアプリを終了し、MonoBrickメインメニューへ戻る - /// - public static void ExitToMenu () - { - MonoBrickFirmware.Native.ProcessHelper.StartProcess (MONO_HOME + "/mono", - "--full-aot " + MONO_HOME + "/StartupApp.exe"); - } - - /// - /// EV3インテリジェントブリックのホスト名を取得する - /// - /// ホスト名 - public static string GetName () - { - return MonoBrickFirmware.Native.ProcessHelper.RunAndWaitForProcessWithOutput ("hostname"); - } - - /// - /// EV3インテリジェントブリックのホスト名およびBluetoothデバイス名を設定する - /// - /// 設定するホスト名（デバイス名） - public static void SetName (string name) - { - FileStream fs = File.Create (LEJOS_HOSTNAME); - StreamWriter sw = new StreamWriter (fs); - sw.WriteLine (name); - sw.Close (); - fs.Close (); - Brick.shell ("cp", " -f " + LEJOS_HOSTNAME + " " +SYSTEM_HOSTNAME); - Brick.shell ("hostname", name); - Brick.shell ("hciconfig", "hci0 name " + name); - } - - /// - /// Bluetooth関係のETロボコン拡張機能を有効にする - /// - public static void InstallETRoboExt () - { - if (!File.Exists (START_NETWORK_ORG)) { - Brick.shell ("cp", "-f " + START_NETWORK + " " + START_NETWORK_ORG); - } - FileStream fs = File.Create (START_NETWORK); - StreamWriter sw = new StreamWriter (fs); - sw.WriteLine ("#! /bin/sh"); - sw.WriteLine ("HOSTNAME=`cat " + LEJOS_HOSTNAME + "`"); - sw.WriteLine ("cp -f " + LEJOS_HOSTNAME + " " +SYSTEM_HOSTNAME); - sw.WriteLine ("hostname ${HOSTNAME}"); - sw.WriteLine (START_NETWORK_ORG); - sw.Close (); - fs.Close (); - - if (!File.Exists (START_BT_ORG)) { - Brick.shell ("cp", "-f " + START_BT + " " + START_BT_ORG); - } - fs = File.Create (START_BT); - sw = new StreamWriter (fs); - sw.WriteLine ("#! /bin/sh"); - sw.WriteLine ("cp -f " + LEJOS_BTPIN + " " + SYSTEM_BTPIN); - sw.WriteLine ("hciconfig hci0 auth"); - sw.WriteLine (START_BT_ORG); - sw.Close (); - fs.Close (); - - if (!File.Exists (LEJOS_BTPIN)) { - fs = File.Create (LEJOS_BTPIN); - sw = new StreamWriter (fs); - sw.WriteLine ("1234"); - sw.Close (); - fs.Close (); - } - - if (!File.Exists (LEJOS_HOSTNAME)) { - fs = File.Create (LEJOS_HOSTNAME); - sw = new StreamWriter (fs); - sw.WriteLine ("EV3"); - sw.Close (); - fs.Close (); - } - } - } -} - diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3body.cs b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3body.cs deleted file mode 100644 index ba55882..0000000 --- a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/EV3body.cs +++ /dev/null @@ -1,31 +0,0 @@ -using System; -using System.Net.Sockets; - -using MonoBrickFirmware.Movement; -using MonoBrickFirmware.Sensors; - -namespace ETRobocon.EV3.Sample -{ - struct EV3body{ - //モータオブジェクト - public Motor motorL; - public Motor motorR; - public Motor motorT; - //センサーオブジェクト - public EV3TouchSensor touch; - public EV3UltrasonicSensor sonar; - public EV3ColorSensor color; - public EV3GyroSensor gyro; - - public static void init(ref EV3body body){ - body.motorL = new Motor (MotorPort.OutC); - body.motorR = new Motor (MotorPort.OutB); - body.motorT = new Motor (MotorPort.OutA); - body.touch = new EV3TouchSensor (SensorPort.In1); - body.sonar = new EV3UltrasonicSensor (SensorPort.In2, UltraSonicMode.Centimeter); // return [mm] - body.color = new EV3ColorSensor (SensorPort.In3, ColorMode.Reflection); - body.gyro = new EV3GyroSensor (SensorPort.In4, GyroMode.AngularVelocity); - } - } -} - diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/Main.cs b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/Main.cs deleted file mode 100644 index a8e561f..0000000 --- a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/Main.cs +++ /dev/null @@ -1,247 +0,0 @@ -using System; -using System.Net; -using System.Net.Sockets; -using System.Threading; -using System.Diagnostics; - -using MonoBrickFirmware.Display.Dialogs; -using MonoBrickFirmware.Display; - -using ETRobocon.EV3; - -// 2輪倒立振子ライントレースロボットの MonoBrick 用 c# サンプルプログラム。 -namespace ETRobocon.EV3.Sample -{ - class MainClass - { - //下記のパラメータは個体/環境に合わせて変更する必要があります - const int GYRO_OFFSET = 0; //ジャイロセンサオフセット値 - const int LIGHT_BLACK = 0; //白色の光センサ値 - const int LIGHT_WHITE = 40; //黒色の光センサ値 - const int SONAR_ALERT_DISTANCE = 300; //超音波センサによる障害物検知距離 [mm] - const int TAIL_ANGLE_STAND_UP = 94; //完全停止時の角度[deg] - const int TAIL_ANGLE_DRIVE = 3; //バランス走行時の角度[deg] - const float P_GAIN = 2.5F; //完全停止用モータ制御比例係数 - const int PWM_ABS_MAX = 60; //完全停止用モータ制御PWM絶対最大値 - const int SOCKET_PORT = 7360; - const int REMOTE_COMMAND_CLOSE = 0; - const int REMOTE_COMMAND_START = 'g'; // 'g' - const int REMOTE_COMMAND_STOP = 's'; // 's' - - public static void Main() - { - // 構造体の宣言と初期化 - var body = new EV3body (); - EV3body.init (ref body); - - // Bluetooth関係のETロボコン拡張機能を有効にする - Brick.InstallETRoboExt (); - - // リモート接続 - NetworkStream connection = connect(); - - // センサーおよびモータに対して初回アクセスをしておく - body.color.Read(); - body.sonar.Read(); - body.gyro.Read (); - body.motorL.SetPower (0); - body.motorR.SetPower (0); - body.motorT.SetPower (0); - - body.motorL.ResetTacho (); - body.motorR.ResetTacho (); - body.motorT.ResetTacho (); - Balancer.init (); - - // スタート待ち - wait_start(body, connection); - - var dialogRun = new InfoDialog ("Running", false); - dialogRun.Show ();//Wait for enter to be pressed - - try{ - run(body, connection); - }catch(Exception){ - var dialogE = new InfoDialog ("Exception.", false); - dialogE.Show();//Wait for enter to be pressed - } - - body.motorL.Off (); - body.motorR.Off (); - body.motorT.Off (); - - // ソケットを閉じる - if (connection != null) { - connection.Close (); - } - - Lcd.Instance.Clear (); - Lcd.Instance.Update (); - - if (Debugger.IsAttached) { - Brick.ExitToMenu (); // MonoBrickメインメニューへ戻る - } - } - - static NetworkStream connect(){ - // サーバとの接続設定 - NetworkStream connection; - IPAddress ipAddr = IPAddress.Parse("10.0.1.1"); - - var listener = new TcpListener (ipAddr, SOCKET_PORT); - listener.Start(); // クライアントからの受信接続要求の待機を開始 - - var dialogCON = new InfoDialog ("Please connect...", false); - dialogCON.Show(); // Wait for enter to be pressed - - try { - Socket sock = listener.AcceptSocket(); // 接続要求の受け入れ - connection = new NetworkStream(sock, true); - } catch (SocketException) { - var dialogE = new InfoDialog ("Connect is Failed.", false); - dialogE.Show(); // Wait for enter to be pressed - connection = null; - } - listener.Stop(); - return connection; - } - - static void wait_start(EV3body body, NetworkStream connection){ - //スタート待ち - var dialogSTART = new InfoDialog ("Touch to START", false); - dialogSTART.Show (); // Wait for enter to be pressed - - while (!body.touch.IsPressed()) { - tail_control(body, TAIL_ANGLE_STAND_UP); //完全停止用角度に制御 - if (checkRemoteCommand(connection, REMOTE_COMMAND_START)) - break; // PC で 'g' キーが押された - Thread.Sleep (4); - } - - while (body.touch.IsPressed ()) { - tail_control(body, TAIL_ANGLE_STAND_UP); //完全停止用角度に制御 - Thread.Sleep (4); - } - } - - static void run(EV3body body, NetworkStream connection){ - // 電圧を取得 - int battery = Brick.GetVoltageMilliVolt(); - - sbyte forward; - sbyte turn; - int counter = 0; - bool alert = false; - - sbyte oldPwmL, oldPwmR; - - while (!body.touch.IsPressed ()) - { - tail_control(body, TAIL_ANGLE_DRIVE); // バランス走行用角度に制御 - if (checkRemoteCommand(connection, REMOTE_COMMAND_STOP)) - break; // PC で 's' キー押されたら走行終了 - - if (++counter >= 40/4) { - alert = sonar_alert (body); - counter = 0; - } - if (alert) { - forward = 0; - turn = 0; - } else { - forward = 50; - turn = (body.color.Read () >= (LIGHT_BLACK + LIGHT_WHITE) / 2) ? (sbyte)50 : (sbyte)-50; - } - - int gyroNow = body.gyro.Read(); - int theTaL = body.motorL.GetTachoCount(); - int theTaR = body.motorR.GetTachoCount(); - - // バックラッシュをキャンセル - Balancer.backlash_cancel(oldPwmL, oldPwmR, ref theTaL, ref theTaR); - - sbyte pwmL, pwmR; - Balancer.control ( - (float)forward, (float)turn, (float)gyroNow, (float)GYRO_OFFSET, (float)theTaL, (float)theTaR, (float)battery, - out pwmL, out pwmR - ); - - oldPwmL = pwmL; - oldPwmR = pwmR; - - if (pwmL == 0) { - body.motorL.Brake(); - } else { - body.motorL.SetPower(pwmL); - } - if (pwmR == 0) { - body.motorR.Brake(); - } else { - body.motorR.SetPower(pwmR); - } - - // バランス制御のみだと3msecで安定 - // 尻尾制御と障害物検知を使用する場合2msecで安定 - Thread.Sleep(2); - } - } - - /* - * 超音波センサによる障害物検知 - * @return true(障害物あり)/false(障害物無し) - */ - static bool sonar_alert(EV3body body) - { - int distance = body.sonar.Read(); - if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0)){ - return true; /* 障害物を検知 */ - }else{ - return false; /* 障害物無し */ - } - } - - /* - * 走行体完全停止用モータの角度制御 - * @param angle モータ目標角度[度] - */ - static void tail_control(EV3body body, int angle) - { - float pwm = (float)(angle - body.motorT.GetTachoCount ()) * P_GAIN; // 比例制御 - // PWM出力飽和処理 - if (pwm > PWM_ABS_MAX) { - pwm = PWM_ABS_MAX; - } else if (pwm < -PWM_ABS_MAX) { - pwm = -PWM_ABS_MAX; - } - if ((sbyte)pwm == 0) { - body.motorT.Brake(); - } else { - body.motorT.SetPower((sbyte)pwm); - } - } - - /* - * リモートコマンドのチェック - */ - static bool checkRemoteCommand(NetworkStream connection, int command) - { - try{ - if (connection.DataAvailable) { - var buff = new byte[4]; - connection.Read(buff, 0, buff.Length); - // ネットワークバイトオーダー(big endian)で受信したため little endian に変換 - Array.Reverse(buff); // big endian -> little endian - - if (BitConverter.ToInt32(buff,0) == command) { - return true; - } - } - }catch(Exception){ - return false; - } - return false; - } - - } -} - diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/MonoBrickFirmware.dll b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/MonoBrickFirmware.dll deleted file mode 100644 index 7564e57..0000000 Binary files a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/MonoBrickFirmware.dll and /dev/null differ diff --git a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/ev3way_monobrick_sample.csproj b/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/ev3way_monobrick_sample.csproj deleted file mode 100644 index eadfb02..0000000 --- a/SampleCode/EV3way_MonoBrick_sample/EV3way_MonoBrick_Remote_2018/ev3way_monobrick_sample_2018/ev3way_monobrick_sample.csproj +++ /dev/null @@ -1,45 +0,0 @@ - - - - Debug - x86 - {803879AC-26C2-49B0-AAA0-0DE4A7C68C87};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} - {D143CFF0-F2F5-474A-B631-698EC2E98A12} - Exe - ETRobocon.EV3.Sample - EV3Way - v4.5 - - - true - full - false - bin\Debug - DEBUG; - prompt - 4 - x86 - false - - - true - bin\Release - prompt - 4 - x86 - false - - - - - MonoBrickFirmware.dll - - - - - - - - - - \ No newline at end of file diff --git a/SampleCode/EV3way_MonoBrick_sample/README.txt b/SampleCode/EV3way_MonoBrick_sample/README.txt index 2d1f911..7f42500 100644 --- a/SampleCode/EV3way_MonoBrick_sample/README.txt +++ b/SampleCode/EV3way_MonoBrick_sample/README.txt @@ -1,26 +1,49 @@ -g +使い方(2018年度版) -PDEV3{̂ƃp\R̐ڑ +１．EV3本体を起動 -EV3ƃp\RBluetoothŐڑ܂B +EV3本体を起動します。 +起動には、1分程度かかります。 -QDEV3{̑vO̎s +２．EV3本体プログラムをAOTコンパイル -MonoDevelopEV3way_MonoBrick_sample\EV3way_MonoBrick_Remote\EV3way_MonoBrick_sample.sln -JAj[usv-uStart Without Debuggingvs܂B +「Programs」-「(実行するプログラム)」-「AOT Compiie」を選択します。 -vOrhEV3ɓ]s܂B -tʂɂ́uPlease connect...v\ڑ҂ɂȂ܂B +コンパイルには、1分程度かかります。 +３．EV3本体プログラムを実行 -RDWindowsp\RvO̎s +「(実行するプログラム)」-「Run In AOT」を選択します。 -PMonoDevelopNAEV3way_MonoBrick_sample\EV3Way_MonoBrick_RemoteConsole\EV3Way_MonoBrick_RemoteConsole.slnJAj[usv-uStart Without Debuggingvs܂B +尻尾が下がりますと準備完了です。 -p\RŃvOJnAR\[ʂ\܂B -EV3̉tʂɂ́uTouch to STARTv\܂B -EV3𗧂ă^b`ZT[邩AR\[ʂgL[œ|Jn܂B -x^b`ZT[邩AR\[ʂsL[ƒ~܂B +タッチセンサーを押下すると走行を開始します。 + + +使い方(リモート用) + +１．EV3本体とパソコンの接続 + +EV3とパソコンをBluetoothで接続します。 + + +２．EV3本体側プログラムの実行 + +MonoDevelopでEV3way_MonoBrick_sample\EV3way_MonoBrick_Remote\EV3way_MonoBrick_sample.sln +を開き、メニュー「実行」-「Start Without Debugging」を実行します。 + +プログラムがビルドされEV3に転送され実行されます。 +液晶画面には「Please connect...」が表示され接続待ちになります。 + + +３．Windowsパソコン側プログラムの実行 + +もう１つMonoDevelopを起動し、EV3way_MonoBrick_sample\EV3Way_MonoBrick_RemoteConsole\EV3Way_MonoBrick_RemoteConsole.slnを開き、メニュー「実行」-「Start Without Debugging」を実行します。 + +パソコン上でプログラムが開始され、コンソール画面が表示されます。 +EV3の液晶画面には「Touch to START」が表示されます。 +EV3を立たせてタッチセンサーを押下するか、コンソール画面のgキーで倒立を開始します。 +もう一度タッチセンサーを押下するか、コンソール画面のsキーを押すと停止します。