• R/O
  • SSH
  • HTTPS

team-ncxx-sl: Commit


Commit MetaInfo

Revision410 (tree)
Time2018-09-15 16:34:44
Authorkitoman33

Log Message

(empty log message)

Change Summary

Incremental Difference

--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/app.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/app.h (revision 410)
@@ -0,0 +1,93 @@
1+/*
2+ * TOPPERS/ASP Kernel
3+ * Toyohashi Open Platform for Embedded Real-Time Systems/
4+ * Advanced Standard Profile Kernel
5+ *
6+ * Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
7+ * Toyohashi Univ. of Technology, JAPAN
8+ * Copyright (C) 2004-2010 by Embedded and Real-Time Systems Laboratory
9+ * Graduate School of Information Science, Nagoya Univ., JAPAN
10+ *
11+ * 上記著作権者は,以下の(1)?(4)の条件を満たす場合に限り,本ソフトウェ
12+ * ア(本ソフトウェアを改変したものを含む.以下同じ)を使用・複製・改
13+ * 変・再配布(以下,利用と呼ぶ)することを無償で許諾する.
14+ * (1) 本ソフトウェアをソースコードの形で利用する場合には,上記の著作
15+ * 権表示,この利用条件および下記の無保証規定が,そのままの形でソー
16+ * スコード中に含まれていること.
17+ * (2) 本ソフトウェアを,ライブラリ形式など,他のソフトウェア開発に使
18+ * 用できる形で再配布する場合には,再配布に伴うドキュメント(利用
19+ * 者マニュアルなど)に,上記の著作権表示,この利用条件および下記
20+ * の無保証規定を掲載すること.
21+ * (3) 本ソフトウェアを,機器に組み込むなど,他のソフトウェア開発に使
22+ * 用できない形で再配布する場合には,次のいずれかの条件を満たすこ
23+ * と.
24+ * (a) 再配布に伴うドキュメント(利用者マニュアルなど)に,上記の著
25+ * 作権表示,この利用条件および下記の無保証規定を掲載すること.
26+ * (b) 再配布の形態を,別に定める方法によって,TOPPERSプロジェクトに
27+ * 報告すること.
28+ * (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
29+ * 害からも,上記著作権者およびTOPPERSプロジェクトを免責すること.
30+ * また,本ソフトウェアのユーザまたはエンドユーザからのいかなる理
31+ * 由に基づく請求からも,上記著作権者およびTOPPERSプロジェクトを
32+ * 免責すること.
33+ *
34+ * 本ソフトウェアは,無保証で提供されているものである.上記著作権者お
35+ * よびTOPPERSプロジェクトは,本ソフトウェアに関して,特定の使用目的
36+ * に対する適合性も含めて,いかなる保証も行わない.また,本ソフトウェ
37+ * アの利用により直接的または間接的に生じたいかなる損害に関しても,そ
38+ * の責任を負わない.
39+ *
40+ * $Id: sample1.h 2416 2012-09-07 08:06:20Z ertl-hiro $
41+ */
42+
43+/*
44+ * サンプルプログラム(1)のヘッダファイル
45+ */
46+
47+#ifdef __cplusplus
48+extern "C" {
49+#endif
50+
51+/*
52+ * ターゲット依存の定義
53+ */
54+#include "target_test.h"
55+
56+/*
57+ * 各タスクの優先度の定義
58+ */
59+
60+#define MAIN_PRIORITY 5 /* メインタスクの優先度 */
61+ /* HIGH_PRIORITYより高くすること */
62+
63+#define HIGH_PRIORITY 9 /* 並行実行されるタスクの優先度 */
64+#define MID_PRIORITY 10
65+#define LOW_PRIORITY 11
66+
67+/*
68+ * ターゲットに依存する可能性のある定数の定義
69+ */
70+
71+#ifndef TASK_PORTID
72+#define TASK_PORTID 1 /* 文字入力するシリアルポートID */
73+#endif /* TASK_PORTID */
74+
75+#ifndef STACK_SIZE
76+#define STACK_SIZE 4096 /* タスクのスタックサイズ */
77+#endif /* STACK_SIZE */
78+
79+/*
80+ * 関数のプロトタイプ宣言
81+ */
82+#ifndef TOPPERS_MACRO_ONLY
83+
84+
85+
86+extern void main_task(intptr_t exinf);
87+extern void bt_task(intptr_t exinf);
88+
89+#endif /* TOPPERS_MACRO_ONLY */
90+
91+#ifdef __cplusplus
92+}
93+#endif
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/nbproject/project.xml (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/nbproject/project.xml (revision 410)
@@ -0,0 +1,26 @@
1+<?xml version="1.0" encoding="UTF-8"?>
2+<project xmlns="http://www.netbeans.org/ns/project/1">
3+ <type>org.netbeans.modules.cnd.makeproject</type>
4+ <configuration>
5+ <data xmlns="http://www.netbeans.org/ns/make-project/1">
6+ <name>calibration2</name>
7+ <c-extensions>c</c-extensions>
8+ <cpp-extensions>cpp</cpp-extensions>
9+ <header-extensions>h</header-extensions>
10+ <sourceEncoding>Shift_JIS</sourceEncoding>
11+ <make-dep-projects/>
12+ <sourceRootList>
13+ <sourceRootElem>.</sourceRootElem>
14+ </sourceRootList>
15+ <confList>
16+ <confElem>
17+ <name>Default</name>
18+ <type>0</type>
19+ </confElem>
20+ </confList>
21+ <formatting>
22+ <project-formatting-style>false</project-formatting-style>
23+ </formatting>
24+ </data>
25+ </configuration>
26+</project>
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/nbproject/configurations.xml (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/nbproject/configurations.xml (revision 410)
@@ -0,0 +1,138 @@
1+<?xml version="1.0" encoding="UTF-8"?>
2+<configurationDescriptor version="95">
3+ <logicalFolder name="root" displayName="root" projectFiles="true" kind="ROOT">
4+ <df root="." name="0">
5+ <df name="balancer">
6+ <in>balancer.c</in>
7+ <in>balancer_param.c</in>
8+ </df>
9+ <df name="course">
10+ <in>Garage.cpp</in>
11+ <in>Garage.h</in>
12+ <in>GrayChecker.cpp</in>
13+ <in>GrayChecker.h</in>
14+ <in>LookupGate.cpp</in>
15+ <in>LookupGate.h</in>
16+ <in>LookupGate_1.cpp</in>
17+ <in>LookupGate_1.h</in>
18+ <in>Stairs.cpp</in>
19+ <in>Stairs.h</in>
20+ <in>StartToGoalL.cpp</in>
21+ <in>StartToGoalL.h</in>
22+ <in>StartToGoalR.cpp</in>
23+ <in>StartToGoalR.h</in>
24+ <in>Stopper.cpp</in>
25+ <in>Stopper.h</in>
26+ </df>
27+ <in>BaseCourse.cpp</in>
28+ <in>BaseCourse.h</in>
29+ <in>CommonApp.cpp</in>
30+ <in>CommonApp.h</in>
31+ <in>PIDDataManager.cpp</in>
32+ <in>PIDDataManager.h</in>
33+ <in>app.cpp</in>
34+ </df>
35+ <logicalFolder name="ExternalFiles"
36+ displayName="重要なファイル"
37+ projectFiles="false"
38+ kind="IMPORTANT_FILES_FOLDER">
39+ <itemPath>../Makefile</itemPath>
40+ </logicalFolder>
41+ </logicalFolder>
42+ <sourceFolderFilter>^(nbproject)$</sourceFolderFilter>
43+ <sourceRootList>
44+ <Elem>.</Elem>
45+ </sourceRootList>
46+ <projectmakefile>../Makefile</projectmakefile>
47+ <confs>
48+ <conf name="Default" type="0">
49+ <toolsSet>
50+ <compilerSet>MinGW|MinGW</compilerSet>
51+ <dependencyChecking>false</dependencyChecking>
52+ <rebuildPropChanged>false</rebuildPropChanged>
53+ </toolsSet>
54+ <codeAssistance>
55+ </codeAssistance>
56+ <makefileType>
57+ <makeTool>
58+ <buildCommandWorkingDir>..</buildCommandWorkingDir>
59+ <buildCommand>${MAKE} app=sample_cpp2 PWD=${CURDIR}</buildCommand>
60+ <cleanCommand>${MAKE} -f Makefile clean</cleanCommand>
61+ <executablePath></executablePath>
62+ <cTool>
63+ <incDir>
64+ <pElem>../../common/library/libcpp-ev3/include</pElem>
65+ <pElem>../../common/ev3api/include</pElem>
66+ <pElem>../../../target/ev3_gcc</pElem>
67+ <pElem>../../../target/ev3_gcc/api/include</pElem>
68+ <pElem>../../../include</pElem>
69+ <pElem>../../../arch</pElem>
70+ <pElem>../../../arch/arm_gcc/am1808</pElem>
71+ </incDir>
72+ </cTool>
73+ <ccTool>
74+ <incDir>
75+ <pElem>../../common/library/libcpp-ev3/include</pElem>
76+ <pElem>../../common/ev3api/include</pElem>
77+ <pElem>../../../target/ev3_gcc</pElem>
78+ <pElem>../../../target/ev3_gcc/api/include</pElem>
79+ <pElem>../../../include</pElem>
80+ <pElem>../../../arch</pElem>
81+ <pElem>../../../arch/arm_gcc/am1808</pElem>
82+ </incDir>
83+ </ccTool>
84+ </makeTool>
85+ </makefileType>
86+ <item path="BaseCourse.cpp" ex="false" tool="1" flavor2="0">
87+ </item>
88+ <item path="BaseCourse.h" ex="false" tool="3" flavor2="0">
89+ </item>
90+ <item path="CommonApp.cpp" ex="false" tool="1" flavor2="0">
91+ </item>
92+ <item path="CommonApp.h" ex="false" tool="3" flavor2="0">
93+ </item>
94+ <item path="PIDDataManager.cpp" ex="false" tool="1" flavor2="0">
95+ </item>
96+ <item path="PIDDataManager.h" ex="false" tool="3" flavor2="0">
97+ </item>
98+ <item path="app.cpp" ex="false" tool="1" flavor2="0">
99+ </item>
100+ <item path="balancer/balancer.c" ex="false" tool="0" flavor2="2">
101+ </item>
102+ <item path="balancer/balancer_param.c" ex="false" tool="0" flavor2="2">
103+ </item>
104+ <item path="course/Garage.cpp" ex="false" tool="1" flavor2="0">
105+ </item>
106+ <item path="course/Garage.h" ex="false" tool="3" flavor2="0">
107+ </item>
108+ <item path="course/GrayChecker.cpp" ex="false" tool="1" flavor2="0">
109+ </item>
110+ <item path="course/GrayChecker.h" ex="false" tool="3" flavor2="0">
111+ </item>
112+ <item path="course/LookupGate.cpp" ex="false" tool="1" flavor2="0">
113+ </item>
114+ <item path="course/LookupGate.h" ex="false" tool="3" flavor2="0">
115+ </item>
116+ <item path="course/LookupGate_1.cpp" ex="false" tool="1" flavor2="0">
117+ </item>
118+ <item path="course/LookupGate_1.h" ex="false" tool="3" flavor2="0">
119+ </item>
120+ <item path="course/Stairs.cpp" ex="false" tool="1" flavor2="0">
121+ </item>
122+ <item path="course/Stairs.h" ex="false" tool="3" flavor2="0">
123+ </item>
124+ <item path="course/StartToGoalL.cpp" ex="false" tool="1" flavor2="0">
125+ </item>
126+ <item path="course/StartToGoalL.h" ex="false" tool="3" flavor2="0">
127+ </item>
128+ <item path="course/StartToGoalR.cpp" ex="false" tool="1" flavor2="0">
129+ </item>
130+ <item path="course/StartToGoalR.h" ex="false" tool="3" flavor2="0">
131+ </item>
132+ <item path="course/Stopper.cpp" ex="false" tool="1" flavor2="0">
133+ </item>
134+ <item path="course/Stopper.h" ex="false" tool="3" flavor2="0">
135+ </item>
136+ </conf>
137+ </confs>
138+</configurationDescriptor>
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/PIDDataManager.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/PIDDataManager.h (revision 410)
@@ -0,0 +1,56 @@
1+/*
2+ * File: PIDDataManager.h
3+ * Author: k.kinoshita
4+ *
5+ * Created on 2016/07/16, 10:46
6+ */
7+
8+#ifndef PIDDATAMANAGER_H
9+#define PIDDATAMANAGER_H
10+
11+#include <vector>
12+
13+// コースクラスに紐づく定数値
14+#define COURSETYPE_A 0
15+#define COURSETYPE_B 1
16+#define COURSETYPE_C 2
17+#define COURSETYPE_D 3
18+#define COURSETYPE_E 4
19+
20+
21+#define HISTORY_MAX 250
22+
23+class PIDDataManager {
24+public:
25+ PIDDataManager();
26+ PIDDataManager(const PIDDataManager& orig);
27+ virtual ~PIDDataManager();
28+
29+ float GetTurnValue();
30+ void SetFixedData(float pVal, float iVal, float dVal);
31+ std::vector<float> GetHensaHistory() { return m_hensaList;}
32+
33+ void SetColor(int black, int white)
34+ {
35+ lightBlack = black;
36+ lightWhite = white;
37+ }
38+
39+private:
40+
41+ std::vector<float> m_hensaList;
42+
43+ // PID制御に使用する係数値
44+ float m_pFixVal;
45+ float m_iFixVal;
46+ float m_dFixVal;
47+
48+ int lightBlack;
49+ int lightWhite;
50+
51+};
52+
53+//extern PIDDataManager PIDMgr;
54+
55+#endif /* PIDDATAMANAGER_H */
56+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer_private.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer_private.h (revision 410)
@@ -0,0 +1,64 @@
1+/**
2+ ******************************************************************************
3+ ** ファイル名 : balancer_private.h
4+ **
5+ ** モデル関連情報:
6+ ** モデル名 : balancer.mdl
7+ ** バージョン : 1.893
8+ ** 履歴 : y_yama - Tue Sep 25 11:37:09 2007
9+ ** takashic - Sun Sep 28 17:50:53 2008
10+ **
11+ ** Copyright (c) 2009-2011 MathWorks, Inc.
12+ ** All rights reserved.
13+ ******************************************************************************
14+ **/
15+
16+/* Imported (extern) block parameters */
17+extern float A_D; /* Variable: A_D
18+ * Referenced by blocks:
19+ * '<S11>/Constant1'
20+ * '<S11>/Gain2'
21+ * ローパスフィルタ係数(左右車輪の平均回転角度用)
22+ */
23+extern float A_R; /* Variable: A_R
24+ * Referenced by blocks:
25+ * '<S8>/Constant1'
26+ * '<S8>/Gain2'
27+ * ローパスフィルタ係数(左右車輪の目標平均回転角度用)
28+ */
29+extern float K_F[4]; /* Variable: K_F
30+ * '<S1>/FeedbackGain'
31+ * サーボ制御用状態フィードバック係数
32+ */
33+extern float K_I; /* Variable: K_I
34+ * '<S1>/IntegralGain'
35+ * サーボ制御用積分係数
36+ */
37+extern float K_PHIDOT; /* Variable: K_PHIDOT
38+ * '<S3>/Gain2'
39+ * 車体の目標平面回転速度(dφ/dt)係数
40+ */
41+extern float K_THETADOT; /* Variable: K_THETADOT
42+ * '<S3>/Gain1'
43+ * 左右車輪の平均回転速度(dθ/dt)係数
44+ */
45+extern const float BATTERY_GAIN; /* PWM出力算出用バッテリ電圧補正係数 */
46+extern const float BATTERY_OFFSET; /* PWM出力算出用バッテリ電圧補正オフセット */
47+
48+/*======================== TOOL VERSION INFORMATION ==========================*
49+ * MATLAB 7.7 (R2008b)30-Jun-2008 *
50+ * Simulink 7.2 (R2008b)30-Jun-2008 *
51+ * Real-Time Workshop 7.2 (R2008b)30-Jun-2008 *
52+ * Real-Time Workshop Embedded Coder 5.2 (R2008b)30-Jun-2008 *
53+ * Stateflow 7.2 (R2008b)30-Jun-2008 *
54+ * Stateflow Coder 7.2 (R2008b)30-Jun-2008 *
55+ * Simulink Fixed Point 6.0 (R2008b)30-Jun-2008 *
56+ *============================================================================*/
57+
58+/*======================= LICENSE IN USE INFORMATION =========================*
59+ * matlab *
60+ * real-time_workshop *
61+ * rtw_embedded_coder *
62+ * simulink *
63+ *============================================================================*/
64+/******************************** END OF FILE ********************************/
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer.c (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer.c (revision 410)
@@ -0,0 +1,374 @@
1+/**
2+ ******************************************************************************
3+ ** FILE NAME : balancer.c
4+ **
5+ ** ABSTRUCT : Two wheeled self-balancing robot "NXTway-GS" balance control program.
6+ ** NXTway-GS balance control algorithum is based on modern control theory
7+ ** which is servo control (state + integral feedback).
8+ ** To develop the controller and indentify the plant, The MathWorks
9+ ** MATLAB&Simulink had been used and this design methodology is
10+ ** called MBD (Model-Based Design/Development). This C source code
11+ ** is automatically generated from a Simulink model by using standard features
12+ ** of Real-Time Workshop Embedded Coder. All control parameters need to be defined
13+ ** by user and the sample parameters are defined in nxtOSEK\samples\nxtway_gs\balancer_param.c.
14+ ** For more detailed information about the controller alogorithum, please check:
15+ ** English : http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=19147&objectType=file
16+ ** Japanese: http://www.cybernet.co.jp/matlab/library/library/detail.php?id=TA060
17+ **
18+ ** MODEL INFO:
19+ ** MODEL NAME : balancer.mdl
20+ ** VERSION : 1.893
21+ ** HISTORY : y_yama - Tue Sep 25 11:37:09 2007
22+ ** takashic - Sun Sep 28 17:50:53 2008
23+ **
24+ ** Copyright (c) 2011 MathWorks, Inc.
25+ ** All rights reserved.
26+ ******************************************************************************
27+ **/
28+/**
29+ ******************************************************************************
30+ ** ファイル名 : balancer.c
31+ **
32+ ** 概要 : 2輪型倒立振子ロボット「NXTway-GS」バランス制御プログラム
33+ ** NXTway-GSのバランス制御には、サーボ制御(状態 + 積分フィードバック)
34+ ** という現代制御を適用しています。制御対象の同定および制御器の開発
35+ ** にはThe MathWorks社のMATLAB&Simulinkという製品を使用した、
36+ ** MBD(モデルベースデザイン/開発)開発手法を用いています。このCプログラムは
37+ ** SimulinkモデルからReal-Time Workshop Embedded Coderコード生成標準機能を
38+ ** 使用して自動生成されたものです。バランス制御器の制御パラメータについては
39+ ** ユーザーハンドコード側で定義する必要があります。定義例として、
40+ ** nxtOSEK\samples\nxtway_gs\balancer_param.cを参照してください。
41+ ** バランス制御アルゴリズムの詳細情報につきましては
42+ ** 日本語: http://www.cybernet.co.jp/matlab/library/library/detail.php?id=TA060
43+ ** 英語 : http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=19147&objectType=file
44+ ** を参照してください。
45+ **
46+ ** モデル関連情報:
47+ ** モデル名 : balancer.mdl
48+ ** バージョン : 1.893
49+ ** 履歴 : -
50+ ** -
51+ **
52+ ** Copyright (c) 2009-2011 MathWorks, Inc.
53+ ** All rights reserved.
54+ ******************************************************************************
55+ **/
56+#include "balancer.h"
57+#include "balancer_private.h"
58+
59+/*============================================================================
60+ * Local macro definitions
61+ *===========================================================================*/
62+#define rt_SATURATE(sig,ll,ul) (((sig) >= (ul)) ? (ul) : (((sig) <= (ll)) ? (ll) : (sig)) )
63+
64+/*============================================================================
65+ * Data definitions
66+ *===========================================================================*/
67+static float ud_err_theta; /* 左右車輪の平均回転角度(θ)目標誤差状態値 */
68+static float ud_psi; /* 車体ピッチ角度(ψ)状態値 */
69+static float ud_theta_lpf; /* 左右車輪の平均回転角度(θ)状態値 */
70+static float ud_theta_ref; /* 左右車輪の目標平均回転角度(θ)状態値 */
71+static float ud_thetadot_cmd_lpf; /* 左右車輪の目標平均回転角速度(dθ/dt)状態値 */
72+
73+/*============================================================================
74+ * Functions
75+ *===========================================================================*/
76+//*****************************************************************************
77+// FUNCTION : balance_control
78+// PARAMETERS :
79+// (float)args_cmd_forward : forward/backward command. 100(max. forward) to -100(max. backward)
80+// (float)args_cmd_turn : turn command. 100(right max.turn) to -100(left max. turn)
81+// (float)args_gyro : gyro sensor data
82+// (float)args_gyro_offset : gyro sensor offset data
83+// (float)args_theta_m_l : left wheel motor count
84+// (float)args_theta_m_r : right wheel motor count
85+// (float)args_battery : battery voltage(mV)
86+// RETURN :
87+// (char*)ret_pwm_l : left motor PWM output
88+// (char*)ret_pwm_r : right motor PWM output
89+// DESCRIPTION : NXTway-GS balance control function
90+// This function must be invoked in 4msec period.
91+// Gyro sensor offset data is varied depending on a product and it has drift
92+// while it's turned on, so these things need to be considered.
93+// Left & right motor revolutions may be different even if a same PWM output is
94+// given, in this case, user needs to add some motor revolution compensator.
95+// SAMPLE CODE :
96+// /* go straight forward with maximum speed */
97+// balance_control(
98+// (float)100, /* max. forward command */
99+// (float)0, /* no turn command */
100+// (float)ecrobot_get_gyro_sensor(NXT_PORT_S4), /* gyro sensor data */
101+// (float)605, /* gyro sensor offset data */
102+// (float)nxt_motor_get_count(NXT_PORT_C), /* left wheel motor count */
103+// (float)nxt_motor_get_count(NXT_PORT_B), /* right wheel motor count */
104+// (float)ecrobot_get_battery_voltage(), /* battery voltage(mV) */
105+// &pwm_l, /* left motor PWM output */
106+// &pwm_r); /* right motor PWM output */
107+//*****************************************************************************
108+//*****************************************************************************
109+// FUNCTION : balance_init
110+// PARAMETERS : none
111+// RETURN : none
112+// DESCRIPTION : NXTway-GS balance control init function. This function
113+// initializes internal state variables. To use this function,
114+// left & right wheel motors count also need to be 0 reset.
115+// SAMPLE CODE :
116+// nxt_motor_set_speed(NXT_PORT_C, 0, 1); /* stop left wheel motor */
117+// nxt_motor_set_speed(NXT_PORT_B, 0, 1); /* stop right wheel motor */
118+// balance_init(); /* init NXTway-GS balance control */
119+// /* motor must be stopped before motor counter is 0 reset */
120+// nxt_motor_set_count(NXT_PORT_C, 0); /* left motor count 0 reset */
121+// nxt_motor_set_count(NXT_PORT_B, 0); /* right motor count 0 reset */
122+//*****************************************************************************
123+
124+//*****************************************************************************
125+// 関数名 : balance_control
126+// 引数 :
127+// (float)args_cmd_forward : 前進/後進命令。100(前進最大値)〜-100(後進最大値)
128+// (float)args_cmd_turn : 旋回命令。100(右旋回最大値)〜-100(左旋回最大値)
129+// (float)args_gyro : ジャイロセンサ値
130+// (float)args_gyro_offset : ジャイロセンサオフセット値
131+// (float)args_theta_m_l : 左モータエンコーダ値
132+// (float)args_theta_m_r : 右モータエンコーダ値
133+// (float)args_battery : バッテリ電圧値(mV)
134+// 戻り値 :
135+// (char*)ret_pwm_l : 左モータPWM出力値
136+// (char*)ret_pwm_r : 右モータPWM出力値
137+// 概要 : NXTway-GSバランス制御関数。
138+// この関数は4msec周期で起動されることを前提に設計されています。
139+// なお、ジャイロセンサオフセット値はセンサ個体および通電によるドリフト
140+// を伴いますので、適宜補正する必要があります。また、左右の車輪駆動
141+// モータは個体差により、同じPWM出力を与えても回転数が異なる場合が
142+// あります。その場合は別途補正機能を追加する必要があります。
143+// 使用例 :
144+// /* 最高速直進命令 */
145+// balance_control(
146+// (float)100, /* 前進最高速命令 */
147+// (float)0, /* 無旋回命令 */
148+// (float)ecrobot_get_gyro_sensor(NXT_PORT_S4), /* ジャイロセンサ値 */
149+// (float)605, /* 車体停止時のジャイロセンサ値 */
150+// (float)nxt_motor_get_count(NXT_PORT_C), /* 左モータエンコーダ値 */
151+// (float)nxt_motor_get_count(NXT_PORT_B), /* 右モータエンコーダ値 */
152+// (float)ecrobot_get_battery_voltage(), /* バッテリ電圧値(mV) */
153+// &pwm_l, /* 左モータPWM出力値 */
154+// &pwm_r); /* 右モータPWM出力値 */
155+//*****************************************************************************
156+//*****************************************************************************
157+// 関数名 : balance_init
158+// 引数 : 無し
159+// 戻り値 : 無し
160+// 概要 : NXTway-GSバランス制御初期化関数。内部状態量変数を初期化します。
161+// この関数によりバランス制御機能を初期化する場合は、併せて左右の
162+// 車輪駆動モーターのエンコーダ値を0にリセットしてください。
163+// 使用例 :
164+// nxt_motor_set_speed(NXT_PORT_C, 0, 1); /* 左モータ停止 */
165+// nxt_motor_set_speed(NXT_PORT_B, 0, 1); /* 右モータ停止 */
166+// balance_init(); /* NXTway-GSバランス制御初期化 */
167+// /* モータエンコーダ値を0リセットする前にモータが停止していること */
168+// nxt_motor_set_count(NXT_PORT_C, 0); /* 左モータエンコーダ0リセット */
169+// nxt_motor_set_count(NXT_PORT_B, 0); /* 右モータエンコーダ0リセット */
170+//*****************************************************************************
171+
172+/* Model step function */
173+void balance_control(float args_cmd_forward, float args_cmd_turn, float
174+ args_gyro, float args_gyro_offset, float
175+ args_theta_m_l, float args_theta_m_r, float
176+ args_battery, signed char *ret_pwm_l, signed char *ret_pwm_r)
177+{
178+ {
179+ float tmp_theta;
180+ float tmp_theta_lpf;
181+ float tmp_pwm_r_limiter;
182+ float tmp_psidot;
183+ float tmp_pwm_turn;
184+ float tmp_pwm_l_limiter;
185+ float tmp_thetadot_cmd_lpf;
186+ float tmp[4];
187+ float tmp_theta_0[4];
188+ long tmp_0;
189+
190+ /* Sum: '<S8>/Sum' incorporates:
191+ * Constant: '<S3>/Constant6'
192+ * Constant: '<S8>/Constant'
193+ * Constant: '<S8>/Constant1'
194+ * Gain: '<S3>/Gain1'
195+ * Gain: '<S8>/Gain2'
196+ * Inport: '<Root>/cmd_forward'
197+ * Product: '<S3>/Divide'
198+ * Product: '<S8>/Product'
199+ * Sum: '<S8>/Sum1'
200+ * UnitDelay: '<S8>/Unit Delay'
201+ */
202+ tmp_thetadot_cmd_lpf = (((args_cmd_forward / CMD_MAX) * K_THETADOT) * (1.0F
203+ - A_R)) + (A_R * ud_thetadot_cmd_lpf);
204+
205+ /* Gain: '<S4>/Gain' incorporates:
206+ * Gain: '<S4>/deg2rad'
207+ * Gain: '<S4>/deg2rad1'
208+ * Inport: '<Root>/theta_m_l'
209+ * Inport: '<Root>/theta_m_r'
210+ * Sum: '<S4>/Sum1'
211+ * Sum: '<S4>/Sum4'
212+ * Sum: '<S4>/Sum6'
213+ * UnitDelay: '<S10>/Unit Delay'
214+ */
215+ tmp_theta = (((DEG2RAD * args_theta_m_l) + ud_psi) + ((DEG2RAD *
216+ args_theta_m_r) + ud_psi)) * 0.5F;
217+
218+ /* Sum: '<S11>/Sum' incorporates:
219+ * Constant: '<S11>/Constant'
220+ * Constant: '<S11>/Constant1'
221+ * Gain: '<S11>/Gain2'
222+ * Product: '<S11>/Product'
223+ * Sum: '<S11>/Sum1'
224+ * UnitDelay: '<S11>/Unit Delay'
225+ */
226+ tmp_theta_lpf = ((1.0F - A_D) * tmp_theta) + (A_D * ud_theta_lpf);
227+
228+ /* Gain: '<S4>/deg2rad2' incorporates:
229+ * Inport: '<Root>/gyro'
230+ * Inport: '<Root>/gyro_offset'
231+ * Sum: '<S4>/Sum2'
232+ */
233+ tmp_psidot = (args_gyro - args_gyro_offset) * DEG2RAD;
234+
235+ /* Gain: '<S2>/Gain' incorporates:
236+ * Constant: '<S3>/Constant2'
237+ * Constant: '<S3>/Constant3'
238+ * Constant: '<S6>/Constant'
239+ * Constant: '<S9>/Constant'
240+ * Gain: '<S1>/FeedbackGain'
241+ * Gain: '<S1>/IntegralGain'
242+ * Gain: '<S6>/Gain3'
243+ * Inport: '<Root>/battery'
244+ * Product: '<S2>/Product'
245+ * Product: '<S9>/Product'
246+ * Sum: '<S1>/Sum2'
247+ * Sum: '<S1>/sum_err'
248+ * Sum: '<S6>/Sum2'
249+ * Sum: '<S9>/Sum'
250+ * UnitDelay: '<S10>/Unit Delay'
251+ * UnitDelay: '<S11>/Unit Delay'
252+ * UnitDelay: '<S5>/Unit Delay'
253+ * UnitDelay: '<S7>/Unit Delay'
254+ */
255+ tmp[0] = ud_theta_ref;
256+ tmp[1] = 0.0F;
257+ tmp[2] = tmp_thetadot_cmd_lpf;
258+ tmp[3] = 0.0F;
259+ tmp_theta_0[0] = tmp_theta;
260+ tmp_theta_0[1] = ud_psi;
261+ tmp_theta_0[2] = (tmp_theta_lpf - ud_theta_lpf) / EXEC_PERIOD;
262+ tmp_theta_0[3] = tmp_psidot;
263+ tmp_pwm_r_limiter = 0.0F;
264+ for (tmp_0 = 0; tmp_0 < 4; tmp_0++) {
265+ tmp_pwm_r_limiter += (tmp[tmp_0] - tmp_theta_0[tmp_0]) * K_F[(tmp_0)];
266+ }
267+
268+ tmp_pwm_r_limiter = (((K_I * ud_err_theta) + tmp_pwm_r_limiter) /
269+ ((BATTERY_GAIN * args_battery) - BATTERY_OFFSET)) *
270+ 100.0F;
271+
272+ /* Gain: '<S3>/Gain2' incorporates:
273+ * Constant: '<S3>/Constant1'
274+ * Inport: '<Root>/cmd_turn'
275+ * Product: '<S3>/Divide1'
276+ */
277+ tmp_pwm_turn = (args_cmd_turn / CMD_MAX) * K_PHIDOT;
278+
279+ /* Sum: '<S2>/Sum' */
280+ tmp_pwm_l_limiter = tmp_pwm_r_limiter + tmp_pwm_turn;
281+
282+ /* Saturate: '<S2>/pwm_l_limiter' */
283+ tmp_pwm_l_limiter = rt_SATURATE(tmp_pwm_l_limiter, -100.0F, 100.0F);
284+
285+ /* Outport: '<Root>/pwm_l' incorporates:
286+ * DataTypeConversion: '<S1>/Data Type Conversion'
287+ */
288+ (*ret_pwm_l) = (signed char)tmp_pwm_l_limiter;
289+
290+ /* Sum: '<S2>/Sum1' */
291+ tmp_pwm_r_limiter -= tmp_pwm_turn;
292+
293+ /* Saturate: '<S2>/pwm_r_limiter' */
294+ tmp_pwm_r_limiter = rt_SATURATE(tmp_pwm_r_limiter, -100.0F, 100.0F);
295+
296+ /* Outport: '<Root>/pwm_r' incorporates:
297+ * DataTypeConversion: '<S1>/Data Type Conversion6'
298+ */
299+ (*ret_pwm_r) = (signed char)tmp_pwm_r_limiter;
300+
301+ /* Sum: '<S7>/Sum' incorporates:
302+ * Gain: '<S7>/Gain'
303+ * UnitDelay: '<S7>/Unit Delay'
304+ */
305+ tmp_pwm_l_limiter = (EXEC_PERIOD * tmp_thetadot_cmd_lpf) + ud_theta_ref;
306+
307+ /* Sum: '<S10>/Sum' incorporates:
308+ * Gain: '<S10>/Gain'
309+ * UnitDelay: '<S10>/Unit Delay'
310+ */
311+ tmp_pwm_turn = (EXEC_PERIOD * tmp_psidot) + ud_psi;
312+
313+ /* Sum: '<S5>/Sum' incorporates:
314+ * Gain: '<S5>/Gain'
315+ * Sum: '<S1>/Sum1'
316+ * UnitDelay: '<S5>/Unit Delay'
317+ * UnitDelay: '<S7>/Unit Delay'
318+ */
319+ tmp_pwm_r_limiter = ((ud_theta_ref - tmp_theta) * EXEC_PERIOD) +
320+ ud_err_theta;
321+
322+ /* user code (Update function Body) */
323+ /* System '<Root>' */
324+ /* 次回演算用状態量保存処理 */
325+
326+ /* Update for UnitDelay: '<S5>/Unit Delay' */
327+ ud_err_theta = tmp_pwm_r_limiter;
328+
329+ /* Update for UnitDelay: '<S7>/Unit Delay' */
330+ ud_theta_ref = tmp_pwm_l_limiter;
331+
332+ /* Update for UnitDelay: '<S8>/Unit Delay' */
333+ ud_thetadot_cmd_lpf = tmp_thetadot_cmd_lpf;
334+
335+ /* Update for UnitDelay: '<S10>/Unit Delay' */
336+ ud_psi = tmp_pwm_turn;
337+
338+ /* Update for UnitDelay: '<S11>/Unit Delay' */
339+ ud_theta_lpf = tmp_theta_lpf;
340+ }
341+}
342+
343+/* Model initialize function */
344+void balance_init(void)
345+{
346+ /* Registration code */
347+
348+ /* states (dwork) */
349+
350+ /* custom states */
351+ ud_err_theta = 0.0F;
352+ ud_theta_ref = 0.0F;
353+ ud_thetadot_cmd_lpf = 0.0F;
354+ ud_psi = 0.0F;
355+ ud_theta_lpf = 0.0F;
356+}
357+
358+/*======================== TOOL VERSION INFORMATION ==========================*
359+ * MATLAB 7.7 (R2008b)30-Jun-2008 *
360+ * Simulink 7.2 (R2008b)30-Jun-2008 *
361+ * Real-Time Workshop 7.2 (R2008b)30-Jun-2008 *
362+ * Real-Time Workshop Embedded Coder 5.2 (R2008b)30-Jun-2008 *
363+ * Stateflow 7.2 (R2008b)30-Jun-2008 *
364+ * Stateflow Coder 7.2 (R2008b)30-Jun-2008 *
365+ * Simulink Fixed Point 6.0 (R2008b)30-Jun-2008 *
366+ *============================================================================*/
367+
368+/*======================= LICENSE IN USE INFORMATION =========================*
369+ * matlab *
370+ * real-time_workshop *
371+ * rtw_embedded_coder *
372+ * simulink *
373+ *============================================================================*/
374+/******************************** END OF FILE ********************************/
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer_param.c (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer_param.c (revision 410)
@@ -0,0 +1,38 @@
1+/**
2+ *******************************************************************************
3+ ** ファイル名 : balancer_param.c
4+ **
5+ ** 概要 : 倒立振子制御パラメーター
6+ **
7+ ** 注記 : 倒立振子制御パラメーターは制御特性に大きな影響を与えます。
8+ **
9+ *******************************************************************************
10+ **/
11+
12+/*============================================================================
13+ * データ定義
14+ *===========================================================================*/
15+float A_D = 0.8F; /* ローパスフィルタ係数(左右車輪の平均回転角度用) */
16+float A_R = 0.996F; /* ローパスフィルタ係数(左右車輪の目標平均回転角度用) */
17+
18+/* 状態フィードバック係数
19+ * K_F[0]: 車輪回転角度係数
20+ * K_F[1]: 車体傾斜角度係数
21+ * K_F[2]: 車輪回転角速度係数
22+ * K_F[3]: 車体傾斜角速度係数
23+ */
24+float K_F[4] = {-0.86526F, -30.73965F, -1.14828F*0.70F, -2.29757F}; // m=0.05;R=0.05;M=0.79;W=0.177;D=0.08;H=0.140
25+float K_I = -0.44721F; /* サーボ制御用積分フィードバック係数 */
26+
27+float K_PHIDOT = 25.0F*2.75F; /* 車体目標旋回角速度係数 */
28+float K_THETADOT = 6.00F; /* モータ目標回転角速度係数 */
29+
30+const float BATTERY_GAIN = 0.001089F; /* PWM出力算出用バッテリ電圧補正係数 */
31+const float BATTERY_OFFSET = 0.625F; /* PWM出力算出用バッテリ電圧補正オフセット */
32+
33+/*****************************************************************************
34+ * 定数変更内容については下記サイトを参照してください. 20180224 東海地区 清水
35+ * 定数変更箇所の解説
36+ * http://bit.ly/2EN22ku
37+ */
38+/******************************** END OF FILE ********************************/
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/balancer/balancer.h (revision 410)
@@ -0,0 +1,100 @@
1+/**
2+ ******************************************************************************
3+ ** ファイル名 : balancer.h
4+ **
5+ ** モデル関連情報:
6+ ** モデル名 : balancer.mdl
7+ ** バージョン : 1.893
8+ ** 履歴 : y_yama - Tue Sep 25 11:37:09 2007
9+ ** takashic - Sun Sep 28 17:50:53 2008
10+ **
11+ ** Copyright (c) 2009-2011 MathWorks, Inc.
12+ ** All rights reserved.
13+ ******************************************************************************
14+ **/
15+#ifndef _ETROBO_BALANCER_H_INCLUDED
16+#define _ETROBO_BALANCER_H_INCLUDED
17+
18+#ifdef __cplusplus
19+extern "C" {
20+#endif
21+
22+/* Macros for accessing real-time model data structure */
23+#ifndef rtmGetErrorStatus
24+# define rtmGetErrorStatus(rtm) ((void*) 0)
25+#endif
26+
27+#ifndef rtmSetErrorStatus
28+# define rtmSetErrorStatus(rtm, val) ((void) 0)
29+#endif
30+
31+#ifndef rtmGetStopRequested
32+# define rtmGetStopRequested(rtm) ((void*) 0)
33+#endif
34+
35+#define CMD_MAX 100.0F /* 前進/旋回命令絶対最大値 */
36+#define DEG2RAD 0.01745329238F /* 角度単位変換係数(=pi/180) */
37+#define EXEC_PERIOD 0.00450000000F /* バランス制御実行周期(秒) *//* sample_c4の処理時間考慮 */
38+//#define EXEC_PERIOD 0.00400000000F /* バランス制御実行周期(秒) *//* 周期タスクでタイミングをとる場合はこちらに変更してください */
39+
40+/* Model entry point functions */
41+extern void balance_init(void);
42+
43+/* Customized model step function */
44+extern void balance_control(float args_cmd_forward, float args_cmd_turn,
45+ float args_gyro, float args_gyro_offset, float args_theta_m_l,
46+ float args_theta_m_r, float args_battery, signed char *ret_pwm_l, signed char
47+ *ret_pwm_r);
48+
49+/*-
50+ * The generated code includes comments that allow you to trace directly
51+ * back to the appropriate location in the model. The basic format
52+ * is <system>/block_name, where system is the system number (uniquely
53+ * assigned by Simulink) and block_name is the name of the block.
54+ *
55+ * Use the MATLAB hilite_system command to trace the generated code back
56+ * to the model. For example,
57+ *
58+ * hilite_system('<S3>') - opens system 3
59+ * hilite_system('<S3>/Kp') - opens and selects block Kp which resides in S3
60+ *
61+ * Here is the system hierarchy for this model
62+ *
63+ * '<Root>' : balancer
64+ * '<S1>' : balancer/Control
65+ * '<S2>' : balancer/Control/Cal_PWM
66+ * '<S3>' : balancer/Control/Cal_Reference
67+ * '<S4>' : balancer/Control/Cal_x1
68+ * '<S5>' : balancer/Control/DiscreteIntegrator
69+ * '<S6>' : balancer/Control/Cal_PWM/Cal_vol_max
70+ * '<S7>' : balancer/Control/Cal_Reference/DiscreteIntegrator
71+ * '<S8>' : balancer/Control/Cal_Reference/LowPathFilter
72+ * '<S9>' : balancer/Control/Cal_x1/DiscreteDerivative
73+ * '<S10>' : balancer/Control/Cal_x1/DiscreteIntegrator
74+ * '<S11>' : balancer/Control/Cal_x1/LowPathFilter
75+ */
76+//#endif /* RTW_HEADER_balancer_h_ */
77+
78+/*======================== TOOL VERSION INFORMATION ==========================*
79+ * MATLAB 7.7 (R2008b)30-Jun-2008 *
80+ * Simulink 7.2 (R2008b)30-Jun-2008 *
81+ * Real-Time Workshop 7.2 (R2008b)30-Jun-2008 *
82+ * Real-Time Workshop Embedded Coder 5.2 (R2008b)30-Jun-2008 *
83+ * Stateflow 7.2 (R2008b)30-Jun-2008 *
84+ * Stateflow Coder 7.2 (R2008b)30-Jun-2008 *
85+ * Simulink Fixed Point 6.0 (R2008b)30-Jun-2008 *
86+ *============================================================================*/
87+
88+/*======================= LICENSE IN USE INFORMATION =========================*
89+ * matlab *
90+ * real-time_workshop *
91+ * rtw_embedded_coder *
92+ * simulink *
93+ *============================================================================*/
94+/******************************** END OF FILE ********************************/
95+
96+#ifdef __cplusplus
97+}
98+#endif
99+
100+#endif /* ! _ETROBO_BALANCER_H_INCLUDED */
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/CommonApp.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/CommonApp.cpp (revision 410)
@@ -0,0 +1,374 @@
1+#include "CommonApp.h"
2+#include <stdarg.h>
3+#include <vector>
4+
5+#include "course/Garage.h"
6+#include "course/LookupGate.h"
7+#include "course/Stairs.h"
8+#include "course/StartToGoalR.h"
9+#include "course/StartToGoalL.h"
10+
11+CommonApp::CommonApp()
12+{
13+}
14+
15+CommonApp::~CommonApp() {
16+}
17+
18+void CommonApp::init() {
19+ // 初期化処理
20+
21+ /*コースクラスの設定*/
22+ CreateCourse(COURSETYPE_RIGHT);
23+ BaseCourseIni();
24+
25+ /* LCD画面表示 */
26+ initLcd();
27+
28+ /* センサー入力ポートの設定 */
29+ initSenserProt();
30+
31+ /* モーター出力ポートの設定 */
32+ initMotorPort();
33+
34+ /* Open Bluetooth file */
35+ openBluetooth();
36+
37+ /* Bluetooth通信タスクの起動 */
38+ startBluetooth();
39+
40+ /* 初期化完了通知 */
41+ initComplete();
42+
43+ /* 尻尾初期値 */
44+ initTailPosition();
45+
46+ /* スタート待機 */
47+ waitStart();
48+
49+ /* 走行モーターエンコーダーリセット */
50+ resetMotor();
51+
52+ /* ジャイロセンサーリセット */
53+ resetGyro();
54+
55+ /* スタート通知 */
56+ startComplete();
57+}
58+
59+void CommonApp::initLcd() {
60+ /* LCD画面表示 */
61+ ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
62+ ev3_lcd_draw_string("EV3way-ET sample_c4", 0, CALIB_FONT_HEIGHT*1);
63+}
64+
65+void CommonApp::initSenserProt() {
66+ /* センサー入力ポートの設定 */
67+ ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
68+ ev3_sensor_config(color_sensor, COLOR_SENSOR);
69+ ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
70+ ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
71+ ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
72+}
73+
74+void CommonApp::initMotorPort() {
75+ /* モーター出力ポートの設定 */
76+ ev3_motor_config(left_motor, LARGE_MOTOR);
77+ ev3_motor_config(right_motor, LARGE_MOTOR);
78+ ev3_motor_config(tail_motor, LARGE_MOTOR);
79+ ev3_motor_reset_counts(tail_motor);
80+}
81+
82+void CommonApp::openBluetooth() {
83+ /* Open Bluetooth file */
84+ bt = ev3_serial_open_file(EV3_SERIAL_BT);
85+ assert(bt != NULL);
86+}
87+
88+void CommonApp::startBluetooth() {
89+ /* Bluetooth通信タスクの起動 */
90+ act_tsk(BT_TASK);
91+}
92+
93+void CommonApp::initComplete() {
94+ /* 初期化完了通知 */
95+ ev3_led_set_color(LED_ORANGE);
96+}
97+
98+void CommonApp::waitStart() {
99+ /* スタート待機 */
100+ while(1)
101+ {
102+ /*
103+ uint8_t colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
104+ char buf[32];
105+ sprintf(buf, "%d", colorSensor);
106+ */
107+ ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
108+ //ev3_lcd_draw_string(buf, 0, CALIB_FONT_HEIGHT*1);
109+
110+ //タッチセンサー値画面出力
111+ bool_t getTouchSensor = ev3_touch_sensor_is_pressed(EV3_PORT_1);
112+ char touch_Sensor[32];
113+ sprintf(touch_Sensor, "Touch: %d", getTouchSensor);
114+ ev3_lcd_draw_string(touch_Sensor, 20, 60);
115+
116+ //超音波センサー値画面出力
117+ int16_t getUltrasonicSensor = ev3_ultrasonic_sensor_get_distance(EV3_PORT_2);
118+ char ultrasonic_sensor[32];
119+ sprintf(ultrasonic_sensor, "Ultrasonic: %d", getUltrasonicSensor);
120+ ev3_lcd_draw_string(ultrasonic_sensor, 20, 40);
121+
122+ //カラーセンサー値画面出力
123+ uint8_t getColorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
124+ char color_sensor[32];
125+ sprintf(color_sensor, "Color: %d", getColorSensor);
126+ ev3_lcd_draw_string(color_sensor, 20, 80);
127+
128+ //ジャイロセンサー値画面出力
129+ int16_t getGyroSensor = ev3_gyro_sensor_get_rate(EV3_PORT_4);
130+ char gyro_sensor[64];
131+ sprintf(gyro_sensor, "Gyro: %d", getGyroSensor);
132+ ev3_lcd_draw_string(gyro_sensor, 20, 20);
133+
134+ tail_control(TAIL_ANGLE_STAND_UP); /* 完全停止用角度に制御 */
135+
136+ if (bt_cmd == 1)
137+ {
138+ break; /* リモートスタート */
139+ }
140+ tslp_tsk(10); /* 10msecウェイト */
141+ }
142+}
143+
144+void CommonApp::initTailPosition()
145+{
146+ // 尻尾モータの初期位置調整
147+ ev3_motor_set_power(tail_motor, -10);
148+ tslp_tsk(2000);
149+ ev3_motor_stop(tail_motor, true);
150+ ev3_motor_reset_counts(tail_motor);
151+}
152+
153+void CommonApp::resetMotor() {
154+ /* 走行モーターエンコーダーリセット */
155+ ev3_motor_reset_counts(left_motor);
156+ ev3_motor_reset_counts(right_motor);
157+}
158+
159+void CommonApp::resetGyro() {
160+ /* ジャイロセンサーリセット */
161+ ev3_gyro_sensor_reset(EV3_PORT_4);
162+ balance_init(); /* 倒立振子API初期化 */
163+}
164+
165+void CommonApp::startComplete() {
166+ /* スタート通知 */
167+ ev3_led_set_color(LED_GREEN);
168+}
169+
170+void CommonApp::mainLoop() {
171+ /**
172+ * Main loop for the self-balance control algorithm */
173+
174+ // メインループ前処理
175+ beforeLoop();
176+
177+ //暫定としてここで設定値を代入
178+ courseNo = 0;
179+ courseList[courseNo]->SetPIDData();
180+
181+ while(1)
182+ {
183+ // メインループ内の先頭で行う処理
184+ if (!startingLoop())
185+ break;
186+ // 各コースのRunを実行
187+ courseList[courseNo]->Run();
188+ fallingFlg = courseList[courseNo]->continueRunning();
189+ if(!continueRunning()) {
190+ break;
191+ }
192+
193+ // 次コースへの判定処理
194+ if(courseList[courseNo]->IsNextCourse()) {
195+ courseNo++;
196+ courseList[courseNo]->SetPIDData();
197+ courseList[courseNo]->SetStartMotorAngle();
198+ }
199+ endingLoop();
200+ loopCnt++;
201+ }
202+}
203+
204+void CommonApp::beforeLoop() {
205+ // ログ出力Header
206+ //log("\r\ncolor_sensor,distance,forward,turn,gyro,gyro_offset,motor_ang_l,motor_ang_r,volt,pwm_L,pwm_R\r\n");
207+}
208+
209+bool CommonApp::continueRunning() {
210+ // 走行終了条件
211+ // 走行体が転倒した場合
212+ // PC で '2'キーを押下する
213+ if (!fallingFlg)
214+ {
215+ ev3_lcd_draw_string("END", 0, CALIB_FONT_HEIGHT*3);
216+ ev3_motor_stop(left_motor, true);
217+ ev3_motor_stop(right_motor, true);
218+ ev3_motor_stop(tail_motor, true);
219+ return false;
220+ }
221+// if (!fallingFlg)
222+// {
223+// ev3_lcd_draw_string("END", 0, CALIB_FONT_HEIGHT*3);
224+// ev3_motor_stop(left_motor, true);
225+// ev3_motor_stop(right_motor, true);
226+// ev3_motor_stop(tail_motor, true);
227+// return false;
228+// }
229+// if (bt_cmd == 2) {
230+// log("%s", "pushed!\r\n");
231+// bt_cmd = 0;
232+// }
233+ return true;
234+}
235+
236+void CommonApp::endingLoop() {
237+ // メインループ内の最後尾で行う処理
238+ tslp_tsk(4); /* 4msec周期起動 */
239+}
240+
241+void CommonApp::end() {
242+ // 終了処理
243+
244+ // モーターを停止
245+ stopMotor();
246+
247+ // Bluetoothを終了
248+ endBluetooth();
249+}
250+
251+void CommonApp::stopMotor() {
252+ // モーターを停止
253+ ev3_motor_stop(left_motor, false);
254+ ev3_motor_stop(right_motor, false);
255+}
256+
257+void CommonApp::endBluetooth() {
258+ // Bluetoothを終了
259+ ter_tsk(BT_TASK);
260+ fclose(bt);
261+}
262+
263+void CommonApp::log(const char* format, ...) {
264+ // ログ出力
265+ va_list vaList;
266+ va_start(vaList, format);
267+ vfprintf(bt, format, vaList);
268+ va_end(vaList);
269+}
270+
271+// Bluetoothタスク内で実行される関数
272+void CommonApp::do_bt_task() {
273+ while(1)
274+ {
275+ uint8_t c = fgetc(bt); /* 受信 */
276+ switch(c)
277+ {
278+ case '1':
279+ bt_cmd = 1;
280+ break;
281+ case '2':
282+ bt_cmd = 2;
283+ break;
284+ default:
285+ break;
286+ }
287+ //fputc(c, bt); /* エコーバック */
288+ }
289+}
290+
291+// 各作業者別のappを作成
292+CommonApp* CommonApp::createApp() {
293+
294+ CommonApp* app = new CommonApp();
295+ return app;
296+}
297+
298+void CommonApp::CreateCourse(int courseType)
299+{
300+ ev3_lcd_draw_string("1", 0, CALIB_FONT_HEIGHT*1);
301+ courseList.clear();
302+ if(COURSETYPE_LEFT == courseType) {
303+ courseList.push_back(new StartToGoalL());
304+ courseList.push_back(new LookupGate());
305+ courseList.push_back(new Garage(courseType));
306+ //courseList.push_back();
307+ }
308+ else {
309+ courseList.push_back(new StartToGoalR());
310+ courseList.push_back(new Stairs());
311+ courseList.push_back(new Garage(courseType));
312+ //courseList.push_back();
313+ }
314+}
315+
316+void CommonApp::BaseCourseIni()
317+{
318+
319+}
320+
321+bool CommonApp::startingLoop() {
322+ // メインループ内の先頭で行う処理
323+
324+ if (ev3_button_is_pressed(BACK_BUTTON)) return false;
325+
326+ if(loopCnt < 50) {
327+ return true;
328+ }
329+ if(!tailRised) {
330+ tailRised = tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */
331+ }
332+
333+ return true;
334+}
335+
336+
337+//*****************************************************************************
338+// 関数名 : tail_control
339+// 引数 : angle (モータ目標角度[度])
340+// 返り値 : 目標角度に到達したか否か
341+// 概要 : 走行体完全停止用モータの角度制御
342+//*****************************************************************************
343+bool CommonApp::tail_control(signed int angle, int powerMax)
344+{
345+ float pwm = (float)(angle - ev3_motor_get_counts(tail_motor))*P_GAIN; /* 比例制御 */
346+ /* PWM出力飽和処理 */
347+ if (pwm > powerMax)
348+ {
349+ pwm = powerMax;
350+ }
351+ else if (pwm < -powerMax)
352+ {
353+ pwm = -powerMax;
354+ }
355+
356+ if (pwm == 0)
357+ {
358+ ev3_motor_stop(tail_motor, true);
359+ return true;
360+ }
361+ else
362+ {
363+
364+ if (pwm < 0) {
365+ pwm -= 0.5;
366+ }
367+ else {
368+ pwm += 0.5;
369+ }
370+
371+ ev3_motor_set_power(tail_motor, pwm);
372+ return false;
373+ }
374+}
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/BaseCourse.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/BaseCourse.cpp (revision 410)
@@ -0,0 +1,205 @@
1+/*
2+ * File: BaseCourse.cpp
3+ * Author: k.kinoshita
4+ *
5+ * Created on 2016/07/23, 10:39
6+ */
7+
8+
9+#include <stdarg.h>
10+#include <math.h>
11+#include "BaseCourse.h"
12+#include "CommonApp.h"
13+#include "course/Stopper.h"
14+
15+int mode = 0;
16+BaseCourse::BaseCourse()
17+{
18+ PIDMgr = new PIDDataManager();
19+}
20+
21+BaseCourse::BaseCourse(const BaseCourse& orig) {
22+ mode = 0;
23+ loopCnt = 0;
24+
25+}
26+
27+BaseCourse::~BaseCourse() {
28+}
29+
30+void BaseCourse::Run() {
31+
32+ // カラーセンサ値を取得
33+ colorSensor = ev3_color_sensor_get_reflect(color_sensor);
34+ //afterRecord(0);
35+
36+ // forward値の設定
37+ setForward();
38+
39+ // PID値の設定
40+ SetPIDData();
41+
42+ // PID関係の各値更新
43+ updatePidValue();
44+
45+ // turn値の限界値制御
46+ normalizeTurnValue();
47+
48+ // 各センサーからの値を取得
49+ updateSensorValue();
50+ //afterRecord(1);
51+
52+ // モーター出力の取得
53+ calcMotorPower();
54+ //afterRecord(2);
55+
56+ // モーター出力の設定
57+ setMotorPower();
58+
59+}
60+
61+bool BaseCourse::IsNextCourse()
62+{
63+ return false;
64+}
65+
66+void BaseCourse::SetPIDData()
67+{
68+}
69+
70+void BaseCourse::setForward()
71+{
72+}
73+
74+void BaseCourse::GetafterRecord(char *log) {
75+ // カラーセンサー読み取り後処理
76+
77+ sprintf(log, "%d,%d,%f,%f,%f,%f,%f,%f,%f,%d,%d\r\n", colorSensor, 0, (float)forward, (float)turn, (float)gyro,
78+ (float)gyro_offset, (float)motor_ang_l, (float)motor_ang_r, (float)volt,(int)pwm_L, (int)pwm_R);
79+}
80+
81+
82+void BaseCourse::updatePidValue() {
83+ // 共通クラスのPID制御を実施
84+ turn = PIDMgr->GetTurnValue();
85+}
86+
87+void BaseCourse::updateSensorValue() {
88+ /* 倒立振子制御API に渡すパラメータを取得する */
89+ motor_ang_l = ev3_motor_get_counts(left_motor);
90+ motor_ang_r = ev3_motor_get_counts(right_motor);
91+// gyro = -1 * ev3_gyro_sensor_get_rate(gyro_sensor) ; /* ※ジャイロセンサーの向きが逆のため符号反転 */
92+ gyro = ev3_gyro_sensor_get_rate(gyro_sensor) ;
93+ volt = ev3_battery_voltage_mV();
94+}
95+
96+void BaseCourse::calcMotorPower() {
97+ /* 倒立振子制御APIを呼び出し、倒立走行するための */
98+ /* 左右モータ出力値を得る */
99+ balance_control(
100+ (float)forward,
101+ (float)turn,
102+ (float)gyro,
103+ //(float)gyro_offset,
104+ (float)(gyro_offset * -1.0F),
105+ (float)motor_ang_l,
106+ (float)motor_ang_r,
107+ (float)volt,
108+ (signed char*)&pwm_L,
109+ (signed char*)&pwm_R);
110+}
111+
112+void BaseCourse::setMotorPower() {
113+ // モーター出力の設定
114+
115+ /* EV3ではモーター停止時のブレーキ設定が事前にできないため */
116+ /* 出力0時に、その都度設定する */
117+ if (pwm_L == 0)
118+ {
119+ ev3_motor_stop(left_motor, true);
120+ }
121+ else
122+ {
123+ ev3_motor_set_power(left_motor, (int)pwm_L);
124+ }
125+
126+ if (pwm_R == 0)
127+ {
128+ ev3_motor_stop(right_motor, true);
129+ }
130+ else
131+ {
132+ ev3_motor_set_power(right_motor, (int)pwm_R);
133+ }
134+}
135+
136+bool BaseCourse::continueRunning() {
137+ // 走り続けていいかを判定
138+
139+ bool val = true;
140+ // 左モータPMW出力が常にMAX状態の場合に転倒判定カウントを開始
141+ if ((int)pwm_L == 100 || (int)pwm_L == -100)
142+ {
143+ // 転倒判定カウントが500回(約2秒間)を越えると強制停止
144+ if (fallingCnt >= 250)
145+ {
146+ val = false;
147+ }
148+ fallingCnt++;
149+ }
150+ else
151+ {
152+ // 転倒判定カウント
153+ fallingCnt = 0;
154+ }
155+ return val;
156+}
157+
158+void BaseCourse::normalizeTurnValue() {
159+ // turn値の限界値制御
160+ if (turn > 100)
161+ {
162+ turn = 100;
163+ }
164+ else if (turn < -100)
165+ {
166+ turn = -100;
167+ }
168+}
169+
170+void BaseCourse::log(const char* format, ...) {
171+ // ログ出力
172+ static FILE *bt = ev3_serial_open_file(EV3_SERIAL_BT);
173+ assert(bt != NULL);
174+ va_list vaList;
175+ va_start(vaList, format);
176+ vfprintf(bt, format, vaList);
177+ va_end(vaList);
178+}
179+
180+void BaseCourse::SetStartMotorAngle()
181+{
182+ startMotorAngR = ev3_motor_get_counts(right_motor);
183+ startMotorAngL = ev3_motor_get_counts(left_motor);
184+}
185+
186+/*スピードアップ
187+ 定義されたMAXスピードへ向け徐々にスピードアップ*/
188+void BaseCourse::SpeedUp(float maxSpeed) {
189+ // 左右モータの総回転角が正の場合、forward値をmaxSpeedまでインクリメント
190+ // 0以下の場合、デフォルト値を設定
191+ double avg = ((motor_ang_r) + (motor_ang_l)) / 2;
192+ if (avg <= 0) {
193+ forward = DEFAULT_FORWARD;
194+ }
195+ else if (forward < maxSpeed) {
196+ forward += 0.5;
197+ }
198+}
199+
200+/*スピードダウン
201+ 次モードのforward値へ向け徐々にスピードダウン*/
202+void BaseCourse::SpeedDown(float minSpeed) {
203+ // forward値をmaxSpeedまでデクリメント
204+ if (forward > minSpeed) forward -= 1;
205+}
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/CommonApp.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/CommonApp.h (revision 410)
@@ -0,0 +1,183 @@
1+#ifndef COMMONAPP_H
2+#define COMMONAPP_H
3+
4+#include <vector>
5+#include "ev3api.h"
6+#include "balancer/balancer.h"
7+#include "BaseCourse.h"
8+
9+#if defined(BUILD_MODULE)
10+#include "module_cfg.h"
11+#else
12+#include "kernel_cfg.h"
13+#endif
14+
15+#define DEBUG
16+
17+#ifdef DEBUG
18+#define _debug(x) (x)1
19+#else
20+#define _debug(x)
21+#endif
22+
23+ #define COURSETYPE_LEFT 1
24+ #define COURSETYPE_RIGHT 0
25+
26+/* 下記のマクロは個体/環境に合わせて変更する必要があります */
27+/* sample_c1マクロ */
28+#define GYRO_OFFSET 0 /* ジャイロセンサオフセット初期値(角速度0[deg/sec]時) */
29+#define LIGHT_WHITE 43 /* 白色の光センサ値 */
30+#define LIGHT_BLACK 0 /* 黒色の光センサ値 */
31+/* sample_c2マクロ */
32+/* sample_c3マクロ */
33+/* キャリブレーション時の尻尾角指定 */
34+//#define TAIL_ANGLE_STAND_UP 60 /* 完全停止時の角度[度] */
35+//#define TAIL_ANGLE_STAND_UP 61 /* 完全停止時の角度[度] */
36+//#define TAIL_ANGLE_STAND_UP 62 /* 完全停止時の角度[度] */
37+//#define TAIL_ANGLE_STAND_UP 63 /* 完全停止時の角度[度] */
38+//#define TAIL_ANGLE_STAND_UP 64 /* 完全停止時の角度[度] */
39+//#define TAIL_ANGLE_STAND_UP 65 /* 完全停止時の角度[度] */
40+//#define TAIL_ANGLE_STAND_UP 66 /* 完全停止時の角度[度] */
41+//#define TAIL_ANGLE_STAND_UP 67 /* 完全停止時の角度[度] */
42+//#define TAIL_ANGLE_STAND_UP 68 /* 完全停止時の角度[度] */
43+//#define TAIL_ANGLE_STAND_UP 69 /* 完全停止時の角度[度] */
44+//#define TAIL_ANGLE_STAND_UP 70 /* 完全停止時の角度[度] */
45+//#define TAIL_ANGLE_STAND_UP 71 /* 完全停止時の角度[度] */
46+//#define TAIL_ANGLE_STAND_UP 72 /* 完全停止時の角度[度] */
47+//#define TAIL_ANGLE_STAND_UP 73 /* 完全停止時の角度[度] */
48+//#define TAIL_ANGLE_STAND_UP 74 /* 完全停止時の角度[度] */
49+//#define TAIL_ANGLE_STAND_UP 75 /* 完全停止時の角度[度] */
50+//#define TAIL_ANGLE_STAND_UP 76 /* 完全停止時の角度[度] */
51+//#define TAIL_ANGLE_STAND_UP 77 /* 完全停止時の角度[度] */
52+//#define TAIL_ANGLE_STAND_UP 78 /* 完全停止時の角度[度] */
53+//#define TAIL_ANGLE_STAND_UP 79 /* 完全停止時の角度[度] */
54+//#define TAIL_ANGLE_STAND_UP 80 /* 完全停止時の角度[度] */
55+//#define TAIL_ANGLE_STAND_UP 81 /* 完全停止時の角度[度] */
56+//#define TAIL_ANGLE_STAND_UP 82 /* 完全停止時の角度[度] */
57+//#define TAIL_ANGLE_STAND_UP 83 /* 完全停止時の角度[度] */
58+//#define TAIL_ANGLE_STAND_UP 84 /* 完全停止時の角度[度] */
59+//#define TAIL_ANGLE_STAND_UP 85 /* 完全停止時の角度[度] */
60+//#define TAIL_ANGLE_STAND_UP 86 /* 完全停止時の角度[度] */
61+//#define TAIL_ANGLE_STAND_UP 87 /* 完全停止時の角度[度] */
62+//#define TAIL_ANGLE_STAND_UP 88 /* 完全停止時の角度[度] */
63+//#define TAIL_ANGLE_STAND_UP 89 /* 完全停止時の角度[度] */
64+//#define TAIL_ANGLE_STAND_UP 90 /* 完全停止時の角度[度] */
65+#define TAIL_ANGLE_STAND_UP 66 /* ルックアップゲートをくぐるときの角度[度] */
66+#define TAIL_ANGLE_DRIVE 30 /* バランス走行時の角度[度] */
67+#define P_GAIN 2.5F /* 完全停止用モータ制御比例係数 */
68+#define PWM_ABS_MAX 10 /* 完全停止用モータ制御PWM絶対最大値 */
69+#define KP 1.0 /* P制御用係数*/
70+#define KI 1.5 /* I制御用係数*/
71+#define KD 1.5 /* D制御用係数*/
72+#define PROCESS_PERIOD 0.004 /* 処理周期(ミリ秒)*/
73+/* sample_c4マクロ */
74+//#define DEVICE_NAME "ET0" /* Bluetooth名 hrp2/target/ev3.h BLUETOOTH_LOCAL_NAMEで設定 */
75+//#define PASS_KEY "1234" /* パスキー hrp2/target/ev3.h BLUETOOTH_PIN_CODEで設定 */
76+
77+#define CMD_START '1' /* リモートスタートコマンド */
78+
79+/* LCDフォントサイズ */
80+#define CALIB_FONT (EV3_FONT_SMALL)
81+#define CALIB_FONT_WIDTH (6/*TODO: magic number*/)
82+#define CALIB_FONT_HEIGHT (8/*TODO: magic number*/)
83+
84+const sensor_port_t
85+ touch_sensor = EV3_PORT_1,
86+ sonar_sensor = EV3_PORT_2,
87+ color_sensor = EV3_PORT_3,
88+ gyro_sensor = EV3_PORT_4;
89+
90+const motor_port_t
91+ left_motor = EV3_PORT_C,
92+ right_motor = EV3_PORT_B,
93+ tail_motor = EV3_PORT_A;
94+
95+class CommonApp {
96+public:
97+ CommonApp();
98+ virtual ~CommonApp();
99+
100+ // 初期化
101+ void init();
102+ virtual void initLcd();
103+ virtual void initSenserProt();
104+ virtual void initMotorPort();
105+
106+
107+ virtual void openBluetooth();
108+ virtual void startBluetooth();
109+
110+ virtual void initComplete();
111+
112+ virtual void waitStart();
113+
114+ virtual void resetMotor();
115+ virtual void resetGyro();
116+
117+ virtual void startComplete();
118+
119+ virtual void initTailPosition();
120+
121+ // メインループ
122+ void mainLoop();
123+ void beforeLoop();
124+ //virtual void afterRecord(int position);
125+ bool startingLoop();
126+ //virtual void setForward();
127+ //virtual void updatePidValue();
128+ //virtual void normalizeTurnValue();
129+ //virtual void updateSensorValue();
130+ //virtual void calcMotorPower();
131+ //virtual void setMotorPower();
132+ bool continueRunning();
133+ void endingLoop();
134+
135+ // 終了処理
136+ void end();
137+ virtual void stopMotor();
138+ virtual void endBluetooth();
139+
140+ // その他
141+ virtual void log(const char* format, ...);
142+ virtual void do_bt_task();
143+
144+ static CommonApp* createApp();
145+
146+
147+ static bool tail_control(signed int angle, int powerMax = PWM_ABS_MAX);
148+
149+
150+
151+ void CreateCourse(int courseType);
152+ void BaseCourseIni();
153+
154+
155+protected:
156+ std::vector<BaseCourse*> courseList;
157+
158+ // メインループ内で使用する変数
159+ // ログを取りやすくするためメンバ変数にしてあります
160+ float forward = 0.0F; /* 前後進命令 */
161+ float turn = 0.0F; /* 旋回命令 */
162+ /* 左右モータPWM出力 */
163+
164+ unsigned long loopCnt = 0;
165+ char buffer[2046];
166+
167+ unsigned int distance = 0;
168+ // 転倒時判定フラグ
169+ bool fallingFlg = false;
170+
171+ int courseNo = 0;
172+
173+ int nowColorSensor = 0;
174+
175+ int bt_cmd = 0; /* Bluetoothコマンド 1:リモートスタート */
176+ FILE *bt = NULL; /* Bluetoothファイルハンドル */
177+
178+private:
179+ bool tailRised = false;
180+};
181+
182+#endif /* COMMONAPP_H */
183+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/LookupGate.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/LookupGate.cpp (revision 410)
@@ -0,0 +1,642 @@
1+/*
2+ * File: LookupGate.cpp
3+ * Author: h.kitoh
4+ *
5+ * Created on 2016/08/14, 10:39
6+ */
7+
8+#include "LookupGate.h"
9+#include "GrayChecker.h"
10+
11+#include "CommonApp.h"
12+
13+LookupGate::LookupGate():lookupGateState(LookupGateStateBeforeSonorDetected), runMode(normalRun) {
14+ grayChecker = new GrayChecker();
15+}
16+
17+LookupGate::~LookupGate() {
18+}
19+const signed int standTailAngle = 84;
20+const signed int interval1stTailAngle = 78;
21+const signed int interval2ndTailAngle = 72;
22+//const signed int interval3ndTailAngle = 68;
23+const signed int throughGateTailAngle = 66;
24+
25+void LookupGate::Run() {
26+ sonar_alert();
27+ switch(lookupGateState) {
28+ case LookupGateStateRun:
29+ BaseCourse::Run();
30+ // 少し倒立振子走行してから尻尾を下す
31+ if (200 < motor_ang_l) {
32+ if (stopper.DoStop()) {
33+ lookupGateState = LookupGateStateBeforeSpin;
34+ log("To BeforeSpin\r\n");
35+ }
36+ }
37+ break;
38+ case LookupGateStateBeforeSonorDetected:
39+ PIDMgr->SetColor(20, 40);
40+ runUntilSonorDetected();
41+ break;
42+ case LookupGateStateAfterSonorDetected:
43+ runSonorDetected();
44+ break;
45+ case LookupGateStateThroughGateReady1st:
46+ throughGateReady1st();
47+ break;
48+ case LookupGateStateThroughGateReady2nd:
49+ tail_control(standTailAngle);
50+ throughGateReady2nd();
51+ break;
52+ case LookupGateStateThroughGateReady3rd:
53+ throughGateReady3rd();
54+ break;
55+ case LookupGateStateThroughGateReady4th:
56+ tail_control(interval1stTailAngle);
57+ throughGateReady4th();
58+ break;
59+ case LookupGateStateThroughGateReady5th:
60+ throughGateReady5th();
61+ break;
62+ case LookupGateStateThroughGateReady6th:
63+ tail_control(interval2ndTailAngle);
64+ throughGateReady6th();
65+ break;
66+ case LookupGateStateThroughGateReady7th:
67+ throughGateReady7th();
68+ break;
69+ case LookupGateStateThroughGateReady8th:
70+ tail_control(throughGateTailAngle);
71+ throughGateReady8th();
72+ break;
73+ case LookupGateStateThroughGateForward:
74+ tail_control(throughGateTailAngle);
75+ ForwardRun();
76+ break;
77+ case LookupGateStateUpridhtBody1st:
78+ UpridhtBody1st();
79+ break;
80+ case LookupGateStateUpridhtBody2nd:
81+ tail_control(interval2ndTailAngle);
82+ UpridhtBody2nd();
83+ break;
84+ case LookupGateStateUpridhtBody3rd:
85+ UpridhtBody3rd();
86+ break;
87+ case LookupGateStateUpridhtBody4th:
88+ tail_control(interval1stTailAngle);
89+ UpridhtBody4th();
90+ break;
91+ case LookupGateStateUpridhtBody5th:
92+ tail_control(standTailAngle);
93+ UpridhtBody5th();
94+ break;
95+ case LookupGateStateBeforeSpin:
96+ tail_control(standTailAngle);
97+ // 0.5秒止まる
98+ if(125 < ++waitCount){
99+ waitCount = 0;
100+ lookupGateState = LookupGateStateSpinRightWheel;
101+ log("To SpinRight\r\n");
102+ }
103+ break;
104+ case LookupGateStateSpinRightWheel:
105+ tail_control(standTailAngle);
106+ // 右ホイールを前進させ0.5秒止まる
107+ if (waitCount == 0) {
108+ if (SpinRight()) {
109+ ++waitCount;
110+ }
111+ } else if(125 < ++waitCount) {
112+ waitCount = 0;
113+ lookupGateState = LookupGateStateSpinLeftWheel;
114+ log("To SpinLeft\r\n");
115+ }
116+ break;
117+ case LookupGateStateSpinLeftWheel:
118+ tail_control(standTailAngle);
119+ // 左ホイールを後退させ0.5秒止まる
120+ if (waitCount == 0) {
121+ if (SpinLeft()) {
122+ ++waitCount;
123+ }
124+ }
125+ else if (125 < ++waitCount) {
126+ waitCount = 0;
127+ lookupGateState = LookupGateStateAfterSpin;
128+ log("To SpinLeft\r\n");
129+ }
130+ break;
131+ case LookupGateStateAfterSpin:
132+ // ライントレース(84度想定)
133+ tail_control(standTailAngle);
134+ LineTraceWithTail();
135+ if (125 < ++waitCount) {
136+ waitCount = 0;
137+ lookupGateState = LookupGateStateThroughGateForwardAjustment;
138+ log("To SpinRight\r\n");
139+ log("To LookupGateStateBodyDown1st\r\n");
140+ }
141+ break;
142+ case LookupGateStateThroughGateForwardAjustment:
143+ runMode = run84;
144+ tail_control(standTailAngle);
145+ ForwardRunAjustment();
146+ break;
147+ case LookupGateStateBodyDown1st:
148+ runMode = run66;
149+ BaseCourse::Run();
150+ if (tail_control(interval1stTailAngle)) {
151+ log("LookupGateStateBodyDown2nd\r\n");
152+ lookupGateState = LookupGateStateBodyDown2nd;
153+ }
154+ break;
155+ case LookupGateStateBodyDown2nd:
156+ tail_control(interval1stTailAngle);
157+ runMode = run66;
158+ BaseCourse::Run();
159+ if (++counter >= 100) {
160+ log("LookupGateStateBodyDown3rd\r\n");
161+ lookupGateState = LookupGateStateBodyDown3rd;
162+ counter = 0;
163+ }
164+ break;
165+ case LookupGateStateBodyDown3rd:
166+ runMode = run66;
167+ BaseCourse::Run();
168+ if (tail_control(interval2ndTailAngle)) {
169+ log("LookupGateStateBodyDown4th\r\n");
170+ lookupGateState = LookupGateStateBodyDown4th;
171+ }
172+ break;
173+ case LookupGateStateBodyDown4th:
174+ tail_control(interval2ndTailAngle);
175+ runMode = run66;
176+ BaseCourse::Run();
177+ if (++counter >= 100) {
178+ log("LookupGateStateBodyDown5th\r\n");
179+ lookupGateState = LookupGateStateBodyDown5th;
180+ counter = 0;
181+ }
182+ break;
183+ case LookupGateStateBodyDown5th:
184+ runMode = run66;
185+ BaseCourse::Run();
186+ if (tail_control(throughGateTailAngle)) {
187+ log("LookupGateStateBodyDown6th\r\n");
188+ lookupGateState = LookupGateStateBodyDown6th;
189+ }
190+ break;
191+ case LookupGateStateBodyDown6th:
192+ tail_control(throughGateTailAngle);
193+ runMode = run66;
194+ BaseCourse::Run();
195+ if (++counter >= 300) {
196+ forwardMotorAngL = 0;
197+ log("LookupGateStateThroughGateForward\r\n");
198+ lookupGateState = LookupGateStateThroughGateForward;
199+ counter = 0;
200+ }
201+ break;
202+ case LookupGateStateThroughGateStop:
203+ tail_control(throughGateTailAngle);
204+ RunStop();
205+ break;
206+ case LookupGateStateGraySerchRun:
207+ tail_control(standTailAngle);
208+ GraySerchRun();
209+ break;
210+ case LookupGateStateChangeGarage:
211+ tail_control(standTailAngle);
212+ ChangeGarage();
213+ break;
214+ }
215+}
216+
217+void LookupGate::setForward()
218+{
219+ forward = 0;
220+ switch(lookupGateState)
221+ {
222+ case LookupGateStateRun:
223+ forward = 20;
224+ break;
225+ case LookupGateStateBeforeSpin:
226+ case LookupGateStateSpinRightWheel:
227+ case LookupGateStateSpinLeftWheel:
228+ case LookupGateStateAfterSpin:
229+ break;
230+ case LookupGateStateBeforeSonorDetected:
231+ forward = 30;
232+ break;
233+ case LookupGateStateAfterSonorDetected:
234+ case LookupGateStateThroughGateReady1st:
235+ break;
236+ case LookupGateStateThroughGateReady2nd:
237+ forward = 5;
238+ break;
239+ case LookupGateStateThroughGateReady3rd:
240+ forward = 2;
241+ break;
242+ case LookupGateStateThroughGateReady4th:
243+ case LookupGateStateThroughGateReady5th:
244+ case LookupGateStateThroughGateReady6th:
245+ case LookupGateStateThroughGateReady7th:
246+ case LookupGateStateThroughGateReady8th:
247+ break;
248+ case LookupGateStateThroughGateForward:
249+ forward = 12;
250+ break;
251+ case LookupGateStateThroughGateForwardAjustment:
252+ forward = 8;
253+ break;
254+ case LookupGateStateBodyDown1st:
255+ case LookupGateStateBodyDown2nd:
256+ case LookupGateStateBodyDown3rd:
257+ case LookupGateStateBodyDown4th:
258+ case LookupGateStateBodyDown5th:
259+ case LookupGateStateBodyDown6th:
260+ case LookupGateStateThroughGateStop:
261+ break;
262+ case LookupGateStateUpridhtBody1st:
263+ forward = -2;
264+ break;
265+ case LookupGateStateUpridhtBody2nd:
266+ break;
267+ case LookupGateStateUpridhtBody3rd:
268+ forward = -2;
269+ break;
270+ case LookupGateStateUpridhtBody4th:
271+ break;
272+ case LookupGateStateUpridhtBody5th:
273+ forward = -2;
274+ break;
275+ case LookupGateStateGraySerchRun:
276+ forward = 10;
277+ break;
278+ case LookupGateStateChangeGarage:
279+ forward = 10;
280+ break;
281+ }
282+}
283+
284+void LookupGate::runUntilSonorDetected()
285+{
286+ runMode = normalRun;
287+ BaseCourse::Run();
288+ if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0))
289+ {
290+ log("LookupGateStateAfterSonorDetected\r\n");
291+ lookupGateState = LookupGateStateAfterSonorDetected;
292+ counter = 0;
293+ }
294+}
295+
296+void LookupGate::runSonorDetected() {
297+ runMode = normalRun;
298+ BaseCourse::Run();
299+ if (++counter >= 250) {
300+ log("LookupGateStateThroughGateReady1st\r\n");
301+ lookupGateState = LookupGateStateThroughGateReady1st;
302+ counter = 0;
303+ }
304+}
305+
306+void LookupGate::throughGateReady1st() {
307+ runMode = normalRun;
308+ BaseCourse::Run();
309+ if (tail_control(standTailAngle)) {
310+ log("LookupGateStateThroughGateReady2nd\r\n");
311+ lookupGateState = LookupGateStateThroughGateReady2nd;
312+ }
313+}
314+
315+void LookupGate::throughGateReady2nd() {
316+ runMode = run66;
317+ BaseCourse::Run();
318+ if (++counter >= 250) {
319+ log("LookupGateStateThroughGateReady3rd\r\n");
320+ lookupGateState = LookupGateStateThroughGateReady3rd;
321+ counter = 0;
322+ }
323+}
324+
325+void LookupGate::throughGateReady3rd() {
326+ runMode = run66;
327+ BaseCourse::Run();
328+ if(tail_control(interval1stTailAngle)) {
329+ log("LookupGateStateThroughGateReady4th\r\n");
330+ lookupGateState = LookupGateStateThroughGateReady4th;
331+ }
332+}
333+
334+void LookupGate::throughGateReady4th() {
335+ runMode = run66;
336+ BaseCourse::Run();
337+ if (++counter >= 200) {
338+ log("LookupGateStateThroughGateReady5th\r\n");
339+ lookupGateState = LookupGateStateThroughGateReady5th;
340+ counter = 0;
341+ }
342+}
343+
344+void LookupGate::throughGateReady5th() {
345+ runMode = run66;
346+ BaseCourse::Run();
347+ if (tail_control(interval2ndTailAngle)) {
348+ log("LookupGateStateThroughGateReady6th\r\n");
349+ lookupGateState = LookupGateStateThroughGateReady6th;
350+ }
351+}
352+
353+void LookupGate::throughGateReady6th()
354+{
355+ runMode = run66;
356+ BaseCourse::Run();
357+ if (++counter >= 200) {
358+ log("LookupGateStateThroughGateReady7th\r\n");
359+ lookupGateState = LookupGateStateThroughGateReady7th;
360+ counter = 0;
361+ }
362+}
363+
364+void LookupGate::throughGateReady7th() {
365+ runMode = run66;
366+ BaseCourse::Run();
367+ if(tail_control(throughGateTailAngle)) {
368+ log("LookupGateStateThroughGateReady8th\r\n");
369+ lookupGateState = LookupGateStateThroughGateReady8th;
370+ }
371+}
372+
373+void LookupGate::throughGateReady8th() {
374+ runMode = run66;
375+ BaseCourse::Run();
376+ if (++counter >= 400) {
377+ forwardMotorAngL = 0;
378+ log("LookupGateStateThroughGateForward\r\n");
379+ lookupGateState = LookupGateStateThroughGateForward;
380+ counter = 0;
381+ }
382+}
383+
384+void LookupGate::ForwardRunAjustment() {
385+ runMode = run84;
386+ BaseCourse::Run();
387+ if ((distance <= 5) && (distance >= 0)) {
388+ log("LookupGateStateBodyDown1st\r\n");
389+ lookupGateState = LookupGateStateBodyDown1st;
390+ }
391+}
392+
393+void LookupGate::ForwardRun() {
394+ runMode = run66;
395+ BaseCourse::Run();
396+ if (forwardMotorAngL == 0) {
397+ if ((distance >= 30) || (distance < 0)) {
398+ forwardMotorAngL = motor_ang_l;
399+ }
400+ } else {
401+ if (170 < motor_ang_l - forwardMotorAngL) {
402+ log("LookupGateStateThroughGateStop\r\n");
403+ lookupGateState = LookupGateStateThroughGateStop;
404+ }
405+ }
406+}
407+
408+void LookupGate::RunStop() {
409+ runMode = run66;
410+ BaseCourse::Run();
411+ if (++counter >= 100) {
412+ log("LookupGateStateUpridhtBody1st\r\n");
413+ lookupGateState = LookupGateStateUpridhtBody1st;
414+ counter = 0;
415+ }
416+}
417+
418+void LookupGate::UpridhtBody1st() {
419+ runMode = run66;
420+ BaseCourse::Run();
421+ if (tail_control(interval2ndTailAngle)) {
422+ log("LookupGateStateUpridhtBody2nd\r\n");
423+ lookupGateState = LookupGateStateUpridhtBody2nd;
424+ }
425+}
426+
427+void LookupGate::UpridhtBody2nd() {
428+ runMode = run66;
429+ BaseCourse::Run();
430+ if (++counter >= 300) {
431+ log("LookupGateStateUpridhtBody3rd\r\n");
432+ lookupGateState = LookupGateStateUpridhtBody3rd;
433+ counter = 0;
434+ }
435+}
436+
437+void LookupGate::UpridhtBody3rd() {
438+ runMode = run66;
439+ BaseCourse::Run();
440+ if (tail_control(interval1stTailAngle)) {
441+ log("LookupGateStateUpridhtBody4th\r\n");
442+ lookupGateState = LookupGateStateUpridhtBody4th;
443+ }
444+}
445+
446+void LookupGate::UpridhtBody4th() {
447+ runMode = run66;
448+ BaseCourse::Run();
449+ if (++counter >= 300) {
450+ log("LookupGateStateUpridhtBody5th\r\n");
451+ lookupGateState = LookupGateStateUpridhtBody5th;
452+ counter = 0;
453+ }
454+}
455+
456+void LookupGate::UpridhtBody5th() {
457+ runMode = run66;
458+ BaseCourse::Run();
459+ if(tail_control(standTailAngle)) {
460+ UpridhtBodyCount++;
461+ if(UpridhtBodyCount != 3){
462+ log("LookupGateStateBeforeSpin\r\n");
463+ lookupGateState = LookupGateStateBeforeSpin;
464+ } else {
465+ log("LookupGateStateGraySerchRun\r\n");
466+ forwardMotorAngL = motor_ang_l;
467+ lookupGateState = LookupGateStateGraySerchRun;
468+ }
469+ }
470+}
471+
472+void LookupGate::SetPIDData() {
473+ // PID制御に使用する定数を設定する
474+ PIDMgr->SetFixedData(1.4, 1, 0.03);
475+}
476+
477+bool LookupGate::tail_control(signed int angle) {
478+ if (angle < 50) {
479+ angle = 50;
480+ }
481+ float pwm = (float)(angle - ev3_motor_get_counts(tail_motor)) * P_GAIN_LOOKUP_GATE; /* 比例制御 */
482+ /* PWM出力飽和処理 */
483+ if (pwm > PWM_ABS_MAX_LOOKUP_GATE) {
484+ pwm = PWM_ABS_MAX_LOOKUP_GATE;
485+ } else if (pwm < -PWM_ABS_MAX_LOOKUP_GATE) {
486+ pwm = -PWM_ABS_MAX_LOOKUP_GATE;
487+ }
488+
489+ if (pwm == 0) {
490+ ev3_motor_stop(tail_motor, true);
491+ return true;
492+ } else {
493+ ev3_motor_set_power(tail_motor, (signed char)pwm);
494+ return false;
495+ }
496+}
497+
498+void LookupGate::calcMotorPower() {
499+ log("%d,%d,\r\n", colorSensor,runMode);
500+ if(runMode == normalRun) {
501+ BaseCourse::calcMotorPower();
502+ } else if(runMode == run66) {
503+ const int LightWhite = 5;
504+ const int LightBlack = 0;
505+ const int targetColor = (LightWhite + LightBlack) / 2;
506+ const int defaultPower = 5;
507+ const int additionalPower = 5;
508+ if (forward > 0) {
509+ int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor);
510+ //pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
511+ //pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
512+ pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
513+ pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
514+ } else if(forward == 0) {
515+ pwm_L = 0;
516+ pwm_R = 0;
517+ } else if(forward < 0) {
518+ pwm_L = forward;
519+ pwm_R = forward;
520+ }
521+ } else if(runMode == run84) {
522+ const int LightWhite = 10;
523+ const int LightBlack = 0;
524+ const int targetColor = (LightWhite + LightBlack) / 2;
525+ const int defaultPower = 5;
526+ const int additionalPower = 5;
527+ if (forward > 0) {
528+ int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor);
529+ //pwm_L = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
530+ //pwm_R = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
531+ pwm_R = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
532+ pwm_L = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
533+ } else if(forward == 0) {
534+ pwm_L = 0;
535+ pwm_R = 0;
536+ } else if(forward < 0) {
537+ pwm_L = forward;
538+ pwm_R = forward;
539+ }
540+ }
541+}
542+void LookupGate::GraySerchRun() {
543+ runMode = run84;
544+ BaseCourse::Run();
545+ if (340 < (motor_ang_l - forwardMotorAngL)) {
546+ log("LookupGateStateChangeGarage\r\n");
547+ lookupGateState = LookupGateStateChangeGarage;
548+ }
549+}
550+void LookupGate::ChangeGarage() {
551+ runMode = run84;
552+ BaseCourse::Run();
553+}
554+
555+/**
556+ * @brief コースモード移行判定(オーバーライド)
557+ * @note
558+ * 自コースの走行クラスにモードを移行していいかを判定
559+ *
560+ */
561+bool LookupGate::IsNextCourse() {
562+ bool result = false;
563+ // グレーサーチモードで、
564+ if (lookupGateState == LookupGateStateChangeGarage) {
565+ ev3_motor_stop(tail_motor, true);
566+ result = true;
567+ }
568+ return result;
569+}
570+
571+//*****************************************************************************
572+// 関数名 : sonar_alert
573+// 引数 : 無し
574+// 返り値 : 1(障害物あり)/0(障害物無し)
575+// 概要 : 超音波センサによる障害物検知
576+//*****************************************************************************
577+int LookupGate::sonar_alert(void) {
578+ static unsigned int counter = 0;
579+ static int alert = 0;
580+ /* 約40msec周期毎に障害物検知 */
581+ if (++counter == 40/4) {
582+// log("distance : %d\r\n", distance);
583+ distance = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
584+ if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0)) {
585+ alert = 1; /* 障害物を検知 */
586+ } else {
587+ alert = 0; /* 障害物無し */
588+ }
589+ counter = 0;
590+ } else {
591+ distance = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
592+ }
593+ return alert;
594+}
595+
596+bool LookupGate::SpinRight() {
597+ int32_t motorAngle = ev3_motor_get_counts(right_motor);
598+ if (targetSpinCount == 0) {
599+ targetSpinCount = motorAngle + 300;
600+ //targetSpinCount = motorAngle - 330;
601+ ev3_motor_stop(left_motor, true);
602+ }
603+ if (motorAngle < targetSpinCount) {
604+// log("target:%d current:%d\r\n", targetSpinCount, motorAngle);
605+ ev3_motor_set_power(right_motor, 10);
606+ return false;
607+ } else {
608+ ev3_motor_stop(right_motor, true);
609+ targetSpinCount = 0;
610+ return true;
611+ }
612+}
613+
614+bool LookupGate::SpinLeft() {
615+ int32_t motorAngle = ev3_motor_get_counts(left_motor);
616+ if (targetSpinCount == 0) {
617+ targetSpinCount = motorAngle - 360;
618+// targetSpinCountBack = motorAngle - 600;
619+ //targetSpinCount = motorAngle + 390;
620+ ev3_motor_stop(right_motor, true);
621+ }
622+ if (targetSpinCount < motorAngle) {
623+// log("target:%d current:%d\r\n", targetSpinCount, motorAngle);
624+ ev3_motor_set_power(left_motor, -10);
625+ return false;
626+ } else {
627+// if(targetSpinCountBack < motorAngle){
628+// ev3_motor_set_power(left_motor, -10);
629+// ev3_motor_set_power(right_motor, -10);
630+// return false;
631+// }
632+ ev3_motor_stop(left_motor, true);
633+ targetSpinCount = 0;
634+ return true;
635+ }
636+}
637+
638+// 尻尾走行ライントレース(84度想定)
639+void LookupGate::LineTraceWithTail() {
640+ runMode = run84;
641+ BaseCourse::Run();
642+}
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stairs.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stairs.h (revision 410)
@@ -0,0 +1,269 @@
1+/*
2+ * File: Stairs.h
3+ * Author: y.nakamura
4+ *
5+ * Created on 2016/08/07, 16:16
6+ */
7+#ifndef STAIRS_H
8+#define STAIRS_H
9+
10+#include "BaseCourse.h"
11+#include "CommonApp.h"
12+#include <cstdlib>
13+#include <cmath>
14+class GrayChecker;
15+class Stairs : public BaseCourse {
16+public:
17+ Stairs();
18+ virtual ~Stairs();
19+ void setForward();
20+ void updateSensorValue();
21+ void updatePidValue();
22+ void SetPIDData();
23+ void normalizeTurnValue();
24+ void Run();
25+ void StepDetection();
26+ void Backward(int motor_ang);
27+ void LineEdgeChangeForward(int motor_ang);
28+ void StepUp(int motor_ang);
29+ void AdjustLineForward();
30+ void AdjustLineBack();
31+ void Spin();
32+ void AdjustPosition(int currentAngle, unsigned int listSize, int targetAngle, int range);
33+ void SearchLineForward();
34+ void SearchLineBackward();
35+ void SearchLineChangeAngle();
36+ void Stop();
37+ void LineTrace(int motor_ang);
38+ void StepDown();
39+ void TraceRun();
40+ void changeNextState();
41+ void changeNextState(int skip);
42+ void changeMode();
43+ bool judgeColor(unsigned int listSize, int color);
44+ bool judgeMotorAngle( int currentAngle, unsigned int listSize, int targetAngle, int range);
45+ bool IsNextCourse();
46+ void variableInit();
47+ void GraySearch();
48+// float GetTurnValue(int LineColor);
49+ float CalcForward(int currentMotorAng, int targetMortorAng, int maxForward, int maxDiffMortorAng);
50+
51+ void setMotorPower();
52+
53+private:
54+ Stairs(const Stairs& orig){}
55+
56+ // リストカウント関連
57+ const int COLOR_LIST_COUNT_SEARCHLINE = 3; // ライン探索時のカラー値累積カウント
58+ const int MORTOR_ANG_LIST_COUNT_STOP = 300; // 指定位置ストップ用の距離累積カウント
59+
60+ // ジャイロ値関連
61+ const float STEP_DETECTION_GYRO = 115; // 段差検知とするジャイロ値
62+ const float STEP_UP_GYRO_OFFSET = -7; // 段差上昇時のジャイロオフセット値
63+ const float NORMAL_GYRO_OFFSET = 0; // 通常時のジャイロオフセット値
64+ const float SPIN_GYRO_OFFSET = 0.00; // スピン時のジャイロオフセット値
65+
66+ // カラーセンサー値関連
67+ const int LIGHT_GRAY = 35; // グレー色の光センサ値
68+
69+ // モーター回転角度関連
70+ const int MORTOR_ANG_BEFORE_STEP_DOWN_LINE_TRACE = 90; // 段差を降りる前のライントレース距離(左右モーターの合計回転角度)
71+ const int MORTOR_ANG_AFTER_STEP_DOWN_LINE_TRACE = 1670; // 段差を降りた後のライントレース距離(左右モーターの合計回転角度)
72+ const int MORTOR_ANG_BACKWARD = -350; // 段差を昇るための助走距離分バック走行する距離(左右モーターの合計回転角度)
73+ const int MORTOR_ANG_STEP_UP = 490; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
74+ const int MORTOR_ANG_STOP = 400; // 段差を昇ったあとから停止する位置の距離(左右モーターの合計回転角度)
75+ const int MORTOR_ANG_STOP_RANGE = 90; // 段差を昇ったあとから停止する位置の誤差許容範囲(左右モーターの合計回転角度)
76+ const int MORTOR_ANG_SPIN_RIGHT = 540; // スピン時の距離(右モーターの回転角度)
77+ const int MORTOR_ANG_SPIN_LEFT = -540; // スピン時の距離(左モーターの回転角度)
78+ const int MORTOR_ANG_SEARCH_LINE_FORWARD = 90; // ライン探索時の前進距離(左右モーターの合計回転角度)
79+ const int MORTOR_ANG_SEARCH_LINE_BACKWARD = -60; // ライン探索時の後進距離(左右モーターの合計回転角度)
80+ const int MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_RIGHT = 30; // スピン時の距離(右モーターの回転角度)
81+ const int MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_LEFT = -30; // スピン時の距離(左モーターの回転角度)
82+ const int MORTOR_ANG_STEP_DOWN = 300; // 段差を昇るときに走行する距離(左右モーターの合計回転角度)
83+ const int MORTOR_ANG_FOR_STOP_DIFF_MAX = 30; // 段差上昇後の停止位置に移動する際、目標位置までの差分距離のMAX値。(これ以上は同じForward値となるよという閾値)
84+ const int MORTOR_ANG_ADJUST_LINE_FORWARD = 100; // ライントレース走行による位置調整時の前進距離(左右モーターの合計回転角度)
85+ const int MORTOR_ANG_ADJUST_LINE_BACKWARD = -80; // ライントレース走行による位置調整時の後進距離(左右モーターの合計回転角度)
86+ const int MORTOR_ANG_LINE_EDGE_CHANGE_FORWARD1 = 120;
87+ const int MORTOR_ANG_LINE_EDGE_CHANGE_FORWARD2 = 60;
88+
89+ // TURN値関連
90+ const int TURN_NONE = 0; // ライントレース無し直線走行時の指定値
91+ const int TURN_SPIN = -60; // スピン時の指定値
92+ const int TURN_CHANGE_ANGLE = -20; // ライン探索時の車体方向角度変更時の指定値
93+ const int TURN_MAX = 60; // TURN値のMAX値(絶対値)
94+ const int TURN_LINE_OVER1 = -10; // ラインをまたぐ際の値
95+ const int TURN_LINE_OVER2 = 10; // ラインをまたぐ際の値
96+
97+
98+ // FORWARD値関連
99+ const int FORWARD_STEP_DETECTION = 30; // 段差検知時の指定値
100+ const int FORWARD_BACKWARD = -70; // 段差を昇るための助走距離分バック走行時指定値
101+ const int FORWARD_STEP_UP = 130; // 段差を昇る時の指定値
102+ const int FORWARD_MAX_FOR_STOP = 30; // 指定停止位置に移動してする際のMAX値
103+ const int FORWARD_SPIN = 0; // スピン時の指定値
104+ const int FORWARD_SEARCH_LINE_FORWARD = 30; // ライン探索前進時の指定値
105+ const int FORWARD_SEARCH_LINE_BACKWARD = -30; // ライン探索後進時の指定値
106+ const int FORWARD_SEARCH_LINE_CHANGE_ANGLE = 0; // スピン探索車体方向変更時の指定値
107+ const int FORWARD_LINE_TRACE = 60; // ライントレース時の指定値
108+ const int FORWARD_STEP_DOWN = 60; // 段差を降りる時の指定値
109+ const int FORWARD_GRAY_SEARCH = 30; // グレーマーカー探索時の指定値
110+ const int FORWARD_ADJUST_LINE_FORWARD = 10; // ライントレース前進による位置調整時の指定値
111+ const int FORWARD_ADJUST_LINE_BACKWARD = -10; // ライントレース位置調整後の後進時の指定値
112+ const int FORWARD_STOP = 0; //
113+
114+ // 階段走行時のPID関連
115+ const float P_VALUE_STAIRS_NORMAL = 2.7;
116+ const float I_VALUE_STAIRS_NORMAL = 0;
117+ const float D_VALUE_STAIRS_NORMAL = 0.1;
118+
119+ // 階段走行時のPID関連
120+ const float P_VALUE_STAIRS_DETECTION = 0.4;
121+ const float I_VALUE_STAIRS_DETECTION = 0;
122+ const float D_VALUE_STAIRS_DETECTION = 0.03;
123+
124+ // 階段スピン時基準電圧
125+ const float STAIRS_SPIN_VOLT = 8200.0;
126+ // 空電池基準電圧
127+ const float STAIRS_EMPTY_VOLT = 6400.0;
128+ // 階段フル基準電圧
129+ const float STAIRS_FULL_VOLT = 9200.0;
130+ // 階段通常時基準電圧
131+ const float STAIRS_NORMAL_VOLT = 9200.0;
132+
133+ GrayChecker *grayChecker;
134+ /**
135+ * @enum StairsState
136+ * 階段走行の各種ステート
137+ */
138+ enum StairsStates
139+ {
140+ StairsStateLineEdgeChangeForward0, // 0
141+ StairsStateLineEdgeChangeForward1, // 1
142+ StairsStateLineEdgeChangeForward2, // 2
143+ StairsState1stStepDetection, // 3 1段目 段差検知モード
144+ StairsState1stBackward, // 4 1段目 段差検知後バック走行モード
145+ StairsState1stStepUp, // 5 1段目 昇段モード
146+ StairsState1stAdjustPosition, // 6 1段目 昇段後位置調整モード
147+ StairsState1stBeforeSpinSearchLineChangeAngle, // 7 1段目 スピン前 ライン探索(進行方向角度変更)モード
148+ StairsState1stBeforeSpinSearchLineForward, // 8 1段目 スピン前 ライン探索(前進)モード
149+ StairsState1stBeforeSpinSearchLineBackward, // 9 1段目 スピン前 ライン探索(後進)モード
150+ StairsState1stBeforeSpinAdjustLineForward, // 10 1段目 スピン前 ライン調整(前進)モード
151+ StairsState1stBeforeSpinAdjustLineBackward, // 11 1段目 スピン前 ライン調整(後進)モード
152+ StairsState1stStop, // 12 1段目 スピン前静止
153+ StairsState1stSpin, // 13 1段目 スピン(360°)モード
154+ StairsState1stAfterSpinSearchLineChangeAngle, // 14 1段目 スピン後 ライン探索(進行方向角度変更)モード
155+ StairsState1stAfterSpinSearchLineForward, // 15 1段目 スピン後 ライン探索(前進)モード
156+ StairsState1stAfterSpinSearchLineBackward, // 16 1段目 スピン後 ライン探索(後進)モード
157+ StairsState1stAfterSpinAdjustLineForward, // 17 1段目 スピン後 ライン調整(前進)モード
158+ StairsState2ndStepDetection, // 18 2段目 段差検知モード
159+ StairsState2ndBackward, // 19 2段目 段差検知誤バック走行モード
160+ StairsState2ndStepUp, // 20 2段目 昇段モード
161+ StairsState2ndAdjustPosition, // 21 2段目 昇段後位置調整モード
162+ StairsState2ndBeforeSpinSearchLineChangeAngle, // 22 2段目 スピン前 ライン探索(進行方向角度変更)モード
163+ StairsState2ndBeforeSpinSearchLineForward, // 23 2段目 スピン前 ライン探索(前進)モード
164+ StairsState2ndBeforeSpinSearchLineBackward, // 24 2段目 スピン前 ライン探索(後進)モード
165+ StairsState2ndBeforeSpinAdjustLineForward, // 25 2段目 スピン前 ライン調整(前進)モード
166+ StairsState2ndBeforeSpinAdjustLineBackward, // 26 2段目 スピン前 ライン調整(後進)モード
167+ StairsState2ndStop, // 27 2段目 スピン前 静止モード
168+ StairsState2ndSpin, // 28 2段目 スピン(360°)モード
169+ StairsState2ndAfterSpinSearchLineChangeAngle, // 29 2段目 スピン後 ライン探索(進行方向角度変更)モード
170+ StairsState2ndAfterSpinSearchLineForward, // 30 2段目 スピン後 ライン探索(前進)モード
171+ StairsState2ndAfterSpinSearchLineBackward, // 31 2段目 スピン後 ライン探索(後進)モード
172+ StairsState2ndAfterSpinAdjustLineForward, // 32 2段目 スピン後 ライン調整(前進)モード
173+ StairsStateBeforeStepDownLineTrace, // 33 降段前ライントレースモード
174+ StairsStateStepDown, // 34 降段モード
175+ StairsStateAfterStepDownSearchLineChangeAngle, // 35 降段後 ライン探索(進行方向角度変更)モード
176+ StairsStateAfterStepDownSearchLineForward, // 36 降段後 ライン探索(前進)モード
177+ StairsStateAfterStepDownSearchLineBackward, // 37 降段後 ライン探索(後進)モード
178+ StairsStateAfterStepDownLineTrace, // 38 降段後ライントレースモード
179+ StairsStateGraySearch, // 39 グレーライン検知モード
180+ };
181+ StairsStates stairsState; // 階段走行のステート
182+
183+
184+ /**
185+ * @enum UpdatePidValueMode
186+ * PID変更時の詳細モード
187+ */
188+ enum ModeUpdatePidValues
189+ {
190+ BlackLineTraceMode,
191+ GrayLineTraceMode,
192+ };
193+ ModeUpdatePidValues modeUpdatePidValue; // PID変更時の詳細モード
194+
195+ /**
196+ * @enum NormalizeTurnValueMode
197+ * Turn値変更時の詳細モード
198+ */
199+ enum ModeNormalizeTurnValues
200+ {
201+ NoneTurnMode,
202+ SpinTurnMode,
203+ ChangeAngleTurnMode,
204+ LineTraceTurnMode,
205+ LineOverMode1,
206+ LineOverMode2,
207+ };
208+ ModeNormalizeTurnValues modeNormalizeTurnValue; // Turn値変更時の詳細モード
209+
210+ /**
211+ * @enum SetForwardMode
212+ * Forward値設定時の詳細モード
213+ */
214+ enum ModeSetForwards
215+ {
216+ StepDetectionMode,
217+ BackwardMode,
218+ StepUpMode,
219+ AdjustPostionMode,
220+ SpinMode,
221+ StopMode,
222+ SearchLineForwardMode,
223+ SearchLineBackwardMode,
224+ SearchLineChangeAngleMode,
225+ LineTraceMode,
226+ StepDownMode,
227+ GraySearchMode,
228+ AdjustLineForwardMode,
229+ AdjustLineBackwardMode,
230+ };
231+ ModeSetForwards modeSetForward; // Forward値設定時の詳細モード
232+
233+ /**
234+ * @enum SetForwardMode
235+ * Forward値設定時の詳細モード
236+ */
237+ enum ModeSetPIDData
238+ {
239+ StepDetectionPIDMode,
240+ OtherPIDMode
241+ };
242+ ModeSetPIDData modeSetPIDData; // Forward値設定時の詳細モード
243+
244+
245+
246+ int32_t wk_motor_ang_LR; // モード単位のR+Lモーター回転角度累積ワーク
247+ int32_t wk_motor_ang_r; // モード単位のRモーター回転角度累積ワーク
248+ int32_t wk_motor_ang_l; // モード単位のLモーター回転角度累積ワーク
249+ int counter; // カウンター
250+ std::vector<int> m_colorCensorList; // カラーセンサー値累積用リスト
251+ std::vector<int> m_motorAngleList; // モーター回転角度履歴リスト
252+
253+ // PID制御用
254+ std::vector<float> m_hensaList;
255+
256+ // PID制御に使用する係数値
257+ float m_pFixVal;
258+ float m_iFixVal;
259+ float m_dFixVal;
260+
261+ // ライン探索で進んだ距離
262+ int addBackRange = 0;
263+
264+ // ライン探索回数
265+ int searchCnt = 0;
266+
267+};
268+
269+#endif /* STAIRS_H */
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalR.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalR.h (revision 410)
@@ -0,0 +1,30 @@
1+#ifndef STARTTOGOALR_H
2+#define STARTTOGOALR_H
3+
4+#include "BaseCourse.h"
5+
6+#define WK 0
7+
8+class StartToGoalR : public BaseCourse{
9+public:
10+ StartToGoalR();
11+ virtual ~StartToGoalR();
12+
13+ void SetPIDData();
14+ bool IsNextCourse();
15+ void setForward();
16+private:
17+ int mode = 0;
18+ enum RunMode {
19+ FirstStraight,
20+ Straight,
21+ Straight2,
22+ SpeedDown,
23+ FirstCurve,
24+ SecondCurve,
25+ ThirdCurve
26+ };
27+};
28+
29+#endif
30+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Garage.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Garage.h (revision 410)
@@ -0,0 +1,48 @@
1+#ifndef GARAGE_H
2+#define GARAGE_H
3+
4+#include "BaseCourse.h"
5+#include "Stopper.h"
6+
7+class Garage : public BaseCourse {
8+public:
9+ Garage(int courseType);
10+ virtual ~Garage();
11+
12+ void SetPIDData();
13+ void Run();
14+ void setForward();
15+ bool IsNextCourse(){ return false; }
16+
17+private:
18+ Garage(const Garage& orig){}
19+
20+ void MoveDistance();
21+ void Stop();
22+ void Adjustment();
23+ void Finish();
24+ bool AngleCheck();
25+ int p_courseType;
26+ double GetDistance();
27+
28+private:
29+ enum GarageState
30+ {
31+ GarageStateRun,
32+ GarageStateStop,
33+ GarageStateAdjustment,
34+ GarageStateFinish,
35+ GarageStateWait,
36+ GarageStateMoveDistance,
37+ };
38+
39+ GarageState garageState;
40+ bool m_isFinished;
41+ Stopper stopper;
42+ int waitCount;
43+ int targetDistance;
44+
45+};
46+
47+#endif /* GARAGE_H */
48+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/GrayChecker.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/GrayChecker.h (revision 410)
@@ -0,0 +1,42 @@
1+/*
2+ * File: GrayChecker.h
3+ * Author: ncxx-sl
4+ *
5+ * Created on 2016/08/21, 15:04
6+ */
7+
8+#ifndef GRAYCHECKER_H
9+#define GRAYCHECKER_H
10+
11+#include <vector>
12+
13+class GrayChecker {
14+public:
15+ GrayChecker();
16+ virtual ~GrayChecker();
17+
18+ bool IsGrayLineEnd(int color);
19+
20+ bool IsCurving(int motorAngL, int motorAngR);
21+
22+private:
23+ enum State
24+ {
25+ BeforeOnGray,
26+ OnGray,
27+ AfterGray,
28+ };
29+ GrayChecker(const GrayChecker& orig){}
30+
31+ State m_state;
32+
33+ std::vector<int> m_colorSensorList; // カラーセンサー値累積用リスト
34+
35+ int m_beforeAngL;
36+ int m_beforeAngR;
37+ std::vector<int> m_motorAngDiffList;
38+
39+};
40+
41+#endif /* GRAYCHECKER_H */
42+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stopper.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stopper.cpp (revision 410)
@@ -0,0 +1,84 @@
1+#include "Stopper.h"
2+
3+#include "CommonApp.h"
4+
5+Stopper::Stopper()
6+ : m_tailDownCompleted(false)
7+ , m_tailMountCount(0)
8+{
9+}
10+
11+Stopper::~Stopper() {
12+}
13+
14+bool Stopper::DoStop()
15+{
16+ static int count = 0;
17+ log("DoStop count:%d, forward:%0.3f ", ++count, forward);
18+
19+ if(!m_tailDownCompleted)
20+ {
21+ log("route1");
22+ gyro_offset = -3;
23+
24+ // 停止と尻尾下し
25+ Run();
26+ m_tailDownCompleted = TailDown();
27+ }
28+ else
29+ {
30+ log("route2");
31+ gyro_offset = 0;
32+
33+ // 尻尾に車体を預ける
34+ if(m_tailMountCount < 40)
35+ {
36+ log("tailcount:%d", m_tailMountCount);
37+ ev3_motor_set_power(right_motor, 35);
38+ ev3_motor_set_power(left_motor, 35);
39+ ++m_tailMountCount;
40+ }
41+ else
42+ {
43+ ev3_motor_set_power(right_motor, 0);
44+ ev3_motor_set_power(left_motor, 0);
45+ m_tailDownCompleted = false;
46+ m_tailMountCount = 0;
47+
48+ log(" DoStopEnd!!!");
49+
50+ return true;
51+ }
52+ }
53+
54+ return false;
55+}
56+
57+void Stopper::Run()
58+{
59+ forward = 0.0F;
60+ turn = 0.0F;
61+
62+ // 各センサーからの値を取得
63+ updateSensorValue();
64+
65+ // モーター出力の取得
66+ calcMotorPower();
67+
68+ // モーター出力の設定
69+ setMotorPower();
70+}
71+
72+bool Stopper::TailDown()
73+{
74+ m_tailAngle = (int)ev3_motor_get_counts(tail_motor);
75+
76+ log("angle:%d, ", m_tailAngle);
77+
78+ const float targetAngle = 83.0F;
79+ float pwm = (float)(targetAngle - ev3_motor_get_counts(tail_motor))*P_GAIN; /* 比例制御 */
80+
81+ log("pwm:%d, %d", pwm, (signed char)pwm);
82+
83+ return CommonApp::tail_control(targetAngle, 20);
84+}
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/LookupGate.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/LookupGate.h (revision 410)
@@ -0,0 +1,151 @@
1+/*
2+ * File: LookupGate.h
3+ * Author: h.kitoh
4+ *
5+ * Created on 2016/08/14, 10:39
6+ */
7+
8+#ifndef LOOKUPGATE_H
9+#define LOOKUPGATE_H
10+
11+#define P_GAIN_LOOKUP_GATE 2.5F /* 完全停止用モータ制御比例係数 */
12+#define PWM_ABS_MAX_LOOKUP_GATE 20 /* 完全停止用モータ制御PWM絶対最大値 */
13+#define SONAR_ALERT_DISTANCE 10/* 超音波センサによる障害物検知距離[cm] */
14+#define LIGHT_WHITE_LOOKUP_GATE 12 /* 白色の光センサ値 */
15+#define LIGHT_GRAY_LOOKUP_GATE 6 /* 灰色の光センサ値 */
16+#define LIGHT_BLACK_LOOKUP_GATE 1 /* 黒色の光センサ値 */
17+#define COLOR_LIST_COUNT_SEARCHLINE_LOOKUP_GATE 3
18+#define COLOR_LIST_COUNT_SEARCHLINEEDGE_LOOKUP_GATE 5
19+#define KP 1.0 /* P制御用係数*/
20+#define KI 1.5 /* I制御用係数*/
21+#define KD 1.5 /* D制御用係数*/
22+
23+#include "BaseCourse.h"
24+#include "CommonApp.h"
25+#include "ev3api.h"
26+
27+//////////////////////////////////
28+#include "Stopper.h"
29+//////////////////////////////////
30+
31+class GrayChecker;
32+
33+class LookupGate : public BaseCourse {
34+ public:
35+ LookupGate();
36+ virtual ~LookupGate();
37+
38+ void setForward();
39+ void runUntilSonorDetected();
40+ void runSonorDetected();
41+ void Run();
42+ void ForwardRun();
43+ void ForwardRunAjustment();
44+ void throughGateReady1st();
45+ void throughGateReady2nd();
46+ void throughGateReady3rd();
47+ void throughGateReady4th();
48+ void throughGateReady5th();
49+ void throughGateReady6th();
50+ void throughGateReady7th();
51+ void throughGateReady8th();
52+ void RunStop();
53+ void UpridhtBody1st();
54+ void UpridhtBody2nd();
55+ void UpridhtBody3rd();
56+ void UpridhtBody4th();
57+ void UpridhtBody5th();
58+ void ChangeGarage();
59+ void GraySerchRun();
60+ void SetPIDData();
61+ void calcMotorPower();
62+ bool IsNextCourse();
63+ bool tail_control(signed int angle);
64+ virtual int sonar_alert(void);
65+ private:
66+ LookupGate(const LookupGate& orig){}
67+ bool m_stop;
68+
69+ bool SpinRight();
70+ bool SpinLeft();
71+ void LineTraceWithTail();
72+
73+ private:
74+ enum LookupGateState
75+ {
76+ LookupGateStateBeforeSonorDetected,
77+ LookupGateStateAfterSonorDetected,
78+ LookupGateStateThroughGateReady1st,
79+ LookupGateStateThroughGateReady2nd,
80+ LookupGateStateThroughGateReady3rd,
81+ LookupGateStateThroughGateReady4th,
82+ LookupGateStateThroughGateReady5th,
83+ LookupGateStateThroughGateReady6th,
84+ LookupGateStateThroughGateReady7th,
85+ LookupGateStateThroughGateReady8th,
86+ LookupGateStateThroughGateForward,
87+ LookupGateStateThroughGateForwardAjustment,
88+ LookupGateStateThroughGateStop,
89+ LookupGateStateUpridhtBody1st,
90+ LookupGateStateUpridhtBody2nd,
91+ LookupGateStateUpridhtBody3rd,
92+ LookupGateStateUpridhtBody4th,
93+ LookupGateStateUpridhtBody5th,
94+ LookupGateStateBodyDown1st,
95+ LookupGateStateBodyDown2nd,
96+ LookupGateStateBodyDown3rd,
97+ LookupGateStateBodyDown4th,
98+ LookupGateStateBodyDown5th,
99+ LookupGateStateBodyDown6th,
100+ LookupGateStateGraySerchRun,
101+ LookupGateStateChangeGarage,
102+ LookupGateStateRun,
103+ LookupGateStateBeforeSpin,
104+ LookupGateStateSpinRightWheel,
105+ LookupGateStateSpinLeftWheel,
106+ LookupGateStateAfterSpin,
107+ };
108+ LookupGateState lookupGateState;
109+ enum RunMode
110+ {
111+ normalRun,
112+ run84,
113+ run66
114+ };
115+ RunMode runMode;
116+ GrayChecker *grayChecker;
117+
118+ unsigned int distance = 0;
119+ signed char pwm = 0;
120+ int32_t distanceWork;
121+ int32_t distanceRWork;
122+ int32_t distanceLWork;
123+ int ForwardCounter = 0;
124+ int ForwardCompareCounter = 0;
125+ int counter;
126+ int forwardMotorAngL = 0;
127+ int UpridhtBodyCount = 0;
128+ int32_t targetSpinCount = 0;
129+ int32_t targetSpinCountBack = 0;
130+ const int COLOR_LIST_COUNT_SEARCHLINE = 3; // ライン探索時のカラー値累積カウント
131+ // カラーセンサー値関連
132+ const int LIGHT_GRAY = 9; // グレー色の光センサ値
133+ std::vector<int> m_colorCensorList;
134+ const sensor_port_t
135+ touch_sensor = EV3_PORT_1,
136+ sonar_sensor = EV3_PORT_2,
137+ color_sensor = EV3_PORT_3,
138+ gyro_sensor = EV3_PORT_4;
139+
140+ const motor_port_t
141+ left_motor = EV3_PORT_C,
142+ right_motor = EV3_PORT_B,
143+ tail_motor = EV3_PORT_A;
144+
145+//////////////////////////////////
146+ Stopper stopper;
147+ int waitCount = 0;
148+//////////////////////////////////
149+};
150+
151+#endif /* LOOKUPGATE_H */
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalL.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalL.cpp (revision 410)
@@ -0,0 +1,165 @@
1+#include "StartToGoalL.h"
2+
3+StartToGoalL::StartToGoalL() {
4+ mode = Straight;
5+}
6+
7+StartToGoalL::~StartToGoalL() {
8+}
9+
10+/*次モードへの移行
11+ 左右モータの総回転角からモードを走行モードを切り替える*/
12+bool StartToGoalL::IsNextCourse()
13+{
14+ // 左右モータ総合回転角平均
15+ double totalAvg = (motor_ang_r + motor_ang_l) / 2;
16+ //forward = 20;
17+
18+ if(totalAvg < 40) {
19+ gyro_offset = -5;
20+ }
21+ else {
22+ gyro_offset = 0;
23+ }
24+
25+ if(totalAvg < 2600) {
26+ //スタートからA地点手前まで ストレート
27+ mode = FirstStraight;
28+ }
29+ else if(totalAvg < 2800) {
30+ // スピードダウン
31+ mode = SpeedDown;
32+ // 次モードのforward値
33+ minSpeed = 120;
34+ }
35+ else if(totalAvg < 4200) {
36+ //A地点からB地点まで カーブ
37+ mode = Curve;
38+ }
39+ else if(totalAvg < 6400) {
40+ //B地点からC地点まで ストレート
41+ mode = Straight;
42+ }
43+ else if(totalAvg < 7100) {
44+ //C地点からD地点まで カーブ
45+ mode = Curve2;
46+ }
47+ // 要速度調整
48+ else if(totalAvg < 7650) {
49+ //D地点からE地点まで ストレート
50+ mode = Curve2_3;
51+ }
52+ // 要PID制御調整
53+ else if(totalAvg < 8600) {
54+ //E地点からF地点まで カーブ
55+ mode = Curve3;
56+ }
57+ else if(totalAvg < 11150) {
58+ //F地点からG地点手前まで ストレート
59+ mode = Straight;
60+ }
61+ else if(totalAvg < 11500) {
62+ // スピードダウン
63+ mode = SpeedDown;
64+ // 次モードのforward値
65+ minSpeed = 120;
66+ }
67+ // 要PID制御調整
68+ else if(totalAvg < 12000) {
69+ //G地点からH地点まで カーブ
70+ mode = Curve4;
71+ }
72+ else {
73+ // ゴール通過後 次の走行クラス(難所)へ切り替え
74+ return true;
75+ }
76+
77+ // ログ出力 コメントアウト
78+ int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
79+ int touchSensor = ev3_touch_sensor_is_pressed(EV3_PORT_1);
80+ //if(touchSensor == 1){
81+ log("%f, %d, %f, %d, %d, %d, %f\r\n", totalAvg, mode, forward, colorSensor, volt, touchSensor, turn);
82+ //}
83+ return false;
84+}
85+
86+void StartToGoalL::updatePidValue() {
87+
88+ const signed int LIGHT_GRAY2 = 25;
89+ const signed int LIGHT_WHITE2 = 54;
90+ const signed int LIGHT_BLACK2 = 0;
91+
92+ switch (mode)
93+ {
94+ case SpeedDown2:
95+ // グレーと白の中間を走るPID制御を実施
96+ PIDMgr->SetColor(LIGHT_GRAY2, LIGHT_WHITE2);
97+ turn = PIDMgr->GetTurnValue();
98+ break;
99+ default:
100+ // 黒と白の中間を走るPID制御を実施
101+ PIDMgr->SetColor(LIGHT_BLACK2, LIGHT_WHITE2);
102+ turn = PIDMgr->GetTurnValue();
103+ break;
104+
105+ }
106+}
107+
108+
109+/*PID制御処理呼び出し
110+ modeによる適正値を設定し、PID制御呼び出し*/
111+void StartToGoalL::SetPIDData() {
112+ if(mode == Straight || mode == FirstStraight || mode == SpeedDown || mode == Curve2_3) {
113+ PIDMgr->SetFixedData(0.5, 0, 0.03);
114+ }
115+ else if(mode == SpeedDown2) {
116+ PIDMgr->SetFixedData(1, 0, 0.03);
117+ }
118+ else if(mode == Curve){
119+ PIDMgr->SetFixedData(0.5, 0.1, 0.03);
120+ }
121+ else if(mode == Curve2){
122+ PIDMgr->SetFixedData(1, 0.8, 0.06);
123+ }
124+ else if(mode == Curve3 || mode == Curve4){
125+ PIDMgr->SetFixedData(1.25, 1, 0.09);
126+ }
127+ else{
128+ PIDMgr->SetFixedData(1, 0.3, 0.06);
129+ }
130+}
131+
132+/*forward値設定
133+ モードからforward値を設定*/
134+void StartToGoalL::setForward() {
135+ //forward = 30;
136+ if (mode == FirstStraight) {
137+ SpeedUp(FIRST_STRAIGHT_SPEED);
138+ }
139+ else if(mode == Straight) {
140+ SpeedUp(STRAIGHT_SPEED);
141+ }
142+ else if(mode == SpeedDown) {
143+ BaseCourse::SpeedDown(minSpeed);
144+ }
145+ else if(mode == SpeedDown2) {
146+ BaseCourse::SpeedDown(minSpeed);
147+ }
148+ else if(mode == Curve) {
149+ forward = 150;
150+ }
151+ else if(mode == Curve2) {
152+ forward = 120;
153+ }
154+ else if(mode == Curve2_3){
155+ forward = 100;
156+ }
157+ else if(mode == Curve3) {
158+ forward = 110;
159+ }
160+ else if(mode == Curve4) {
161+ forward = 30;
162+ }
163+}
164+
165+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stopper.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stopper.h (revision 410)
@@ -0,0 +1,26 @@
1+#ifndef STOPPER_H
2+#define STOPPER_H
3+
4+#include "BaseCourse.h"
5+
6+class Stopper : public BaseCourse {
7+public:
8+ Stopper();
9+ virtual ~Stopper();
10+
11+ bool DoStop();
12+
13+private:
14+ Stopper(const Stopper& orig){}
15+
16+ void Run();
17+ bool TailDown();
18+
19+private:
20+ bool m_tailDownCompleted;
21+ int m_tailMountCount;
22+ int m_tailAngle;
23+};
24+
25+#endif /* STOPPER_H */
26+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalL.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalL.h (revision 410)
@@ -0,0 +1,33 @@
1+#ifndef STARTTOGOALL_H
2+#define STARTTOGOALL_H
3+#define LIGHT_GRAY 25
4+
5+#include "BaseCourse.h"
6+#include "CommonApp.h"
7+
8+class StartToGoalL : public BaseCourse{
9+public:
10+ StartToGoalL();
11+ virtual ~StartToGoalL();
12+
13+ void SetPIDData();
14+ bool IsNextCourse();
15+ void setForward();
16+ void updatePidValue();
17+private:
18+ int mode = 0;
19+ enum RunMode {
20+ FirstStraight,
21+ Straight,
22+ SpeedDown,
23+ SpeedDown2,
24+ Curve,
25+ Curve2,
26+ Curve2_3,
27+ Curve3,
28+ Curve4
29+ };
30+};
31+
32+#endif
33+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stairs.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Stairs.cpp (revision 410)
@@ -0,0 +1,1280 @@
1+/*
2+ * File: Stairs.cpp
3+ * Author: y.nakamura
4+ *
5+ * Created on 2016/08/07, 16:16
6+ */
7+#include "Stairs.h"
8+#include "GrayChecker.h"
9+#include <algorithm>
10+Stairs::Stairs(): stairsState(StairsState1stStepDetection)
11+{
12+ // 各種初期化処理
13+ grayChecker = new GrayChecker();
14+ m_hensaList.clear();
15+ m_hensaList.push_back(0.0f);
16+
17+ // モード設定
18+ changeMode();
19+
20+}
21+
22+Stairs::~Stairs()
23+{
24+ //解放
25+ if(grayChecker != NULL) delete [] grayChecker;
26+ std::vector<int>().swap(m_colorCensorList);
27+ std::vector<int>().swap(m_motorAngleList);
28+ std::vector<float>().swap(m_hensaList);
29+}
30+
31+/**
32+ * @brief 走行処理(オーバーライド)
33+ * @note
34+ * 階段走行時のモードに従い処理を分岐する
35+ *
36+ */
37+void Stairs::Run()
38+{
39+
40+
41+ switch(stairsState)
42+ {
43+ case StairsStateLineEdgeChangeForward0:
44+ AdjustLineForward();
45+
46+ case StairsStateLineEdgeChangeForward1:
47+ LineEdgeChangeForward(MORTOR_ANG_LINE_EDGE_CHANGE_FORWARD1);
48+ break;
49+ case StairsStateLineEdgeChangeForward2:
50+ LineEdgeChangeForward(MORTOR_ANG_LINE_EDGE_CHANGE_FORWARD2);
51+ break;
52+
53+ //段差検知モード
54+ case StairsState1stStepDetection:
55+ case StairsState2ndStepDetection:
56+ StepDetection();
57+ break;
58+
59+ //バック走行
60+ case StairsState1stBackward:
61+ case StairsState2ndBackward:
62+ Backward(MORTOR_ANG_BACKWARD);
63+ break;
64+ //昇段モード
65+ case StairsState1stStepUp:
66+ case StairsState2ndStepUp:
67+ gyro_offset = STEP_UP_GYRO_OFFSET;
68+ StepUp(MORTOR_ANG_STEP_UP);
69+ break;
70+
71+ // 昇段後位置調整&静止モード
72+ case StairsState1stAdjustPosition:
73+ case StairsState2ndAdjustPosition:
74+ AdjustPosition((motor_ang_l + motor_ang_r) - wk_motor_ang_LR, MORTOR_ANG_LIST_COUNT_STOP, MORTOR_ANG_STOP, MORTOR_ANG_STOP_RANGE);
75+ break;
76+
77+ // ライン調整モード
78+ case StairsState1stBeforeSpinAdjustLineForward:
79+ case StairsState1stAfterSpinAdjustLineForward:
80+ case StairsState2ndBeforeSpinAdjustLineForward:
81+ case StairsState2ndAfterSpinAdjustLineForward:
82+ AdjustLineForward();
83+ break;
84+
85+ // ライン調整モード
86+ case StairsState1stBeforeSpinAdjustLineBackward:
87+// case StairsState1stAfterSpinAdjustLineBackward:
88+ case StairsState2ndBeforeSpinAdjustLineBackward:
89+// case StairsState2ndAfterSpinAdjustLineBackward:
90+ AdjustLineBack();
91+ break;
92+ case StairsState2ndStop:
93+ case StairsState1stStop:
94+ Stop();
95+ break;
96+
97+ // スピン(360°)モード
98+ case StairsState1stSpin:
99+ case StairsState2ndSpin:
100+ gyro_offset = SPIN_GYRO_OFFSET;
101+ Spin();
102+ break;
103+
104+ // ライン探索モード(前進)モード
105+ case StairsState1stBeforeSpinSearchLineForward:
106+ case StairsState1stAfterSpinSearchLineForward:
107+ case StairsState2ndBeforeSpinSearchLineForward:
108+ case StairsState2ndAfterSpinSearchLineForward:
109+ case StairsStateAfterStepDownSearchLineForward:
110+ SearchLineForward();
111+ break;
112+
113+ // ライン探索モード(後進)モード
114+ case StairsState1stBeforeSpinSearchLineBackward:
115+ case StairsState1stAfterSpinSearchLineBackward:
116+ case StairsState2ndBeforeSpinSearchLineBackward:
117+ case StairsState2ndAfterSpinSearchLineBackward:
118+ case StairsStateAfterStepDownSearchLineBackward:
119+ SearchLineBackward();
120+ break;
121+
122+ // ライン探索モード(進行方向角度変更)モード
123+ case StairsState1stBeforeSpinSearchLineChangeAngle:
124+ case StairsState1stAfterSpinSearchLineChangeAngle:
125+ case StairsState2ndBeforeSpinSearchLineChangeAngle:
126+ case StairsState2ndAfterSpinSearchLineChangeAngle:
127+ case StairsStateAfterStepDownSearchLineChangeAngle:
128+ SearchLineChangeAngle();
129+ break;
130+
131+ // ライントレースモード
132+ case StairsStateBeforeStepDownLineTrace:
133+ LineTrace(MORTOR_ANG_BEFORE_STEP_DOWN_LINE_TRACE);
134+ break;
135+ case StairsStateAfterStepDownLineTrace:
136+ LineTrace(MORTOR_ANG_AFTER_STEP_DOWN_LINE_TRACE);
137+ break;
138+
139+ // 降段モード
140+ case StairsStateStepDown:
141+ StepDown();
142+ break;
143+
144+ // グレーライントレースモード
145+ case StairsStateGraySearch:
146+ GraySearch();
147+ break;
148+ }
149+}
150+
151+/**
152+ * @brief 段差検知
153+ * @note
154+ * ジャイロ値を元に段差を検知する
155+ * 段差検知したら次のモードに移行
156+ */
157+void Stairs::StepDetection()
158+{
159+ BaseCourse::Run();
160+ log("StepDetection %d \r\n", gyro);
161+ if(STEP_DETECTION_GYRO < std::abs(gyro))
162+ {
163+ changeNextState();
164+ }
165+}
166+
167+
168+/**
169+ * @brief バック走行
170+ * @note
171+ * 一定距離をバック走行して段差を昇るための助走距離分戻る
172+ * 一定距離を走ったら次のモードに移行
173+ */
174+void Stairs::Backward(int motor_ang)
175+{
176+ BaseCourse::Run();
177+ //log("Backward %d, %d, %d, %d \r\n", wk_motor_ang_l, wk_motor_ang_r, motor_ang_l, motor_ang_r);
178+ if(motor_ang > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
179+ {
180+ changeNextState();
181+ }
182+}
183+
184+/**
185+ * @brief ラインエッジ変更
186+ * @note
187+ * 左エッジトレースから右エッジトレースに変更
188+ */
189+void Stairs::LineEdgeChangeForward(int motor_ang)
190+{
191+ BaseCourse::Run();
192+ if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
193+ {
194+ changeNextState();
195+ }
196+}
197+
198+/**
199+ * @brief 昇段
200+ * @note
201+ * 一定距離を走行して段差を昇る
202+ * 段差を昇ったら次のモードに移行
203+ */
204+void Stairs::StepUp(int motor_ang)
205+{
206+ BaseCourse::Run();
207+ if(motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
208+ {
209+ changeNextState();
210+ }
211+}
212+
213+/**
214+ * @brief 静止+位置調整
215+ * @note
216+ * 段差を上った後のふらつきを止めて、スピンする場所に移動する
217+ * 指定場所に一定時間とどまったら次のモードに移行
218+ */
219+void Stairs::AdjustPosition(int currentAngle, unsigned int listSize, int targetAngle, int range)
220+{
221+ BaseCourse::Run();
222+ if (judgeMotorAngle(currentAngle, listSize, targetAngle, range))
223+ {
224+ changeNextState();
225+ }
226+}
227+
228+void Stairs::Stop()
229+{
230+ BaseCourse::Run();
231+
232+ if (counter > 300)
233+ {
234+ changeNextState();
235+ }
236+ ++counter;
237+}
238+
239+/**
240+ * @brief スピン
241+ * @note
242+ * その場でスピンする
243+ * 一定角度スピンしたら次のモードに移行
244+ */
245+void Stairs::Spin()
246+{
247+ BaseCourse::Run();
248+ if (MORTOR_ANG_SPIN_RIGHT < motor_ang_r - wk_motor_ang_r &&
249+ MORTOR_ANG_SPIN_LEFT > motor_ang_l - wk_motor_ang_l)
250+ {
251+ changeNextState();
252+ }
253+}
254+
255+/**
256+ * @brief ライン調整
257+ * @note
258+ */
259+void Stairs::AdjustLineForward()
260+{
261+ BaseCourse::Run();
262+
263+
264+
265+ if(MORTOR_ANG_ADJUST_LINE_FORWARD + (searchCnt * 50) < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
266+ {
267+ changeNextState();
268+ }
269+}
270+
271+/**
272+ * @brief ライン調整
273+ * @note
274+ */
275+void Stairs::AdjustLineBack()
276+{
277+ BaseCourse::Run();
278+// if (counter > 500)
279+// {
280+// changeNextState();
281+// }
282+// counter++;
283+
284+// int adjust = 0;
285+// if (stairsState == StairsState2ndBeforeSpinAdjustLineBackward) {
286+// adjust = 90;
287+// }
288+
289+// if(MORTOR_ANG_ADJUST_LINE_BACKWARD - addBackRange - adjust > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
290+ searchCnt = 0;
291+ if (
292+ (MORTOR_ANG_ADJUST_LINE_BACKWARD - addBackRange > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
293+ //|| (stairsState == StairsState2ndBeforeSpinAdjustLineBackward)
294+ )
295+ {
296+ changeNextState();
297+ }
298+}
299+
300+
301+/**
302+ * @brief ライン探索+前進
303+ * @note
304+ * ライン探索しながら前進する
305+ * 一定距離前進したらライン探索モード(後進)モードに移行
306+ * ラインを発見したらしたら段差検知モードに移行
307+ */
308+void Stairs::SearchLineForward()
309+{
310+ BaseCourse::Run();
311+
312+
313+ log("SearchLineForward %d, %d, %d, %d \r\n", (int)MORTOR_ANG_SEARCH_LINE_FORWARD, (int)motor_ang_l, (int)motor_ang_r, (int)(wk_motor_ang_LR));
314+
315+ if (MORTOR_ANG_SEARCH_LINE_FORWARD < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
316+ {
317+ searchCnt += 1;
318+ changeNextState();
319+ }
320+ if (judgeColor(COLOR_LIST_COUNT_SEARCHLINE, (LIGHT_WHITE + LIGHT_BLACK) / 2))
321+ {
322+ // 前進した分だけ後でバックするために進んでいる距離を退避
323+ addBackRange = (motor_ang_l + motor_ang_r) - wk_motor_ang_LR;
324+
325+ changeNextState(1);
326+ }
327+}
328+
329+/**
330+ * @brief ライン探索+後進
331+ * @note
332+ * ライン探索しながら後進する
333+ * 一定距離後進したらライン探索モード(進行方向角度変更)モードに移行
334+ * ラインを発見したらしたら段差検知モードに移行
335+ */
336+void Stairs::SearchLineBackward()
337+{
338+ BaseCourse::Run();
339+
340+ if (MORTOR_ANG_SEARCH_LINE_BACKWARD >= (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
341+ {
342+ changeNextState(-3);
343+ }
344+ if (judgeColor(COLOR_LIST_COUNT_SEARCHLINE, (LIGHT_WHITE + LIGHT_BLACK) / 2))
345+ {
346+ changeNextState();
347+ }
348+}
349+
350+/**
351+ * @brief ライン探索+進行方向角度変更
352+ * @note
353+ * ライン探索しながら進行方向角度を変更する
354+ * 一定角度回転したらライン探索モード(前進)モードに移行
355+ * ラインを発見したらしたら段差検知モードに移行
356+ */
357+void Stairs::SearchLineChangeAngle()
358+{
359+ BaseCourse::Run();
360+// log("Spin %d, %d, %d, %d \r\n", wk_motor_ang_l, wk_motor_ang_r, motor_ang_l, motor_ang_r);
361+// if (MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_RIGHT < motor_ang_r - wk_motor_ang_r &&
362+// MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_LEFT > motor_ang_l - wk_motor_ang_l)
363+ if (MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_RIGHT < motor_ang_r - wk_motor_ang_r &&
364+ MORTOR_ANG_SEARCH_LINE_CHANGE_ANGLE_LEFT > motor_ang_l - wk_motor_ang_l)
365+ {
366+ changeNextState();
367+ }
368+ if (judgeColor(COLOR_LIST_COUNT_SEARCHLINE, (LIGHT_WHITE + LIGHT_BLACK) / 2))
369+ {
370+ changeNextState(2);
371+ }
372+}
373+
374+/**
375+ * @brief ライントレース走行
376+ * @note
377+ * 一定距離を走行する
378+ * 一定距離を走行したら次のモードに移行
379+ */
380+void Stairs::LineTrace(int motor_ang)
381+{
382+ BaseCourse::Run();
383+ if (motor_ang < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
384+ {
385+ changeNextState();
386+ }
387+}
388+
389+/**
390+ * @brief 降段
391+ * @note
392+ * 一定距離を走行して段差を降りる
393+ * 段差を降りたら次のモードに移行
394+ */
395+void Stairs::StepDown()
396+{
397+ BaseCourse::Run();
398+ if (MORTOR_ANG_STEP_DOWN < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
399+ {
400+ changeNextState();
401+ }
402+}
403+
404+/**
405+ * @brief ライントレース走行
406+ * @note
407+ * ライン復帰を行い復帰後、一定距離走行する
408+ * 一定距離走行したらガレージモードに移行
409+ */
410+void Stairs::TraceRun()
411+{
412+ // ベースの走行処理を実行するだけ
413+ BaseCourse::Run();
414+}
415+
416+/**
417+ * @brief ライントレース走行
418+ * @note
419+ * ライン復帰を行い復帰後、一定距離走行する
420+ * 一定距離走行したらガレージモードに移行
421+ */
422+void Stairs::GraySearch()
423+{
424+ BaseCourse::Run();
425+}
426+/**
427+ * @brief PID値の設定(オーバーライド)
428+ * @note
429+ * 階段走行時のPID値を指定する
430+ *
431+ */
432+void Stairs::SetPIDData()
433+{
434+ switch (modeSetPIDData)
435+ {
436+ case StepDetectionPIDMode:
437+ PIDMgr->SetFixedData(P_VALUE_STAIRS_DETECTION, I_VALUE_STAIRS_DETECTION, D_VALUE_STAIRS_DETECTION);
438+ m_pFixVal = P_VALUE_STAIRS_DETECTION;
439+ m_iFixVal = I_VALUE_STAIRS_DETECTION;
440+ m_dFixVal = D_VALUE_STAIRS_DETECTION;
441+ break;
442+ case OtherPIDMode:
443+ PIDMgr->SetFixedData(P_VALUE_STAIRS_NORMAL, I_VALUE_STAIRS_NORMAL, D_VALUE_STAIRS_NORMAL);
444+ m_pFixVal = P_VALUE_STAIRS_NORMAL;
445+ m_iFixVal = I_VALUE_STAIRS_NORMAL;
446+ m_dFixVal = D_VALUE_STAIRS_NORMAL;
447+ break;
448+ }
449+
450+
451+}
452+
453+/**
454+ * @brief PID関係の各値更新(オーバーライド)
455+ * @note
456+ * モードによって呼び出すPID計算処理を切り替える
457+ *
458+ */
459+void Stairs::updatePidValue() {
460+ // 階段クラスのPID制御を実施
461+// switch(stairsState)
462+// {
463+// case StairsState1stBackward:
464+// case StairsState1stStepUp:
465+// case StairsState1stAdjustPosition:
466+// case StairsState1stBeforeSpinSearchLineForward:
467+// case StairsState1stBeforeSpinSearchLineBackward:
468+// case StairsState1stAfterSpinSearchLineForward:
469+// case StairsState1stAfterSpinSearchLineBackward:
470+// case StairsState2ndBackward:
471+// case StairsState2ndStepUp:
472+// case StairsState2ndAdjustPosition:
473+// case StairsState2ndBeforeSpinSearchLineForward:
474+// case StairsState2ndBeforeSpinSearchLineBackward:
475+// case StairsState2ndAfterSpinSearchLineForward:
476+// case StairsState2ndAfterSpinSearchLineBackward:
477+// case StairsStateStepDown:
478+// case StairsStateAfterStepDownSearchLineForward:
479+// case StairsStateAfterStepDownSearchLineBackward:
480+// case StairsState1stSpin:
481+// case StairsState2ndSpin:
482+// case StairsState1stBeforeSpinSearchLineChangeAngle:
483+// case StairsState1stAfterSpinSearchLineChangeAngle:
484+// case StairsState2ndBeforeSpinSearchLineChangeAngle:
485+// case StairsState2ndAfterSpinSearchLineChangeAngle:
486+// case StairsStateAfterStepDownSearchLineChangeAngle:
487+// case StairsStateBeforeStepDownLineTrace:
488+// case StairsStateAfterStepDownLineTrace:
489+// case StairsState2ndStepDetection:
490+// case StairsState1stBeforeSpinAdjustLineForward:
491+// case StairsState1stAfterSpinAdjustLineForward:
492+// case StairsState2ndBeforeSpinAdjustLineForward:
493+// case StairsState2ndAfterSpinAdjustLineForward:
494+// case StairsState1stBeforeSpinAdjustLineBackward:
495+// case StairsState1stAfterSpinAdjustLineBackward:
496+// case StairsState2ndBeforeSpinAdjustLineBackward:
497+// case StairsState2ndAfterSpinAdjustLineBackward:
498+// // 共通クラスのPID制御を実施
499+// turn = PIDMgr->GetTurnValue();
500+// break;
501+//
502+// case StairsState1stStepDetection:
503+// case StairsStateGraySearch:
504+// // 階段クラスのPID制御を実施
505+// turn = GetTurnValue(LIGHT_GRAY);
506+// break;
507+// }
508+ switch (modeUpdatePidValue)
509+ {
510+ case BlackLineTraceMode:
511+ // 黒と白の中間を走るPID制御を実施
512+ PIDMgr->SetColor(LIGHT_BLACK, LIGHT_WHITE);
513+ turn = PIDMgr->GetTurnValue();
514+ break;
515+ case GrayLineTraceMode:
516+ // グレーと白の中間を走るPID制御を実施
517+ PIDMgr->SetColor(LIGHT_GRAY, LIGHT_WHITE);
518+ turn = PIDMgr->GetTurnValue();
519+ break;
520+ }
521+
522+ switch(stairsState)
523+ {
524+// case StairsState1stStepDetection: // 0 1段目 段差検知モード
525+// case StairsState1stBackward: // 1 1段目 段差検知誤バック走行モード
526+// break;
527+
528+ case StairsStateLineEdgeChangeForward1:
529+ case StairsStateLineEdgeChangeForward2:
530+ break;
531+ default:
532+ turn = turn * -1;
533+ break;
534+ }
535+}
536+/**
537+ * @brief 各センサーからの値を取得(オーバーライド)
538+ * @note
539+ * ベース処理呼出 + ログ出力
540+ *
541+ */
542+void Stairs::updateSensorValue()
543+{
544+ BaseCourse::updateSensorValue();
545+ log("%d, %f, %d, %d, %d, %f, %f, %d,\r\n", gyro, gyro_offset, wk_motor_ang_LR, motor_ang_r, motor_ang_l, forward, turn, stairsState);
546+ switch(stairsState)
547+ {
548+ // スピン(360°)モード
549+ case StairsState1stSpin:
550+ case StairsState2ndSpin:
551+ volt = 7500;
552+ break;
553+
554+// case StairsState1stStepUp:
555+// case StairsState2ndStepUp:
556+// volt = volt * 0.8;
557+// break;
558+
559+ default:
560+ break;
561+ }
562+
563+}
564+
565+/**
566+ * @brief turn値の限界値制御(オーバーライド)
567+ * @note
568+ * 階段走行時のモードにturn値を上書きする
569+ *  または限界値を指定する
570+ *
571+ */
572+void Stairs::normalizeTurnValue() {
573+// switch(stairsState)
574+// {
575+// case StairsState1stBackward:
576+// case StairsState1stStepUp:
577+// case StairsState1stAdjustPosition:
578+// case StairsState1stBeforeSpinSearchLineForward:
579+// case StairsState1stBeforeSpinSearchLineBackward:
580+// case StairsState1stAfterSpinSearchLineForward:
581+// case StairsState1stAfterSpinSearchLineBackward:
582+// case StairsState2ndBackward:
583+// case StairsState2ndStepUp:
584+// case StairsState2ndAdjustPosition:
585+// case StairsState2ndBeforeSpinSearchLineForward:
586+// case StairsState2ndBeforeSpinSearchLineBackward:
587+// case StairsState2ndAfterSpinSearchLineForward:
588+// case StairsState2ndAfterSpinSearchLineBackward:
589+// case StairsStateStepDown:
590+// case StairsStateAfterStepDownSearchLineForward:
591+// case StairsStateAfterStepDownSearchLineBackward:
592+// case StairsState1stBeforeSpinAdjustLineBackward:
593+// case StairsState1stAfterSpinAdjustLineBackward:
594+// case StairsState2ndBeforeSpinAdjustLineBackward:
595+// case StairsState2ndAfterSpinAdjustLineBackward:
596+// turn = TURN_NONE;
597+// break;
598+//
599+// case StairsState1stSpin:
600+// case StairsState2ndSpin:
601+// turn = TURN_SPIN;
602+// break;
603+//
604+// case StairsState1stBeforeSpinSearchLineChangeAngle:
605+// case StairsState2ndBeforeSpinSearchLineChangeAngle:
606+// case StairsState1stAfterSpinSearchLineChangeAngle:
607+// case StairsState2ndAfterSpinSearchLineChangeAngle:
608+// case StairsStateAfterStepDownSearchLineChangeAngle:
609+// turn = TURN_CHANGE_ANGLE;
610+// break;
611+//
612+// case StairsState1stStepDetection:
613+// case StairsState2ndStepDetection:
614+// case StairsStateBeforeStepDownLineTrace:
615+// case StairsStateAfterStepDownLineTrace:
616+// case StairsStateGraySearch:
617+// case StairsState1stBeforeSpinAdjustLineForward:
618+// case StairsState1stAfterSpinAdjustLineForward:
619+// case StairsState2ndBeforeSpinAdjustLineForward:
620+// case StairsState2ndAfterSpinAdjustLineForward:
621+// // turn値の限界値制御
622+// if (std::abs(turn) > TURN_MAX)
623+// {
624+// turn = TURN_MAX * (turn > 0) - (turn < 0);
625+// }
626+// break;
627+// }
628+ switch(modeNormalizeTurnValue)
629+ {
630+ case NoneTurnMode:
631+ turn = TURN_NONE;
632+ break;
633+
634+ case SpinTurnMode:
635+ turn = TURN_SPIN;
636+ break;
637+
638+ case ChangeAngleTurnMode:
639+ turn = TURN_CHANGE_ANGLE;
640+ break;
641+
642+ case LineTraceTurnMode:
643+ // turn値の限界値制御
644+ if (std::abs(turn) > TURN_MAX)
645+ {
646+ turn = TURN_MAX * (turn > 0) - (turn < 0);
647+ }
648+ break;
649+
650+ case LineOverMode1:
651+ turn = TURN_LINE_OVER1;
652+ break;
653+ case LineOverMode2:
654+ turn = TURN_LINE_OVER2;
655+ break;
656+ }
657+}
658+
659+/**
660+ * @brief forward値の設定(オーバーライド)
661+ * @note
662+ * 階段走行時のモードにforward値を上書きする
663+ *
664+ */
665+void Stairs::setForward()
666+{
667+ forward = 0;
668+// switch(stairsState)
669+// {
670+// case StairsState1stStepDetection:
671+// case StairsState2ndStepDetection:
672+// forward = FORWARD_STEP_DETECTION;
673+// break;
674+//
675+// case StairsState1stBackward:
676+// case StairsState2ndBackward:
677+// forward = FORWARD_BACKWARD;
678+// break;
679+//
680+// case StairsState1stStepUp:
681+// case StairsState2ndStepUp:
682+// forward = FORWARD_STEP_UP;
683+// break;
684+//
685+// case StairsState1stAdjustPosition:
686+// case StairsState2ndAdjustPosition:
687+// forward = CalcForward((motor_ang_l + motor_ang_r) - wk_motor_ang_LR, MORTOR_ANG_STOP, FORWARD_MAX_FOR_STOP, MORTOR_ANG_FOR_STOP_DIFF_MAX);
688+// break;
689+//
690+// case StairsState1stSpin:
691+// case StairsState2ndSpin:
692+// forward = FORWARD_SPIN;
693+// break;
694+//
695+// case StairsState1stBeforeSpinSearchLineForward:
696+// case StairsState1stAfterSpinSearchLineForward:
697+// case StairsState2ndBeforeSpinSearchLineForward:
698+// case StairsState2ndAfterSpinSearchLineForward:
699+// case StairsStateAfterStepDownSearchLineForward:
700+// forward = FORWARD_SEARCH_LINE_FORWARD;
701+// break;
702+//
703+// case StairsState1stBeforeSpinSearchLineBackward:
704+// case StairsState1stAfterSpinSearchLineBackward:
705+// case StairsState2ndBeforeSpinSearchLineBackward:
706+// case StairsState2ndAfterSpinSearchLineBackward:
707+// case StairsStateAfterStepDownSearchLineBackward:
708+// forward = FORWARD_SEARCH_LINE_BACKWARD;
709+// break;
710+//
711+// case StairsState1stBeforeSpinSearchLineChangeAngle:
712+// case StairsState1stAfterSpinSearchLineChangeAngle:
713+// case StairsState2ndBeforeSpinSearchLineChangeAngle:
714+// case StairsState2ndAfterSpinSearchLineChangeAngle:
715+// case StairsStateAfterStepDownSearchLineChangeAngle:
716+// forward = FORWARD_SEARCH_LINE_CHANGE_ANGLE;
717+// break;
718+//
719+// case StairsStateBeforeStepDownLineTrace:
720+// case StairsStateAfterStepDownLineTrace:
721+// forward = FORWARD_LINE_TRACE;
722+// break;
723+//
724+// case StairsStateStepDown:
725+// forward = FORWARD_STEP_DOWN;
726+// break;
727+//
728+// case StairsStateGraySearch:
729+// forward = FORWARD_GRAY_SEARCH;
730+// break;
731+//
732+// case StairsState1stBeforeSpinAdjustLineForward:
733+// case StairsState1stAfterSpinAdjustLineForward:
734+// case StairsState2ndBeforeSpinAdjustLineForward:
735+// case StairsState2ndAfterSpinAdjustLineForward:
736+// forward = FORWARD_ADJUST_LINE_FORWARD;
737+// break;
738+//
739+// case StairsState1stBeforeSpinAdjustLineBackward:
740+// case StairsState1stAfterSpinAdjustLineBackward:
741+// case StairsState2ndBeforeSpinAdjustLineBackward:
742+// case StairsState2ndAfterSpinAdjustLineBackward:
743+// forward = FORWARD_ADJUST_LINE_BACKWARD;
744+// break;
745+// }
746+ switch(modeSetForward)
747+ {
748+ case StepDetectionMode:
749+ forward = FORWARD_STEP_DETECTION;
750+ break;
751+
752+ case BackwardMode:
753+ forward = FORWARD_BACKWARD;
754+ break;
755+
756+ case StepUpMode:
757+ forward = FORWARD_STEP_UP;
758+ break;
759+
760+ case AdjustPostionMode:
761+ forward = CalcForward((motor_ang_l + motor_ang_r) - wk_motor_ang_LR, MORTOR_ANG_STOP, FORWARD_MAX_FOR_STOP, MORTOR_ANG_FOR_STOP_DIFF_MAX);
762+ break;
763+
764+ case SpinMode:
765+ forward = FORWARD_SPIN;
766+ break;
767+
768+ case SearchLineForwardMode:
769+ forward = FORWARD_SEARCH_LINE_FORWARD;
770+ break;
771+
772+ case SearchLineBackwardMode:
773+ forward = FORWARD_SEARCH_LINE_BACKWARD;
774+ break;
775+
776+ case SearchLineChangeAngleMode:
777+ forward = FORWARD_SEARCH_LINE_CHANGE_ANGLE;
778+ break;
779+
780+ case LineTraceMode:
781+ forward = FORWARD_LINE_TRACE;
782+ break;
783+
784+ case StepDownMode:
785+ forward = FORWARD_STEP_DOWN;
786+ break;
787+
788+ case GraySearchMode:
789+ forward = FORWARD_GRAY_SEARCH;
790+ break;
791+
792+ case AdjustLineForwardMode:
793+ forward = FORWARD_ADJUST_LINE_FORWARD;
794+ break;
795+
796+ case AdjustLineBackwardMode:
797+ forward = FORWARD_ADJUST_LINE_BACKWARD;
798+ break;
799+
800+ case StopMode:
801+ forward = FORWARD_STOP;
802+ }
803+}
804+
805+/**
806+ * @brief 階段走行ステート移行処理
807+ * @note
808+ * 階段走行ステートを次のステートに切り替えて、計算用の変数群を初期化する
809+ *
810+ */
811+void Stairs::changeNextState()
812+{
813+ // ステート移行
814+ changeNextState(0);
815+}
816+
817+/**
818+ * @brief 階段走行モード移行処理
819+ * @note
820+ * 階段走行ステートを次のステートに切り替えて、計算用の変数群を初期化する
821+ * さらに指定のスキップ数分ステートをスキップする
822+ *
823+ */
824+void Stairs::changeNextState(int skip)
825+{
826+ // ステート単位で利用する変数群の初期化
827+ variableInit();
828+
829+ if (stairsState != StairsStateGraySearch)
830+ {
831+ // ステート移行
832+ stairsState = static_cast<StairsStates>(stairsState + 1 + skip);
833+ // モード設定
834+ changeMode();
835+ }
836+}
837+
838+
839+/**
840+ * @brief モード切替処理
841+ * @note
842+ * ステートに合わせて各種モードを設定する
843+ *
844+ */
845+void Stairs::changeMode()
846+{
847+ switch(stairsState)
848+ {
849+ case StairsStateLineEdgeChangeForward0:
850+ modeUpdatePidValue = GrayLineTraceMode;
851+ modeNormalizeTurnValue = LineTraceTurnMode;
852+ modeSetForward = AdjustLineForwardMode;
853+ modeSetPIDData = OtherPIDMode;
854+ break;
855+ case StairsStateLineEdgeChangeForward1:
856+ modeUpdatePidValue = GrayLineTraceMode;
857+ modeNormalizeTurnValue = LineOverMode1;
858+ modeSetForward = BackwardMode;
859+ modeSetPIDData = OtherPIDMode;
860+ break;
861+ case StairsStateLineEdgeChangeForward2:
862+ modeUpdatePidValue = GrayLineTraceMode;
863+ modeNormalizeTurnValue = LineOverMode2;
864+ modeSetForward = BackwardMode;
865+ modeSetPIDData = OtherPIDMode;
866+ break;
867+ case StairsState1stStepDetection: // 0 1段目 段差検知モード
868+ modeUpdatePidValue = GrayLineTraceMode;
869+ modeNormalizeTurnValue = LineTraceTurnMode;
870+ modeSetForward = StepDetectionMode;
871+ modeSetPIDData = OtherPIDMode;
872+ break;
873+ case StairsState1stBackward: // 1 1段目 段差検知誤バック走行モード
874+ modeUpdatePidValue = GrayLineTraceMode;
875+ modeNormalizeTurnValue = NoneTurnMode;
876+ modeSetForward = BackwardMode;
877+ modeSetPIDData = OtherPIDMode;
878+ break;
879+ case StairsState1stStepUp: // 2 1段目 昇段モード
880+ modeUpdatePidValue = GrayLineTraceMode;
881+ modeNormalizeTurnValue = NoneTurnMode;
882+ modeSetForward = StepUpMode;
883+ modeSetPIDData = StepDetectionPIDMode;
884+ break;
885+ case StairsState1stAdjustPosition: // 3 1段目 昇段後位置調整モード
886+ modeUpdatePidValue = BlackLineTraceMode;
887+ modeNormalizeTurnValue = NoneTurnMode;
888+ modeSetForward = AdjustPostionMode;
889+ modeSetPIDData = OtherPIDMode;
890+ break;
891+ case StairsState1stBeforeSpinSearchLineChangeAngle: // 4 1段目 スピン前 ライン探索(進行方向角度変更)モード
892+ modeUpdatePidValue = BlackLineTraceMode;
893+ modeNormalizeTurnValue = ChangeAngleTurnMode;
894+ modeSetForward = SearchLineChangeAngleMode;
895+ modeSetPIDData = OtherPIDMode;
896+ break;
897+ case StairsState1stBeforeSpinSearchLineForward: // 5 1段目 スピン前 ライン探索(前進)モード
898+ modeUpdatePidValue = BlackLineTraceMode;
899+ modeNormalizeTurnValue = NoneTurnMode;
900+ modeSetForward = SearchLineForwardMode;
901+ modeSetPIDData = OtherPIDMode;
902+ break;
903+ case StairsState1stBeforeSpinSearchLineBackward: // 6 1段目 スピン前 ライン探索(後進)モード
904+ modeUpdatePidValue = BlackLineTraceMode;
905+ modeNormalizeTurnValue = NoneTurnMode;
906+ modeSetForward = SearchLineBackwardMode;
907+ modeSetPIDData = OtherPIDMode;
908+ break;
909+ case StairsState1stBeforeSpinAdjustLineForward: // 7 1段目 スピン前 ライン調整(前進)モード
910+ modeUpdatePidValue = BlackLineTraceMode;
911+ modeNormalizeTurnValue = LineTraceTurnMode;
912+ modeSetForward = AdjustLineForwardMode;
913+ modeSetPIDData = OtherPIDMode;
914+ break;
915+ case StairsState1stBeforeSpinAdjustLineBackward: // 8 1段目 スピン前 ライン調整(後進)モード
916+ modeUpdatePidValue = BlackLineTraceMode;
917+ modeNormalizeTurnValue = NoneTurnMode;
918+ modeSetForward = AdjustLineBackwardMode;
919+ modeSetPIDData = OtherPIDMode;
920+ break;
921+ case StairsState1stStop: // 9 1段目 スピン前静止モード
922+ modeUpdatePidValue = BlackLineTraceMode;
923+ modeNormalizeTurnValue = NoneTurnMode;
924+ modeSetForward = StopMode;
925+ modeSetPIDData = OtherPIDMode;
926+ break;
927+
928+ case StairsState1stSpin: // 10 1段目 スピン(360°)モード
929+ modeUpdatePidValue = BlackLineTraceMode;
930+ modeNormalizeTurnValue = SpinTurnMode;
931+ modeSetForward = SpinMode;
932+ modeSetPIDData = OtherPIDMode;
933+ break;
934+ case StairsState1stAfterSpinSearchLineChangeAngle: // 11 1段目 スピン後 ライン探索(進行方向角度変更)モード
935+ modeUpdatePidValue = BlackLineTraceMode;
936+ modeNormalizeTurnValue = ChangeAngleTurnMode;
937+ modeSetForward = SearchLineChangeAngleMode;
938+ modeSetPIDData = OtherPIDMode;
939+ break;
940+ case StairsState1stAfterSpinSearchLineForward: // 12 1段目 スピン後 ライン探索(前進)モード
941+ modeUpdatePidValue = BlackLineTraceMode;
942+ modeNormalizeTurnValue = NoneTurnMode;
943+ modeSetForward = SearchLineForwardMode;
944+ modeSetPIDData = OtherPIDMode;
945+ break;
946+ case StairsState1stAfterSpinSearchLineBackward: // 13 1段目 スピン後 ライン探索(後進)モード
947+ modeUpdatePidValue = BlackLineTraceMode;
948+ modeNormalizeTurnValue = NoneTurnMode;
949+ modeSetForward = SearchLineBackwardMode;
950+ modeSetPIDData = OtherPIDMode;
951+ break;
952+ case StairsState1stAfterSpinAdjustLineForward: // 14 1段目 スピン後 ライン調整(前進)モード
953+ modeUpdatePidValue = BlackLineTraceMode;
954+ modeNormalizeTurnValue = LineTraceTurnMode;
955+ modeSetForward = AdjustLineForwardMode;
956+ modeSetPIDData = OtherPIDMode;
957+ break;
958+// case StairsState1stAfterSpinAdjustLineBackward: // 15 1段目 スピン後 ライン調整(後進)モード
959+// modeUpdatePidValue = BlackLineTraceMode;
960+// modeNormalizeTurnValue = NoneTurnMode;
961+// modeSetForward = AdjustLineBackwardMode;
962+// modeSetPIDData = OtherPIDMode;
963+// break;
964+ case StairsState2ndStepDetection: // 16 2段目 段差検知モード
965+ modeUpdatePidValue = BlackLineTraceMode;
966+ modeNormalizeTurnValue = LineTraceTurnMode;
967+ modeSetForward = StepDetectionMode;
968+ modeSetPIDData = StepDetectionPIDMode;
969+ break;
970+ case StairsState2ndBackward: // 17 2段目 段差検知誤バック走行モード
971+ modeUpdatePidValue = BlackLineTraceMode;
972+ modeNormalizeTurnValue = NoneTurnMode;
973+ modeSetForward = BackwardMode;
974+ modeSetPIDData = OtherPIDMode;
975+ break;
976+ case StairsState2ndStepUp: // 18 2段目 昇段モード
977+ modeUpdatePidValue = BlackLineTraceMode;
978+ modeNormalizeTurnValue = NoneTurnMode;
979+ modeSetForward = StepUpMode;
980+ modeSetPIDData = OtherPIDMode;
981+ break;
982+ case StairsState2ndAdjustPosition: // 19 2段目 昇段後位置調整モード
983+ modeUpdatePidValue = BlackLineTraceMode;
984+ modeNormalizeTurnValue = NoneTurnMode;
985+ modeSetForward = AdjustPostionMode;
986+ modeSetPIDData = OtherPIDMode;
987+ break;
988+ case StairsState2ndBeforeSpinSearchLineChangeAngle: // 20 2段目 スピン前 ライン探索(進行方向角度変更)モード
989+ modeUpdatePidValue = BlackLineTraceMode;
990+ modeNormalizeTurnValue = ChangeAngleTurnMode;
991+ modeSetForward = SearchLineChangeAngleMode;
992+ modeSetPIDData = OtherPIDMode;
993+ break;
994+ case StairsState2ndBeforeSpinSearchLineForward: // 21 2段目 スピン前 ライン探索(前進)モード
995+ modeUpdatePidValue = BlackLineTraceMode;
996+ modeNormalizeTurnValue = NoneTurnMode;
997+ modeSetForward = SearchLineForwardMode;
998+ modeSetPIDData = OtherPIDMode;
999+ break;
1000+ case StairsState2ndBeforeSpinSearchLineBackward: // 22 2段目 スピン前 ライン探索(後進)モード
1001+ modeUpdatePidValue = BlackLineTraceMode;
1002+ modeNormalizeTurnValue = NoneTurnMode;
1003+ modeSetForward = SearchLineBackwardMode;
1004+ modeSetPIDData = OtherPIDMode;
1005+ break;
1006+ case StairsState2ndBeforeSpinAdjustLineForward: // 23 2段目 スピン前 ライン調整(前進)モード
1007+ modeUpdatePidValue = BlackLineTraceMode;
1008+ modeNormalizeTurnValue = LineTraceTurnMode;
1009+ modeSetForward = AdjustLineForwardMode;
1010+ modeSetPIDData = OtherPIDMode;
1011+ break;
1012+ case StairsState2ndBeforeSpinAdjustLineBackward: // 24 2段目 スピン前 ライン調整(後進)モード
1013+ modeUpdatePidValue = BlackLineTraceMode;
1014+ modeNormalizeTurnValue = NoneTurnMode;
1015+ modeSetForward = AdjustLineBackwardMode;
1016+ modeSetPIDData = OtherPIDMode;
1017+ break;
1018+ case StairsState2ndStop: // 25 2段目 スピン前静止モード
1019+ modeUpdatePidValue = BlackLineTraceMode;
1020+ modeNormalizeTurnValue = NoneTurnMode;
1021+ modeSetForward = StopMode;
1022+ modeSetPIDData = OtherPIDMode;
1023+ break;
1024+
1025+ case StairsState2ndSpin: // 26 2段目 スピン(360°)モード
1026+ modeUpdatePidValue = BlackLineTraceMode;
1027+ modeNormalizeTurnValue = SpinTurnMode;
1028+ modeSetForward = SpinMode;
1029+ modeSetPIDData = OtherPIDMode;
1030+ break;
1031+ case StairsState2ndAfterSpinSearchLineChangeAngle: // 27 2段目 スピン後 ライン探索(進行方向角度変更)モード
1032+ modeUpdatePidValue = BlackLineTraceMode;
1033+ modeNormalizeTurnValue = ChangeAngleTurnMode;
1034+ modeSetForward = SearchLineChangeAngleMode;
1035+ modeSetPIDData = OtherPIDMode;
1036+ break;
1037+ case StairsState2ndAfterSpinSearchLineForward: // 28 2段目 スピン後 ライン探索(前進)モード
1038+ modeUpdatePidValue = BlackLineTraceMode;
1039+ modeNormalizeTurnValue = NoneTurnMode;
1040+ modeSetForward = SearchLineForwardMode;
1041+ modeSetPIDData = OtherPIDMode;
1042+ break;
1043+ case StairsState2ndAfterSpinSearchLineBackward: // 29 2段目 スピン後 ライン探索(後進)モード
1044+ modeUpdatePidValue = BlackLineTraceMode;
1045+ modeNormalizeTurnValue = NoneTurnMode;
1046+ modeSetForward = SearchLineBackwardMode;
1047+ modeSetPIDData = OtherPIDMode;
1048+ break;
1049+ case StairsState2ndAfterSpinAdjustLineForward: // 30 2段目 スピン後 ライン調整(前進)モード
1050+ modeUpdatePidValue = BlackLineTraceMode;
1051+ modeNormalizeTurnValue = LineTraceTurnMode;
1052+ modeSetForward = AdjustLineForwardMode;
1053+ modeSetPIDData = OtherPIDMode;
1054+ break;
1055+// case StairsState2ndAfterSpinAdjustLineBackward: // 31 2段目 スピン後 ライン調整(後進)モード
1056+// modeUpdatePidValue = BlackLineTraceMode;
1057+// modeNormalizeTurnValue = NoneTurnMode;
1058+// modeSetForward = AdjustLineBackwardMode;
1059+// modeSetPIDData = OtherPIDMode;
1060+// break;
1061+ case StairsStateBeforeStepDownLineTrace: // 32 降段前ライントレースモード
1062+ modeUpdatePidValue = BlackLineTraceMode;
1063+ modeNormalizeTurnValue = LineTraceTurnMode;
1064+ modeSetForward = LineTraceMode;
1065+ modeSetPIDData = OtherPIDMode;
1066+ break;
1067+ case StairsStateStepDown: // 33 降段モード
1068+ modeUpdatePidValue = BlackLineTraceMode;
1069+ modeNormalizeTurnValue = NoneTurnMode;
1070+ modeSetForward = StepDownMode;
1071+ modeSetPIDData = OtherPIDMode;
1072+ break;
1073+ case StairsStateAfterStepDownSearchLineChangeAngle: // 34 降段後 ライン探索(進行方向角度変更)モード
1074+ modeUpdatePidValue = BlackLineTraceMode;
1075+ modeNormalizeTurnValue = ChangeAngleTurnMode;
1076+ modeSetForward = SearchLineChangeAngleMode;
1077+ modeSetPIDData = OtherPIDMode;
1078+ break;
1079+ case StairsStateAfterStepDownSearchLineForward: // 35 降段後 ライン探索(前進)モード
1080+ modeUpdatePidValue = BlackLineTraceMode;
1081+ modeNormalizeTurnValue = NoneTurnMode;
1082+ modeSetForward = SearchLineForwardMode;
1083+ modeSetPIDData = OtherPIDMode;
1084+ break;
1085+ case StairsStateAfterStepDownSearchLineBackward: // 36 降段後 ライン探索(後進)モード
1086+ modeUpdatePidValue = BlackLineTraceMode;
1087+ modeNormalizeTurnValue = NoneTurnMode;
1088+ modeSetForward = SearchLineBackwardMode;
1089+ modeSetPIDData = OtherPIDMode;
1090+ break;
1091+ case StairsStateAfterStepDownLineTrace: // 37 降段後ライントレースモード
1092+ modeUpdatePidValue = BlackLineTraceMode;
1093+ modeNormalizeTurnValue = LineTraceTurnMode;
1094+ modeSetForward = LineTraceMode;
1095+ modeSetPIDData = OtherPIDMode;
1096+ break;
1097+ case StairsStateGraySearch: // 38 グレーライン検知モード
1098+ modeUpdatePidValue = GrayLineTraceMode;
1099+ modeNormalizeTurnValue = LineTraceTurnMode;
1100+ modeSetForward = GraySearchMode;
1101+ modeSetPIDData = OtherPIDMode;
1102+ break;
1103+ }
1104+}
1105+
1106+/**
1107+ * @brief 変数初期化処理
1108+ * @note
1109+ * 変数群を初期化する
1110+ *
1111+ */
1112+void Stairs::variableInit()
1113+{
1114+ // 変数群の初期化
1115+ wk_motor_ang_LR = motor_ang_l + motor_ang_r;
1116+ wk_motor_ang_l = motor_ang_l;
1117+ wk_motor_ang_r = motor_ang_r;
1118+ counter = 0;
1119+ m_colorCensorList.clear();
1120+ m_motorAngleList.clear();
1121+ gyro_offset = NORMAL_GYRO_OFFSET;
1122+}
1123+
1124+/**
1125+ * @brief ライン判定
1126+ * @note
1127+ * 指定回数連続して指定色より大きいカラーセンサー値が検出された場合にTrueを返却する
1128+ */
1129+bool Stairs::judgeColor(unsigned int listSize, int color)
1130+{ // カラー値の履歴管理
1131+ if ((unsigned int)m_colorCensorList.size() >= listSize)
1132+ {
1133+ m_colorCensorList.erase(m_colorCensorList.begin());
1134+ }
1135+ m_colorCensorList.push_back(colorSensor);
1136+
1137+ bool find = false;
1138+ if (m_colorCensorList.size() >= listSize)
1139+ {
1140+ find = true;
1141+ for (unsigned int i = 0; i <= m_colorCensorList.size() - 1; i++ )
1142+ {
1143+ if (m_colorCensorList[i] > color)
1144+ {
1145+ find = false;
1146+ break;
1147+ }
1148+ }
1149+ }
1150+ return find;
1151+}
1152+
1153+/**
1154+ * @brief 指定距離地点停滞チェック
1155+ * @note
1156+ * 指定距離位置周辺に指定回数連続して停滞しているかどうかを判定する
1157+ */
1158+bool Stairs::judgeMotorAngle( int currentAngle, unsigned int listSize, int targetAngle, int range)
1159+{
1160+// log("judgeMotorAngle %d, %d, %d, %d \r\n", currentAngle, listSize, targetAngle, range);
1161+
1162+ // 回転角度の履歴管理
1163+ if ((unsigned int)m_motorAngleList.size() >= listSize)
1164+ {
1165+ m_motorAngleList.erase(m_motorAngleList.begin());
1166+ }
1167+ m_motorAngleList.push_back(currentAngle);
1168+
1169+ bool find = false;
1170+ if (m_motorAngleList.size() >= listSize)
1171+ {
1172+ find = true;
1173+ for (unsigned int i = 0; i <= m_motorAngleList.size() - 1; i++ )
1174+ {
1175+ if ((m_motorAngleList[i] >= targetAngle + range) || (m_motorAngleList[i] <= targetAngle - range))
1176+ {
1177+ find = false;
1178+ break;
1179+ }
1180+ }
1181+ }
1182+// log("judgeMotorAngle ret %d \r\n", find);
1183+ return find;
1184+}
1185+
1186+/**
1187+ * @brief コースモード移行判定(オーバーライド)
1188+ * @note
1189+ * 自コースの走行クラスにモードを移行していいかを判定
1190+ *
1191+ */
1192+bool Stairs::IsNextCourse()
1193+{
1194+ bool result = false;
1195+
1196+ // グレーサーチモードで、
1197+ if (stairsState == StairsStateGraySearch)
1198+ {
1199+// // さらにグレーマーカー⇒黒ラインを感知したら次コース処理に遷移
1200+// if(grayChecker->IsGrayLineEnd(colorSensor))
1201+// {
1202+ result = true;
1203+// }
1204+ }
1205+ return result;
1206+}
1207+//
1208+///**
1209+// * @brief * @note
1210+// * PID制御を実行し、計算したTurn値を返す
1211+// * 時間がないのでPIDDataManagerかたまるっとコピー。
1212+// *  オリジナルと違うのは閾値計算に引数を利用しているところのみ
1213+// *
1214+// */
1215+//float Stairs::GetTurnValue(int LineColor)
1216+//{
1217+// // PID制御
1218+// int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
1219+// float hensa = colorSensor - (LIGHT_WHITE + 0 + LineColor)/2;
1220+// float hensaTotal = std::accumulate(m_hensaList.begin(), m_hensaList.end(), 0.001) + hensa;
1221+// float integral = (hensaTotal) / 2 * PROCESS_PERIOD;
1222+// float wkP = hensa * m_pFixVal;
1223+// float wkI = integral * m_iFixVal;
1224+// float wkD = (hensa - m_hensaList.back()) / PROCESS_PERIOD * m_dFixVal ;
1225+// float turn = wkP + wkI + wkD;
1226+//
1227+// // 偏差値の履歴(配列)作成
1228+// // 履歴数が履歴の最大数を超えている場合、最古の偏差値を削除
1229+// if ((int)m_hensaList.size() >= HISTORY_MAX)
1230+// {
1231+// m_hensaList.erase(m_hensaList.begin());
1232+// }
1233+//
1234+// // 履歴へ現在の偏差値を追加
1235+// m_hensaList.push_back(hensa);
1236+//
1237+// turn = turn * -1;
1238+//
1239+// return turn;
1240+//}
1241+
1242+
1243+/**
1244+ * @brief * @note
1245+ * 現在の両輪モーター回転角度と、目標位置の両輪モーター回転角度の差分から
1246+ * 最大Forwad値と最大差分モーター回転角度をにより、適切なForward値を算出する
1247+ */
1248+float Stairs::CalcForward(int currentMotorAng, int targetMortorAng, int maxForward, int maxDiffMortorAng) {
1249+
1250+// log("CalcForward %d, %d, %d, %d \r\n", currentMotorAng, targetMortorAng, maxForward, maxDiffMortorAng);
1251+ float ret = 0;
1252+ // 係数の算出 係数=最大差分(モーター角度)/最大FORWARD
1253+ float coefficient = (float)maxDiffMortorAng / (float)maxForward;
1254+// log("CalcForward coefficient %f \r\n", coefficient);
1255+
1256+ // 戻り値 = (現在の両輪モーター回転角度 - 目標位置の両輪モーター回転角度) * 係数
1257+ ret = (float)(targetMortorAng - currentMotorAng) * coefficient;
1258+// log("CalcForward ret %f \r\n", ret);
1259+
1260+ // 戻り値が最大Forward値を超えていたら最大Forward値に設定
1261+ if (std::abs(ret) > maxForward)
1262+ {
1263+ // maxForward * retの符号
1264+ ret = maxForward * ((ret > 0) - (ret < 0));
1265+ }
1266+// log("CalcForward ret %f \r\n", ret);
1267+ return ret;
1268+}
1269+
1270+void Stairs::setMotorPower() {
1271+ // モーター出力の設定
1272+ log("setMotorPower a %d, %d, %d, %d \r\n", (int)motor_ang_l, (int)wk_motor_ang_l, (int)(motor_ang_r), (int)(wk_motor_ang_r));
1273+ log("setMotorPower b %d, %d, %d, %d \r\n", volt, (int)pwm_L, (int)(pwm_R), colorSensor);
1274+
1275+ BaseCourse::setMotorPower();
1276+
1277+}
1278+
1279+
1280+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalR.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/StartToGoalR.cpp (revision 410)
@@ -0,0 +1,127 @@
1+#include "StartToGoalR.h"
2+
3+StartToGoalR::StartToGoalR() {
4+ mode = Straight;
5+}
6+
7+StartToGoalR::~StartToGoalR() {
8+}
9+
10+/*次モードへの移行
11+ 左右モータの総回転角からモードを走行モードを切り替える*/
12+bool StartToGoalR::IsNextCourse()
13+{
14+ // 左右モータ総合回転角平均
15+ double totalAvg = (motor_ang_r + WK + motor_ang_l + WK) / 2;
16+// forward = 30;
17+
18+ if(totalAvg < 40) {
19+ gyro_offset = -5;
20+ }
21+ else {
22+ gyro_offset = 0;
23+ }
24+
25+ if(totalAvg < 2100) {
26+ //スタートからA地点手前まで ストレート
27+ mode = FirstStraight;
28+ }
29+ else if(totalAvg < 2400) {
30+ // スピードダウン
31+ mode = SpeedDown;
32+ // 次モードのforward値
33+ minSpeed = 90;
34+ }
35+ else if(totalAvg < 4100) {
36+ //A地点からB地点まで カーブ
37+ mode = FirstCurve;
38+ }
39+ else if(totalAvg < 4800) {
40+ //B地点からC地点まで ストレート
41+ mode = Straight2;
42+ }
43+ else if(totalAvg < 6100) {
44+ //C地点からD地点まで カーブ
45+ mode = SecondCurve;
46+ }
47+ else if(totalAvg < 7000) {
48+ //D地点からE地点まで ストレート
49+ mode = Straight2;
50+ }
51+ else if(totalAvg < 8300) {
52+ //E地点からF地点まで カーブ
53+ mode = ThirdCurve;
54+ }
55+ else if(totalAvg < 10700) {
56+ //F地点からG地点手前まで ストレート
57+ mode = Straight;
58+ }
59+ else if(totalAvg < 11000) {
60+ // スピードダウン
61+ mode = SpeedDown;
62+ // 次モードのforward値
63+ minSpeed = 90;
64+ }
65+ else if(totalAvg < 12000) {
66+ //G地点からH地点まで カーブ
67+ mode = SecondCurve;
68+ }
69+
70+ else {
71+ // ゴール通過後 次の走行クラス(難所)へ切り替え
72+ return true;
73+ }
74+
75+ // ログ出力 コメントアウト
76+ int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
77+ int touchSensor = ev3_touch_sensor_is_pressed(EV3_PORT_1);
78+ //if(touchSensor == 1){
79+ log("%f, %d, %f, %d, %d, %d\r\n", totalAvg, mode, forward, colorSensor, volt, touchSensor);
80+ //}
81+ return false;
82+}
83+
84+/*PID制御処理呼び出し
85+ modeによる適正値を設定し、PID制御呼び出し*/
86+void StartToGoalR::SetPIDData() {
87+ if(mode == Straight || mode == SpeedDown || mode == FirstStraight || mode == Straight2) {
88+ PIDMgr->SetFixedData(0.5, 0, 0.03);
89+ }
90+ else if(mode == FirstCurve){
91+ //PIDMgr->SetFixedData(1.3, 1, 0.05);
92+ PIDMgr->SetFixedData(1.0, 0.5, 0.05);
93+ }
94+ else if(mode == SecondCurve){
95+ PIDMgr->SetFixedData(1.2, 0.6, 0.05);
96+ }
97+ else if(mode == ThirdCurve){
98+ PIDMgr->SetFixedData(0.9, 0.5, 0.07);
99+ }
100+}
101+
102+/*forward値設定
103+ モードからforward値を設定*/
104+void StartToGoalR::setForward() {
105+ if (mode == FirstStraight) {
106+ SpeedUp(FIRST_STRAIGHT_SPEED);
107+ }
108+ else if(mode == Straight) {
109+ SpeedUp(STRAIGHT_SPEED);
110+ }
111+ else if(mode == Straight2) {
112+ forward = 120;
113+ }
114+ else if(mode == SpeedDown) {
115+ BaseCourse::SpeedDown(minSpeed);
116+ }
117+ else if(mode == FirstCurve) {
118+ forward = 125;//110
119+ }
120+ else if(mode == SecondCurve) {
121+ forward = 110;//90
122+ }
123+ else if(mode == ThirdCurve) {
124+ forward = 120;
125+ }
126+}
127+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Garage.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/Garage.cpp (revision 410)
@@ -0,0 +1,189 @@
1+#include "Garage.h"
2+
3+#include "CommonApp.h"
4+
5+const int GarageTargetDistance = 250; // 525度で35cm のはず タイヤが大きくなったので変更
6+//const int GarageTargetDistance = 555; // 35cm + 2cm
7+
8+Garage::Garage(int courseType)
9+ : garageState(GarageStateRun)
10+ , m_isFinished(false)
11+ , waitCount(0)
12+ , targetDistance(GarageTargetDistance)
13+{
14+ p_courseType = courseType;
15+}
16+
17+Garage::~Garage()
18+{
19+}
20+
21+void Garage::SetPIDData()
22+{
23+ // PID制御に使用する定数を設定する
24+ wkP = 0.7;
25+ wkI = 1;
26+ wkD = 0.03;
27+
28+ PIDMgr->SetFixedData(wkP, wkI, wkD);
29+
30+ AngleCheck();
31+}
32+
33+void Garage::Run()
34+{
35+ switch(garageState)
36+ {
37+ case GarageStateRun:
38+ log("state:run ");
39+ MoveDistance();
40+ break;
41+ case GarageStateStop:
42+ log("state:stop ");
43+ Stop();
44+ break;
45+ case GarageStateAdjustment:
46+ log("state:adjust ");
47+ Adjustment();
48+ break;
49+ case GarageStateFinish:
50+ if (!m_isFinished) {
51+ m_isFinished = true;
52+ log("state:finish ");
53+ }
54+ Finish();
55+ break;
56+ case GarageStateWait:
57+ log("state:wait ");
58+ ev3_motor_stop(right_motor, true);
59+ ev3_motor_stop(left_motor, true);
60+ if(1400 < ++waitCount) {
61+ waitCount = 0;
62+ targetDistance = GetDistance() + 45;
63+ garageState = GarageStateAdjustment;
64+ }
65+ break;
66+ case GarageStateMoveDistance:
67+ BaseCourse::Run();
68+ log("state:Run ");
69+ // 少し倒立振子走行してから尻尾を下す
70+ if(300 < motor_ang_l){
71+ targetDistance += 300;
72+ garageState = GarageStateRun;
73+ }
74+ break;
75+ }
76+
77+ if ( !m_isFinished )
78+ log("\r\n");
79+// log("%d,%f,%f,%f,%f,%d,%d\r\n", colorSensor, forward, turn,
80+// motor_ang_l, motor_ang_r, (int)pwm_L, (int)pwm_R);
81+}
82+
83+void Garage::setForward()
84+{
85+ switch(garageState)
86+ {
87+ case GarageStateRun:
88+ forward = 18.0;
89+ break;
90+ case GarageStateStop:
91+ forward = 0.0F;
92+ break;
93+ case GarageStateAdjustment:
94+ forward = 0.0F;
95+ break;
96+ case GarageStateFinish:
97+ forward = 0.0F;
98+ break;
99+ case GarageStateWait:
100+ forward = 0.0F;
101+ break;
102+ case GarageStateMoveDistance:
103+ forward = 30.0F;
104+ break;
105+ }
106+}
107+
108+void Garage::MoveDistance()
109+{
110+ BaseCourse::Run();
111+ if(200 < motor_ang_l)
112+ {
113+ garageState = GarageStateStop;
114+ }
115+}
116+
117+void Garage::Stop()
118+{
119+ if(stopper.DoStop())
120+ {
121+ garageState = GarageStateAdjustment;
122+ }
123+}
124+
125+void Garage::Adjustment()
126+{
127+// const int LightWhite = 14;
128+// const int LightBlack = 9;
129+ const int LightWhite = 10;
130+ const int LightBlack = 0;
131+ const int Offset = 1;
132+ const int targetColor = (LightWhite + LightBlack) / 2 - Offset;
133+ const int defaultPower = 5;
134+ const int additionalPower = 5;
135+
136+ int colorDiff = targetColor - ev3_color_sensor_get_reflect(color_sensor);
137+// int powerL = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
138+// int powerR = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
139+
140+
141+ int powerR = 0;
142+ int powerL = 0;
143+
144+ if (p_courseType == COURSETYPE_LEFT) {
145+ powerR = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
146+ powerL = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
147+ } else
148+ {
149+ powerR = defaultPower + ((colorDiff < 0) ? additionalPower : 0);
150+ powerL = defaultPower + ((0 < colorDiff) ? additionalPower : 0);
151+ }
152+
153+
154+ ev3_motor_set_power(left_motor, powerL);
155+ ev3_motor_set_power(right_motor, powerR);
156+
157+ if(targetDistance < GetDistance())
158+ {
159+ garageState = GarageStateWait;
160+ }
161+
162+ log(" tmpTurn:%d pwrL:%d pwrR:%d", colorDiff, powerL, powerR);
163+}
164+
165+void Garage::Finish()
166+{
167+ ev3_motor_stop(right_motor, true);
168+ ev3_motor_stop(left_motor, true);
169+}
170+
171+bool Garage::AngleCheck()
172+{
173+ if ( 40 < ev3_motor_get_counts(tail_motor) ) {
174+ garageState = GarageStateAdjustment;
175+ }
176+
177+ return true;
178+}
179+
180+double Garage::GetDistance()
181+{
182+ int32_t distanceL = ev3_motor_get_counts(left_motor) - startMotorAngL;
183+ int32_t distanceR = ev3_motor_get_counts(right_motor) - startMotorAngR;
184+ double distance = (static_cast<double>(distanceL + distanceR) / 2);
185+
186+ log("distance:%.2f", distance);
187+
188+ return distance;
189+}
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/GrayChecker.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/course/GrayChecker.cpp (revision 410)
@@ -0,0 +1,111 @@
1+/*
2+ * File: GrayChecker.cpp
3+ * Author: ncxx-sl
4+ *
5+ * Created on 2016/08/21, 15:04
6+ */
7+
8+#include "GrayChecker.h"
9+#include "../BaseCourse.h"
10+
11+#include <algorithm>
12+
13+#define COLOR_HISTORY_MAX 125
14+
15+GrayChecker::GrayChecker()
16+ : m_state(BeforeOnGray)
17+ , m_beforeAngL(0)
18+ , m_beforeAngR(0)
19+{
20+}
21+
22+GrayChecker::~GrayChecker() {
23+}
24+
25+bool GrayChecker::IsGrayLineEnd(int color)
26+{
27+ const int LIGHT_GRAY_TRACE = 26;
28+
29+ if ((int)m_colorSensorList.size() >= COLOR_HISTORY_MAX)
30+ {
31+ m_colorSensorList.erase(m_colorSensorList.begin());
32+ }
33+
34+ // 履歴へ現在のカラーセンサー値を追加
35+ m_colorSensorList.push_back(color);
36+
37+ switch(m_state)
38+ {
39+ case BeforeOnGray:
40+ BaseCourse::log("BeforeOnGray\r\n");
41+ //灰色検知
42+ for(std::vector<int>::const_iterator it = m_colorSensorList.begin(); it != m_colorSensorList.end(); ++it)
43+ {
44+ if(LIGHT_GRAY_TRACE < *it)
45+ {
46+ return false;
47+ }
48+ }
49+ m_state = OnGray;
50+ break;
51+ case OnGray:
52+ BaseCourse::log("OnGray\r\n");
53+ if(color <= LIGHT_GRAY_TRACE)
54+ {
55+ m_state = AfterGray;
56+ return true;
57+ }
58+ break;
59+ case AfterGray:
60+ static bool isFinished = false;
61+ if(!isFinished){
62+ BaseCourse::log("OnBlack\r\n");
63+ isFinished = true;
64+ }
65+ return true;
66+ break;
67+ }
68+ return false;
69+}
70+
71+bool GrayChecker::IsCurving(int motorAngL, int motorAngR)
72+{
73+ const int CurveCheckCount = 125;
74+ const int CurvingAngDiff = 50;
75+
76+ if(m_beforeAngL == 0 && m_beforeAngR == 0)
77+ {
78+ m_beforeAngL = motorAngL;
79+ m_beforeAngR = motorAngR;
80+
81+ return false;
82+ }
83+
84+ int currentAngDiff = (motorAngL - m_beforeAngL) - (motorAngR - m_beforeAngR);
85+ m_motorAngDiffList.push_back(currentAngDiff);
86+ m_beforeAngL = motorAngL;
87+ m_beforeAngR = motorAngR;
88+
89+ if(m_motorAngDiffList.size() < CurveCheckCount)
90+ {
91+ return false;
92+ }
93+ else if (CurveCheckCount < m_motorAngDiffList.size())
94+ {
95+ m_motorAngDiffList.erase(m_motorAngDiffList.begin());
96+ }
97+
98+ int totalAngDiff = std::accumulate(m_motorAngDiffList.begin(), m_motorAngDiffList.end(), 0);
99+
100+ if(CurvingAngDiff < abs(totalAngDiff))
101+ {
102+ BaseCourse::log("curve! current:%d, total:%d\r\n", currentAngDiff, totalAngDiff);
103+ return true;
104+ }
105+ else
106+ {
107+ BaseCourse::log("not curve! current%d, total:%d\r\n", currentAngDiff, totalAngDiff);
108+ return false;
109+ }
110+}
111+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/BaseCourse.h (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/BaseCourse.h (revision 410)
@@ -0,0 +1,74 @@
1+/*
2+ * File: BaseCourse.h
3+ * Author: k.kinoshita
4+ *
5+ * Created on 2016/07/23, 10:39
6+ */
7+
8+#ifndef BASECOURSE_H
9+#define BASECOURSE_H
10+
11+#include "ev3api.h"
12+#include "PIDDataManager.h"
13+
14+class BaseCourse {
15+public:
16+ BaseCourse();
17+ BaseCourse(const BaseCourse& orig);
18+
19+ virtual ~BaseCourse();
20+
21+ virtual void SetPIDData();
22+ virtual void Run();
23+ virtual bool IsNextCourse();
24+
25+ virtual void GetafterRecord(char *log);
26+ virtual void setForward();
27+ virtual void updatePidValue();
28+ virtual void updateSensorValue();
29+ virtual void calcMotorPower();
30+ virtual void setMotorPower();
31+ virtual bool continueRunning();
32+ virtual void normalizeTurnValue();
33+ virtual void SpeedUp(float maxSpeed);
34+ virtual void SpeedDown(float minSpeed);
35+
36+ static void log(const char* format, ...);
37+
38+ void SetStartMotorAngle();
39+
40+protected:
41+
42+ float forward = 0.0F; /* 前後進命令 */
43+ float turn = 0.0F; /* 旋回命令 */
44+ int colorSensor = 0;
45+ //unsigned int distance = 0;
46+ int32_t motor_ang_l = 0;
47+ int32_t motor_ang_r = 0;
48+ int gyro = 0;
49+ float gyro_offset = 0;
50+ int volt = 0;
51+ signed char pwm_R = 0;
52+ signed char pwm_L = 0;
53+ // 転倒判定カウント
54+ int fallingCnt = 0;
55+
56+ unsigned long loopCnt = 0;
57+ int tmpR = 0;
58+ int tmpL = 0;
59+ double wkP = 0.0;
60+ double wkI = 0.0;
61+ double wkD = 0.0;
62+ float const FIRST_STRAIGHT_SPEED = 170;
63+ float const STRAIGHT_SPEED = 170;
64+ float minSpeed = 0;
65+ float const DEFAULT_FORWARD = 50;
66+
67+ PIDDataManager *PIDMgr;
68+
69+ int32_t startMotorAngR = 0.0;
70+ int32_t startMotorAngL = 0.0;
71+};
72+
73+#endif /* BASECOURSE_H */
74+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/app.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/app.cpp (revision 410)
@@ -0,0 +1,60 @@
1+/**
2+ ******************************************************************************
3+ ** ファイル名 : app.cpp
4+ **
5+ ** 概要 : 2輪倒立振子ライントレースロボットのTOPPERS/HRP2用Cサンプルプログラム
6+ **
7+ ** 注記 : sample_c4 (sample_c3にBluetooth通信リモートスタート機能を追加)
8+ ******************************************************************************
9+ **/
10+
11+#include <vector>
12+
13+#include "ev3api.h"
14+#include "app.h"
15+#include "CommonApp.h"
16+
17+
18+static CommonApp *app = NULL;
19+
20+/* メインタスク */
21+void main_task(intptr_t unused)
22+{
23+ try
24+ {
25+ app = CommonApp::createApp();
26+
27+ // 初期化処理
28+ app->init();
29+
30+ /**
31+ * Main loop for the self-balance control algorithm
32+ */
33+ app->mainLoop();
34+
35+ // ループ後 終了処理
36+ app->end();
37+
38+ } catch(...) { }
39+
40+ if (app != NULL) {
41+ delete app;
42+ app = NULL;
43+ }
44+
45+ ext_tsk();
46+}
47+
48+
49+//*****************************************************************************
50+// 関数名 : bt_task
51+// 引数 : unused
52+// 返り値 : なし
53+// 概要 : Bluetooth通信によるリモートスタート。 Tera Termなどのターミナルソフトから、
54+// ASCIIコードで1を送信すると、リモートスタートする。
55+//*****************************************************************************
56+void bt_task(intptr_t unused)
57+{
58+ app->do_bt_task();
59+}
60+
--- trunk/src/hrp2_beta7/sdk/workspace/calibration2/PIDDataManager.cpp (revision 0)
+++ trunk/src/hrp2_beta7/sdk/workspace/calibration2/PIDDataManager.cpp (revision 410)
@@ -0,0 +1,71 @@
1+/*
2+ * File: PIDDataManager.cpp
3+ * Author: k.kinoshita
4+ *
5+ * Created on 2016/07/16, 10:46
6+ */
7+
8+#include "PIDDataManager.h"
9+#include "CommonApp.h"
10+
11+#include <vector>
12+#include <algorithm>
13+
14+PIDDataManager::PIDDataManager()
15+{
16+ m_pFixVal = 0;
17+ m_iFixVal = 0;
18+ m_dFixVal = 0;
19+ m_hensaList.clear();
20+ m_hensaList.push_back(0.0f);
21+
22+ lightBlack = LIGHT_BLACK;
23+ lightWhite = LIGHT_WHITE;
24+}
25+
26+PIDDataManager::PIDDataManager(const PIDDataManager& orig) {
27+}
28+
29+PIDDataManager::~PIDDataManager() {
30+}
31+
32+void PIDDataManager::SetFixedData(float pVal, float iVal, float dVal)
33+{
34+ m_pFixVal = pVal;
35+ m_iFixVal = iVal;
36+ m_dFixVal = dVal;
37+}
38+
39+// PID制御を実行し、計算したTurn値を返す
40+float PIDDataManager::GetTurnValue()
41+{
42+ // PID制御
43+ int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
44+ float hensa = colorSensor - (lightWhite + 0 + lightBlack)/2;
45+ float hensaTotal = std::accumulate(m_hensaList.begin(), m_hensaList.end(), 0.001) + hensa;
46+ float integral = (hensaTotal) / 2 * PROCESS_PERIOD;
47+ float wkP = hensa * m_pFixVal;
48+ float wkI = integral * m_iFixVal;
49+ float wkD = (hensa - m_hensaList.back()) / PROCESS_PERIOD * m_dFixVal ;
50+ float turn = wkP + wkI + wkD;
51+
52+ // 偏差値の履歴(配列)作成
53+ // 履歴数が履歴の最大数を超えている場合、最古の偏差値を削除
54+ if ((int)m_hensaList.size() >= HISTORY_MAX)
55+ {
56+ m_hensaList.erase(m_hensaList.begin());
57+ }
58+
59+ // 履歴へ現在の偏差値を追加
60+ m_hensaList.push_back(hensa);
61+
62+ //turn = turn * -1;
63+
64+ return turn;
65+}
66+
67+
68+
69+
70+
71+
Show on old repository browser