• R/O
  • SSH
  • HTTPS

team-ncxx-sl: Commit


Commit MetaInfo

Revision409 (tree)
Time2018-09-15 16:05:41
Authorp12010043

Log Message

(empty log message)

Change Summary

Incremental Difference

--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/BaseCourse.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/BaseCourse.cpp (revision 409)
@@ -0,0 +1,183 @@
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+ volt = ev3_battery_voltage_mV();
93+}
94+
95+void BaseCourse::calcMotorPower() {
96+ /* 倒立振子制御APIを呼び出し、倒立走行するための */
97+ /* 左右モータ出力値を得る */
98+ balance_control(
99+ (float)forward,
100+ (float)turn,
101+ (float)gyro,
102+ (float)gyro_offset,
103+ (float)motor_ang_l,
104+ (float)motor_ang_r,
105+ (float)volt,
106+ (signed char*)&pwm_L,
107+ (signed char*)&pwm_R);
108+}
109+
110+void BaseCourse::setMotorPower() {
111+ // モーター出力の設定
112+
113+ /* EV3ではモーター停止時のブレーキ設定が事前にできないため */
114+ /* 出力0時に、その都度設定する */
115+ if (pwm_L == 0)
116+ {
117+ ev3_motor_stop(left_motor, true);
118+ }
119+ else
120+ {
121+ ev3_motor_set_power(left_motor, (int)pwm_L);
122+ }
123+
124+ if (pwm_R == 0)
125+ {
126+ ev3_motor_stop(right_motor, true);
127+ }
128+ else
129+ {
130+ ev3_motor_set_power(right_motor, (int)pwm_R);
131+ }
132+}
133+
134+bool BaseCourse::continueRunning() {
135+ // 走り続けていいかを判定
136+
137+ bool val = true;
138+ // 左モータPMW出力が常にMAX状態の場合に転倒判定カウントを開始
139+ if ((int)pwm_L == 100 || (int)pwm_L == -100)
140+ {
141+ // 転倒判定カウントが500回(約2秒間)を越えると強制停止
142+ if (fallingCnt >= 250)
143+ {
144+ val = false;
145+ }
146+ fallingCnt++;
147+ }
148+ else
149+ {
150+ // 転倒判定カウント
151+ fallingCnt = 0;
152+ }
153+ return val;
154+}
155+
156+void BaseCourse::normalizeTurnValue() {
157+ // turn値の限界値制御
158+ if (turn > 100)
159+ {
160+ turn = 100;
161+ }
162+ else if (turn < -100)
163+ {
164+ turn = -100;
165+ }
166+}
167+
168+void BaseCourse::log(const char* format, ...) {
169+ // ログ出力
170+ static FILE *bt = ev3_serial_open_file(EV3_SERIAL_BT);
171+ assert(bt != NULL);
172+ va_list vaList;
173+ va_start(vaList, format);
174+ vfprintf(bt, format, vaList);
175+ va_end(vaList);
176+}
177+
178+void BaseCourse::SetStartMotorAngle()
179+{
180+ startMotorAngR = motor_ang_r;
181+ startMotorAngL = motor_ang_l;
182+}
183+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/BaseCourse.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/BaseCourse.h (revision 409)
@@ -0,0 +1,70 @@
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+
34+ virtual void log(const char* format, ...);
35+
36+ void SetStartMotorAngle();
37+
38+protected:
39+
40+ float forward = 0.0F; /* 前後進命令 */
41+ float turn = 0.0F; /* 旋回命令 */
42+ int colorSensor = 0;
43+ //unsigned int distance = 0;
44+ int32_t motor_ang_l = 0;
45+ int32_t motor_ang_r = 0;
46+ int gyro = 0;
47+ int gyro_offset = 0;
48+ int volt = 0;
49+ signed char pwm_R = 0;
50+ signed char pwm_L = 0;
51+ // 転倒判定カウント
52+ int fallingCnt = 0;
53+
54+ unsigned long loopCnt = 0;
55+ int tmpR = 0;
56+ int tmpL = 0;
57+ double wkP = 0.0;
58+ double wkI = 0.0;
59+ double wkD = 0.0;
60+
61+
62+ PIDDataManager *PIDMgr;
63+
64+ int32_t startMotorAngR = 0.0;
65+ int32_t startMotorAngL = 0.0;
66+
67+};
68+
69+#endif /* BASECOURSE_H */
70+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/CommonApp.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/CommonApp.cpp (revision 409)
@@ -0,0 +1,358 @@
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_LEFT);
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+ waitStart();
45+
46+ /* 走行モーターエンコーダーリセット */
47+ resetMotor();
48+
49+ /* ジャイロセンサーリセット */
50+ resetGyro();
51+
52+ /* スタート通知 */
53+ startComplete();
54+}
55+
56+void CommonApp::initLcd() {
57+ /* LCD画面表示 */
58+ ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
59+ ev3_lcd_draw_string("EV3way-ET sample_c4", 0, CALIB_FONT_HEIGHT*1);
60+}
61+
62+void CommonApp::initSenserProt() {
63+ /* センサー入力ポートの設定 */
64+ ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
65+ ev3_sensor_config(color_sensor, COLOR_SENSOR);
66+ ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
67+ ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
68+ ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
69+}
70+
71+void CommonApp::initMotorPort() {
72+ /* モーター出力ポートの設定 */
73+ ev3_motor_config(left_motor, LARGE_MOTOR);
74+ ev3_motor_config(right_motor, LARGE_MOTOR);
75+ ev3_motor_config(tail_motor, LARGE_MOTOR);
76+ ev3_motor_reset_counts(tail_motor);
77+}
78+
79+void CommonApp::openBluetooth() {
80+ /* Open Bluetooth file */
81+ bt = ev3_serial_open_file(EV3_SERIAL_BT);
82+ assert(bt != NULL);
83+}
84+
85+void CommonApp::startBluetooth() {
86+ /* Bluetooth通信タスクの起動 */
87+ act_tsk(BT_TASK);
88+}
89+
90+void CommonApp::initComplete() {
91+ /* 初期化完了通知 */
92+ ev3_led_set_color(LED_ORANGE);
93+}
94+
95+void CommonApp::waitStart() {
96+
97+ ev3_sensor_config(color_sensor, COLOR_SENSOR);
98+ ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
99+ ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
100+
101+
102+ /* スタート待機 */
103+ while(1)
104+ {
105+ /*
106+ uint8_t colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
107+ char buf[32];
108+ sprintf(buf, "%d", colorSensor);
109+ */
110+ ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
111+ //ev3_lcd_draw_string(buf, 0, CALIB_FONT_HEIGHT*1);
112+
113+ //タッチセンサー値画面出力
114+ bool_t getTouchSensor = ev3_touch_sensor_is_pressed(EV3_PORT_1);
115+ char touch_Sensor[32];
116+ sprintf(touch_Sensor, "Touch: %d", getTouchSensor);
117+ ev3_lcd_draw_string(touch_Sensor, 20, 60);
118+
119+ //超音波センサー値画面出力
120+ int16_t getUltrasonicSensor = ev3_ultrasonic_sensor_get_distance(EV3_PORT_2);
121+ char ultrasonic_sensor[32];
122+ sprintf(ultrasonic_sensor, "Ultrasonic: %d", getUltrasonicSensor);
123+ ev3_lcd_draw_string(ultrasonic_sensor, 20, 40);
124+
125+ //カラーセンサー値画面出力
126+ uint8_t getColorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
127+ char color_sensor[32];
128+ sprintf(color_sensor, "Color: %d", getColorSensor);
129+ ev3_lcd_draw_string(color_sensor, 20, 80);
130+
131+ //ジャイロセンサー値画面出力
132+ int16_t getGyroSensor = ev3_gyro_sensor_get_rate(EV3_PORT_4);
133+ char gyro_sensor[64];
134+ sprintf(gyro_sensor, "Gyro: %d", getGyroSensor);
135+ ev3_lcd_draw_string(gyro_sensor, 20, 20);
136+
137+ tail_control(TAIL_ANGLE_STAND_UP); /* 完全停止用角度に制御 */
138+
139+ if (bt_cmd == 1)
140+ {
141+ break; /* リモートスタート */
142+ }
143+ tslp_tsk(10); /* 10msecウェイト */
144+ }
145+}
146+
147+void CommonApp::resetMotor() {
148+ /* 走行モーターエンコーダーリセット */
149+ ev3_motor_reset_counts(left_motor);
150+ ev3_motor_reset_counts(right_motor);
151+}
152+
153+void CommonApp::resetGyro() {
154+ /* ジャイロセンサーリセット */
155+ ev3_gyro_sensor_reset(EV3_PORT_4);
156+ balance_init(); /* 倒立振子API初期化 */
157+}
158+
159+void CommonApp::startComplete() {
160+ /* スタート通知 */
161+ ev3_led_set_color(LED_GREEN);
162+}
163+
164+void CommonApp::mainLoop() {
165+ /**
166+ * Main loop for the self-balance control algorithm */
167+
168+ // メインループ前処理
169+ beforeLoop();
170+
171+ //暫定としてここで設定値を代入
172+ courseNo = 0;
173+ courseList[courseNo]->SetPIDData();
174+
175+ while(1)
176+ {
177+ // メインループ内の先頭で行う処理
178+ if (!startingLoop())
179+ break;
180+ // 各コースのRunを実行
181+ courseList[courseNo]->Run();
182+ fallingFlg = courseList[courseNo]->continueRunning();
183+ if(!continueRunning()) {
184+ break;
185+ }
186+
187+ // 次コースへの判定処理
188+ if(courseList[courseNo]->IsNextCourse()) {
189+ courseNo++;
190+ courseList[courseNo]->SetPIDData();
191+ courseList[courseNo]->SetStartMotorAngle();
192+ }
193+ endingLoop();
194+ //loopCnt++;
195+ }
196+}
197+
198+void CommonApp::beforeLoop() {
199+ // ログ出力Header
200+ //log("\r\ncolor_sensor,distance,forward,turn,gyro,gyro_offset,motor_ang_l,motor_ang_r,volt,pwm_L,pwm_R\r\n");
201+}
202+
203+bool CommonApp::continueRunning() {
204+ // 走行終了条件
205+ // 走行体が転倒した場合
206+ // PC で '2'キーを押下する
207+ if (!fallingFlg || bt_cmd == 2)
208+ {
209+ ev3_lcd_draw_string("END", 0, CALIB_FONT_HEIGHT*3);
210+ ev3_motor_stop(left_motor, true);
211+ ev3_motor_stop(right_motor, true);
212+ ev3_motor_stop(tail_motor, true);
213+ return false;
214+ }
215+// if (fallingFlg)
216+// {
217+// ev3_lcd_draw_string("END", 0, CALIB_FONT_HEIGHT*3);
218+// ev3_motor_stop(left_motor, true);
219+// ev3_motor_stop(right_motor, true);
220+// ev3_motor_stop(tail_motor, true);
221+// return false;
222+// }
223+// if (bt_cmd == 2) {
224+// log("%s", "pushed!\r\n");
225+// bt_cmd = 0;
226+// }
227+ return true;
228+}
229+
230+void CommonApp::endingLoop() {
231+ // メインループ内の最後尾で行う処理
232+ tslp_tsk(4); /* 4msec周期起動 */
233+}
234+
235+void CommonApp::end() {
236+ // 終了処理
237+
238+ // モーターを停止
239+ stopMotor();
240+
241+ // Bluetoothを終了
242+ endBluetooth();
243+}
244+
245+void CommonApp::stopMotor() {
246+ // モーターを停止
247+ ev3_motor_stop(left_motor, false);
248+ ev3_motor_stop(right_motor, false);
249+}
250+
251+void CommonApp::endBluetooth() {
252+ // Bluetoothを終了
253+ ter_tsk(BT_TASK);
254+ fclose(bt);
255+}
256+
257+void CommonApp::log(const char* format, ...) {
258+ // ログ出力
259+ va_list vaList;
260+ va_start(vaList, format);
261+ vfprintf(bt, format, vaList);
262+ va_end(vaList);
263+}
264+
265+// Bluetoothタスク内で実行される関数
266+void CommonApp::do_bt_task() {
267+ while(1)
268+ {
269+ uint8_t c = fgetc(bt); /* 受信 */
270+ switch(c)
271+ {
272+ case '1':
273+ bt_cmd = 1;
274+ break;
275+ case '2':
276+ bt_cmd = 2;
277+ break;
278+ default:
279+ break;
280+ }
281+ //fputc(c, bt); /* エコーバック */
282+ }
283+}
284+
285+// 各作業者別のappを作成
286+CommonApp* CommonApp::createApp() {
287+
288+ CommonApp* app = new CommonApp();
289+ return app;
290+}
291+
292+void CommonApp::CreateCourse(int courseType)
293+{
294+ ev3_lcd_draw_string("1", 0, CALIB_FONT_HEIGHT*1);
295+ courseList.clear();
296+// courseList.push_back(new Stairs());
297+ if(COURSETYPE_LEFT == courseType) {
298+ courseList.push_back(new StartToGoalL());
299+ courseList.push_back(new LookupGate());
300+ courseList.push_back(new Garage());
301+ //courseList.push_back();
302+ }
303+ else {
304+ courseList.push_back(new StartToGoalR());
305+ courseList.push_back(new Stairs());
306+ courseList.push_back(new Garage());
307+ //courseList.push_back();
308+ }
309+}
310+
311+void CommonApp::BaseCourseIni()
312+{
313+
314+}
315+
316+bool CommonApp::startingLoop() {
317+ // メインループ内の先頭で行う処理
318+
319+ if (ev3_button_is_pressed(BACK_BUTTON)) return false;
320+
321+ if(!tailRised) {
322+ tailRised = tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */
323+ }
324+
325+ return true;
326+}
327+
328+
329+//*****************************************************************************
330+// 関数名 : tail_control
331+// 引数 : angle (モータ目標角度[度])
332+// 返り値 : 目標角度に到達したか否か
333+// 概要 : 走行体完全停止用モータの角度制御
334+//*****************************************************************************
335+bool CommonApp::tail_control(signed int angle)
336+{
337+ float pwm = (float)(angle - ev3_motor_get_counts(tail_motor))*P_GAIN; /* 比例制御 */
338+ /* PWM出力飽和処理 */
339+ if (pwm > PWM_ABS_MAX)
340+ {
341+ pwm = PWM_ABS_MAX;
342+ }
343+ else if (pwm < -PWM_ABS_MAX)
344+ {
345+ pwm = -PWM_ABS_MAX;
346+ }
347+
348+ if (pwm == 0)
349+ {
350+ ev3_motor_stop(tail_motor, true);
351+ return true;
352+ }
353+ else
354+ {
355+ ev3_motor_set_power(tail_motor, (signed char)pwm);
356+ return false;
357+ }
358+}
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/CommonApp.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/CommonApp.h (revision 409)
@@ -0,0 +1,193 @@
1+#ifndef COMMONAPP_H
2+#define COMMONAPP_H
3+
4+
5+/**********************************/
6+/* キャリブレーション時の尻尾角指定 */
7+/**********************************/
8+//#define TAIL_ANGLE_STAND_UP 60 /* 完全停止時の角度[度] */
9+//#define TAIL_ANGLE_STAND_UP 61 /* 完全停止時の角度[度] */
10+//#define TAIL_ANGLE_STAND_UP 62 /* 完全停止時の角度[度] */
11+//#define TAIL_ANGLE_STAND_UP 63 /* 完全停止時の角度[度] */
12+//#define TAIL_ANGLE_STAND_UP 64 /* 完全停止時の角度[度] */
13+//#define TAIL_ANGLE_STAND_UP 65 /* 完全停止時の角度[度] */
14+//#define TAIL_ANGLE_STAND_UP 66 /* 完全停止時の角度[度] */
15+//#define TAIL_ANGLE_STAND_UP 67 /* 完全停止時の角度[度] */
16+//#define TAIL_ANGLE_STAND_UP 68 /* 完全停止時の角度[度] */
17+//#define TAIL_ANGLE_STAND_UP 69 /* 完全停止時の角度[度] */
18+//#define TAIL_ANGLE_STAND_UP 70 /* 完全停止時の角度[度] */
19+//#define TAIL_ANGLE_STAND_UP 71 /* 完全停止時の角度[度] */
20+//#define TAIL_ANGLE_STAND_UP 72 /* 完全停止時の角度[度] */
21+//#define TAIL_ANGLE_STAND_UP 73 /* 完全停止時の角度[度] */
22+//#define TAIL_ANGLE_STAND_UP 74 /* 完全停止時の角度[度] */
23+//#define TAIL_ANGLE_STAND_UP 75 /* 完全停止時の角度[度] */
24+//#define TAIL_ANGLE_STAND_UP 76 /* 完全停止時の角度[度] */
25+//#define TAIL_ANGLE_STAND_UP 77 /* 完全停止時の角度[度] */
26+//#define TAIL_ANGLE_STAND_UP 78 /* 完全停止時の角度[度] */
27+//#define TAIL_ANGLE_STAND_UP 79 /* 完全停止時の角度[度] */
28+//#define TAIL_ANGLE_STAND_UP 80 /* 完全停止時の角度[度] */
29+//#define TAIL_ANGLE_STAND_UP 81 /* 完全停止時の角度[度] */
30+//#define TAIL_ANGLE_STAND_UP 82 /* 完全停止時の角度[度] */
31+//#define TAIL_ANGLE_STAND_UP 83 /* 完全停止時の角度[度] */
32+//#define TAIL_ANGLE_STAND_UP 84 /* 完全停止時の角度[度] */
33+//#define TAIL_ANGLE_STAND_UP 85 /* 完全停止時の角度[度] */
34+//#define TAIL_ANGLE_STAND_UP 86 /* 完全停止時の角度[度] */
35+//#define TAIL_ANGLE_STAND_UP 87 /* 完全停止時の角度[度] */
36+//#define TAIL_ANGLE_STAND_UP 88 /* 完全停止時の角度[度] */
37+//#define TAIL_ANGLE_STAND_UP 89 /* 完全停止時の角度[度] */
38+//#define TAIL_ANGLE_STAND_UP 90 /* 完全停止時の角度[度] */
39+#define TAIL_ANGLE_STAND_UP 66 /* ルックアップゲートをくぐるときの角度[度] */
40+//#define TAIL_ANGLE_STAND_UP 84 /* ガレージインのときの角度 */
41+
42+
43+
44+#include <vector>
45+#include "ev3api.h"
46+#include "balancer/balancer.h"
47+#include "../include/member.h"
48+#include "BaseCourse.h"
49+
50+#if defined(BUILD_MODULE)
51+#include "module_cfg.h"
52+#else
53+#include "kernel_cfg.h"
54+#endif
55+
56+#define DEBUG
57+
58+#ifdef DEBUG
59+#define _debug(x) (x)
60+#else
61+#define _debug(x)
62+#endif
63+
64+ #define COURSETYPE_LEFT 0
65+ #define COURSETYPE_RIGHT 1
66+
67+/* 下記のマクロは個体/環境に合わせて変更する必要があります */
68+/* sample_c1マクロ */
69+#define GYRO_OFFSET 0 /* ジャイロセンサオフセット初期値(角速度0[deg/sec]時) */
70+#define LIGHT_WHITE 40 /* 白色の光センサ値 */
71+#define LIGHT_BLACK 0 /* 黒色の光センサ値 */
72+/* sample_c2マクロ */
73+/* sample_c3マクロ */
74+
75+
76+
77+
78+#define TAIL_ANGLE_DRIVE 30 /* バランス走行時の角度[度] */
79+#define P_GAIN 2.5F /* 完全停止用モータ制御比例係数 */
80+#define PWM_ABS_MAX 20 /* 完全停止用モータ制御PWM絶対最大値 */
81+#define KP 1.0 /* P制御用係数*/
82+#define KI 1.5 /* I制御用係数*/
83+#define KD 1.5 /* D制御用係数*/
84+#define PROCESS_PERIOD 0.004 /* 処理周期(ミリ秒)*/
85+/* sample_c4マクロ */
86+//#define DEVICE_NAME "ET0" /* Bluetooth名 hrp2/target/ev3.h BLUETOOTH_LOCAL_NAMEで設定 */
87+//#define PASS_KEY "1234" /* パスキー hrp2/target/ev3.h BLUETOOTH_PIN_CODEで設定 */
88+
89+#define CMD_START '1' /* リモートスタートコマンド */
90+
91+/* LCDフォントサイズ */
92+#define CALIB_FONT (EV3_FONT_SMALL)
93+#define CALIB_FONT_WIDTH (6/*TODO: magic number*/)
94+#define CALIB_FONT_HEIGHT (8/*TODO: magic number*/)
95+
96+
97+const sensor_port_t
98+ touch_sensor = EV3_PORT_1,
99+ sonar_sensor = EV3_PORT_2,
100+ color_sensor = EV3_PORT_3,
101+ gyro_sensor = EV3_PORT_4;
102+
103+const motor_port_t
104+ left_motor = EV3_PORT_C,
105+ right_motor = EV3_PORT_B,
106+ tail_motor = EV3_PORT_A;
107+
108+class CommonApp {
109+public:
110+ CommonApp();
111+ virtual ~CommonApp();
112+
113+ // 初期化
114+ void init();
115+ virtual void initLcd();
116+ virtual void initSenserProt();
117+ virtual void initMotorPort();
118+
119+
120+ virtual void openBluetooth();
121+ virtual void startBluetooth();
122+
123+ virtual void initComplete();
124+
125+ virtual void waitStart();
126+
127+ virtual void resetMotor();
128+ virtual void resetGyro();
129+
130+ virtual void startComplete();
131+
132+ // メインループ
133+ void mainLoop();
134+ void beforeLoop();
135+ //virtual void afterRecord(int position);
136+ bool startingLoop();
137+ //virtual void setForward();
138+ //virtual void updatePidValue();
139+ //virtual void normalizeTurnValue();
140+ //virtual void updateSensorValue();
141+ //virtual void calcMotorPower();
142+ //virtual void setMotorPower();
143+ bool continueRunning();
144+ void endingLoop();
145+
146+ // 終了処理
147+ void end();
148+ virtual void stopMotor();
149+ virtual void endBluetooth();
150+
151+ // その他
152+ virtual void log(const char* format, ...);
153+ virtual void do_bt_task();
154+
155+ static CommonApp* createApp();
156+
157+
158+ static bool tail_control(signed int angle);
159+
160+
161+
162+ void CreateCourse(int courseType);
163+ void BaseCourseIni();
164+
165+
166+protected:
167+ std::vector<BaseCourse*> courseList;
168+
169+ // メインループ内で使用する変数
170+ // ログを取りやすくするためメンバ変数にしてあります
171+ float forward = 0.0F; /* 前後進命令 */
172+ float turn = 0.0F; /* 旋回命令 */
173+ /* 左右モータPWM出力 */
174+
175+ unsigned long loopCnt = 0;
176+ char buffer[2046];
177+
178+ unsigned int distance = 0;
179+ // 転倒時判定フラグ
180+ bool fallingFlg = false;
181+
182+ int courseNo = 0;
183+
184+
185+ int bt_cmd = 0; /* Bluetoothコマンド 1:リモートスタート */
186+ FILE *bt = NULL; /* Bluetoothファイルハンドル */
187+
188+private:
189+ bool tailRised = false;
190+};
191+
192+#endif /* COMMONAPP_H */
193+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/PIDDataManager.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/PIDDataManager.cpp (revision 409)
@@ -0,0 +1,69 @@
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+
23+PIDDataManager::PIDDataManager(const PIDDataManager& orig) {
24+}
25+
26+PIDDataManager::~PIDDataManager() {
27+}
28+
29+void PIDDataManager::SetFixedData(float pVal, float iVal, float dVal)
30+{
31+ m_pFixVal = pVal;
32+ m_iFixVal = iVal;
33+ m_dFixVal = dVal;
34+}
35+
36+// PID制御を実行し、計算したTurn値を返す
37+float PIDDataManager::GetTurnValue()
38+{
39+
40+ // PID制御
41+ int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
42+ float hensa = colorSensor - (LIGHT_WHITE + 0 + LIGHT_BLACK)/2;
43+ float hensaTotal = std::accumulate(m_hensaList.begin(), m_hensaList.end(), 0.001) + hensa;
44+ float integral = (hensaTotal) / 2 * PROCESS_PERIOD;
45+ float wkP = hensa * m_pFixVal;
46+ float wkI = integral * m_iFixVal;
47+ float wkD = (hensa - m_hensaList.back()) / PROCESS_PERIOD * m_dFixVal ;
48+ float turn = wkP + wkI + wkD;
49+
50+ // 偏差値の履歴(配列)作成
51+ // 履歴数が履歴の最大数を超えている場合、最古の偏差値を削除
52+ if ((int)m_hensaList.size() >= HISTORY_MAX)
53+ {
54+ m_hensaList.erase(m_hensaList.begin());
55+ }
56+
57+ // 履歴へ現在の偏差値を追加
58+ m_hensaList.push_back(hensa);
59+
60+ turn = turn * -1;
61+
62+ return turn;
63+}
64+
65+
66+
67+
68+
69+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/PIDDataManager.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/PIDDataManager.h (revision 409)
@@ -0,0 +1,48 @@
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 1250
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+private:
34+
35+ std::vector<float> m_hensaList;
36+
37+ // PID制御に使用する係数値
38+ float m_pFixVal;
39+ float m_iFixVal;
40+ float m_dFixVal;
41+
42+
43+};
44+
45+//extern PIDDataManager PIDMgr;
46+
47+#endif /* PIDDATAMANAGER_H */
48+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/app.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/app.cpp (revision 409)
@@ -0,0 +1,73 @@
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+ while (true)
32+ {
33+
34+
35+ if (ev3_touch_sensor_is_pressed(EV3_PORT_1) == 1)
36+ {
37+ break; /* タッチセンサが押された */
38+ }
39+
40+ tslp_tsk(10); /* 10msecウェイト */
41+ }
42+
43+ /**
44+ * Main loop for the self-balance control algorithm
45+ */
46+// app->mainLoop();
47+
48+ // ループ後 終了処理
49+ app->end();
50+
51+ } catch(...) { }
52+
53+ if (app != NULL) {
54+ delete app;
55+ app = NULL;
56+ }
57+
58+ ext_tsk();
59+}
60+
61+
62+//*****************************************************************************
63+// 関数名 : bt_task
64+// 引数 : unused
65+// 返り値 : なし
66+// 概要 : Bluetooth通信によるリモートスタート。 Tera Termなどのターミナルソフトから、
67+// ASCIIコードで1を送信すると、リモートスタートする。
68+//*****************************************************************************
69+void bt_task(intptr_t unused)
70+{
71+ app->do_bt_task();
72+}
73+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/app.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/app.h (revision 409)
@@ -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/calib_cpp/balancer/balancer.c (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/balancer/balancer.c (revision 409)
@@ -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/calib_cpp/balancer/balancer.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/balancer/balancer.h (revision 409)
@@ -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.00400000019F /* バランス制御実行周期(秒) */
38+#define EXEC_PERIOD 0.00500000000F /* バランス制御実行周期(秒) *//* forEV3 20141102koji */
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/calib_cpp/balancer/balancer_param.c (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/balancer/balancer_param.c (revision 409)
@@ -0,0 +1,33 @@
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.870303F, -31.9978F, -1.1566F*0.6, -2.78873F};
25+float K_I = -0.44721F; /* サーボ制御用積分フィードバック係数 */
26+
27+float K_PHIDOT = 25.0F*2.5F; /* 車体目標旋回角速度係数 */
28+float K_THETADOT = 7.5F; /* モータ目標回転角速度係数 */
29+
30+const float BATTERY_GAIN = 0.001089F; /* PWM出力算出用バッテリ電圧補正係数 */
31+const float BATTERY_OFFSET = 0.625F; /* PWM出力算出用バッテリ電圧補正オフセット */
32+
33+/******************************** END OF FILE ********************************/
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/balancer/balancer_private.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/balancer/balancer_private.h (revision 409)
@@ -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/calib_cpp/course/Garage.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Garage.cpp (revision 409)
@@ -0,0 +1,63 @@
1+#include "Garage.h"
2+
3+Garage::Garage()
4+ : garageState(GarageStateRun)
5+{
6+}
7+
8+Garage::~Garage()
9+{
10+}
11+
12+void Garage::Run()
13+{
14+ switch(garageState)
15+ {
16+ case GarageStateRun:
17+ MoveDistance();
18+ break;
19+ case GarageStateStop:
20+ Stop();
21+ break;
22+ case GarageStateAdjustment:
23+ Adjustment();
24+ break;
25+ }
26+}
27+
28+void Garage::setForward()
29+{
30+ switch(garageState)
31+ {
32+ case GarageStateRun:
33+ forward = 30.0;
34+ break;
35+ case GarageStateStop:
36+ case GarageStateAdjustment:
37+ forward = 0.0F;
38+ break;
39+ }
40+}
41+
42+void Garage::MoveDistance()
43+{
44+ BaseCourse::Run();
45+ if(800 < motor_ang_l)
46+ {
47+ garageState = GarageStateStop;
48+ }
49+}
50+
51+void Garage::Stop()
52+{
53+ if(stopper.DoStop())
54+ {
55+ garageState = GarageStateRun;
56+ stopper = Stopper();
57+ }
58+}
59+
60+void Garage::Adjustment()
61+{
62+
63+}
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Garage.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Garage.h (revision 409)
@@ -0,0 +1,36 @@
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();
10+ virtual ~Garage();
11+
12+ void Run();
13+ void setForward();
14+
15+private:
16+ Garage(const Garage& orig){}
17+
18+ void MoveDistance();
19+ void Stop();
20+ void Adjustment();
21+
22+private:
23+ enum GarageState
24+ {
25+ GarageStateRun,
26+ GarageStateStop,
27+ GarageStateAdjustment,
28+ };
29+
30+ GarageState garageState;
31+ Stopper stopper;
32+
33+};
34+
35+#endif /* GARAGE_H */
36+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/LookupGate.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/LookupGate.cpp (revision 409)
@@ -0,0 +1,448 @@
1+#include "LookupGate.h"
2+
3+LookupGate::LookupGate():lookupGateState(LookupGateStateBeforeSonorDetected)
4+{
5+}
6+
7+LookupGate::~LookupGate()
8+{
9+}
10+
11+void LookupGate::Run()
12+{
13+ switch(lookupGateState)
14+ {
15+ case LookupGateStateBeforeSonorDetected:
16+ runUntilSonorDetected();
17+ break;
18+ case LookupGateStateAfterSonorDetected:
19+ runSonorDetected();
20+ break;
21+ case LookupGateStateThroughGateReady1st:
22+ throughGateReady1st();
23+ break;
24+ case LookupGateStateThroughGateReady2nd:
25+ tail_control(86.0F);
26+ throughGateReady2nd();
27+ break;
28+ case LookupGateStateThroughGateReady3rd:
29+ throughGateReady3rd();
30+ break;
31+ case LookupGateStateThroughGateReady4th:
32+ tail_control(76.0F);
33+ throughGateReady4th();
34+ break;
35+ case LookupGateStateThroughGateReady5th:
36+ throughGateReady5th();
37+ break;
38+ case LookupGateStateThroughGateReady6th:
39+ tail_control(66.0F);
40+ throughGateReady6th();
41+ break;
42+ case LookupGateStateThroughGateForward:
43+ tail_control(66.0F);
44+ ForwardRun();
45+ break;
46+ case LookupGateStateThroughGateStop1:
47+ tail_control(66.0F);
48+ RunStop1();
49+ break;
50+ case LookupGateStateThroughGateBack:
51+ tail_control(66.0F);
52+ BackRun();
53+ break;
54+ case LookupGateStateThroughGateStop2:
55+ tail_control(66.0F);
56+ RunStop2();
57+ break;
58+ case LookupGateStateThroughGateForwardAgain:
59+ tail_control(66.0F);
60+ ForwardRunAgain();
61+ break;
62+ case LookupGateStateThroughGateStop3:
63+ tail_control(66.0F);
64+ RunStop3();
65+ break;
66+ case LookupGateStateUpridhtBody1st:
67+ UpridhtBody1st();
68+ break;
69+ case LookupGateStateUpridhtBody2nd:
70+ UpridhtBody2nd();
71+ break;
72+ case LookupGateStateLineTraceRun:
73+ tail_control(81.0F);
74+ ForwardRunFinal();
75+ break;
76+ }
77+}
78+
79+void LookupGate::setForward()
80+{
81+ forward = 0;
82+ switch(lookupGateState)
83+ {
84+ case LookupGateStateBeforeSonorDetected:
85+ forward = 10;
86+ break;
87+ case LookupGateStateAfterSonorDetected:
88+ forward = 0;
89+ break;
90+ case LookupGateStateThroughGateReady1st:
91+ forward = 0;
92+ break;
93+ case LookupGateStateThroughGateReady2nd:
94+ forward = 5;
95+ break;
96+ case LookupGateStateThroughGateReady3rd:
97+ forward = 5;
98+ break;
99+ case LookupGateStateThroughGateReady4th:
100+ forward = 5;
101+ break;
102+ case LookupGateStateThroughGateReady5th:
103+ forward = 5;
104+ break;
105+ case LookupGateStateThroughGateReady6th:
106+ forward = 0;
107+ break;
108+ case LookupGateStateThroughGateForward:
109+ forward = 5;
110+ break;
111+ case LookupGateStateThroughGateStop1:
112+ forward = 0;
113+ break;
114+ case LookupGateStateThroughGateBack:
115+ forward = -10;
116+ break;
117+ case LookupGateStateThroughGateStop2:
118+ forward = 0;
119+ break;
120+ case LookupGateStateThroughGateForwardAgain:
121+ forward = 5;
122+ break;
123+ case LookupGateStateThroughGateStop3:
124+ forward = 0;
125+ break;
126+ case LookupGateStateUpridhtBody1st:
127+ forward = -3;
128+ break;
129+ case LookupGateStateUpridhtBody2nd:
130+ forward = -3;
131+ break;
132+ case LookupGateStateLineTraceRun:
133+ forward = 5;
134+ break;
135+ }
136+}
137+
138+void LookupGate::runUntilSonorDetected()
139+{
140+ runMode = 0;
141+ BaseCourse::Run();
142+ if (sonar_alert()==1)
143+ {
144+ log("LookupGateStateAfterSonorDetected\r\n");
145+ lookupGateState = LookupGateStateAfterSonorDetected;
146+ counter = 0;
147+ }
148+}
149+
150+void LookupGate::runSonorDetected()
151+{
152+ runMode = 0;
153+ BaseCourse::Run();
154+ if (++counter >= 300)
155+ {
156+ log("LookupGateStateThroughGateReady1st\r\n");
157+ lookupGateState = LookupGateStateThroughGateReady1st;
158+ counter = 0;
159+ }
160+}
161+
162+void LookupGate::throughGateReady1st()
163+{
164+ runMode = 0;
165+ BaseCourse::Run();
166+ if(tail_control(86.0F))
167+ {
168+ log("LookupGateStateThroughGateReady2nd\r\n");
169+ lookupGateState = LookupGateStateThroughGateReady2nd;
170+ }
171+}
172+
173+void LookupGate::throughGateReady2nd()
174+{
175+ runMode = 0;
176+ BaseCourse::Run();
177+ if (++counter >= 300)
178+ {
179+ log("LookupGateStateThroughGateReady3rd\r\n");
180+ lookupGateState = LookupGateStateThroughGateReady3rd;
181+ counter = 0;
182+ }
183+}
184+
185+void LookupGate::throughGateReady3rd()
186+{
187+ runMode = 1;
188+ BaseCourse::Run();
189+ if(tail_control(76.0F))
190+ {
191+ log("LookupGateStateThroughGateReady4th\r\n");
192+ lookupGateState = LookupGateStateThroughGateReady4th;
193+ }
194+}
195+
196+void LookupGate::throughGateReady4th()
197+{
198+ runMode = 1;
199+ BaseCourse::Run();
200+ if (++counter >= 300)
201+ {
202+ log("LookupGateStateThroughGateReady6th\r\n");
203+ lookupGateState = LookupGateStateThroughGateReady6th;
204+ counter = 0;
205+ }
206+}
207+
208+void LookupGate::throughGateReady5th()
209+{
210+ runMode = 1;
211+ BaseCourse::Run();
212+ if(tail_control(66.0F))
213+ {
214+ log("LookupGateStateThroughGateReady5th\r\n");
215+ lookupGateState = LookupGateStateThroughGateReady5th;
216+ }
217+}
218+
219+void LookupGate::throughGateReady6th()
220+{
221+ runMode = 1;
222+ BaseCourse::Run();
223+ if (++counter >= 300)
224+ {
225+ log("LookupGateStateThroughGateForward\r\n");
226+ lookupGateState = LookupGateStateThroughGateForward;
227+ counter = 0;
228+ }
229+}
230+
231+void LookupGate::ForwardRun()
232+{
233+ runMode = 1;
234+ BaseCourse::Run();
235+ if (++counter >= 1100)
236+ {
237+ log("LookupGateStateThroughGateStop1\r\n");
238+ lookupGateState = LookupGateStateThroughGateStop1;
239+ counter = 0;
240+ }
241+}
242+
243+void LookupGate::RunStop1()
244+{
245+ runMode = 1;
246+ BaseCourse::Run();
247+ if (++counter >= 100)
248+ {
249+ log("LookupGateStateThroughGateBack\r\n");
250+ lookupGateState = LookupGateStateThroughGateBack;
251+ counter = 0;
252+ }
253+}
254+
255+void LookupGate::BackRun()
256+{
257+ runMode = 2;
258+ BaseCourse::Run();
259+ if (++counter >= 700)
260+ {
261+ log("LookupGateStateThroughGateStop2\r\n");
262+ lookupGateState = LookupGateStateThroughGateStop2;
263+ counter = 0;
264+ }
265+}
266+
267+void LookupGate::RunStop2()
268+{
269+ runMode = 1;
270+ BaseCourse::Run();
271+ if (++counter >= 100)
272+ {
273+ log("LookupGateStateThroughGateForwardAgain\r\n");
274+ lookupGateState = LookupGateStateThroughGateForwardAgain;
275+ counter = 0;
276+ }
277+}
278+
279+void LookupGate::ForwardRunAgain()
280+{
281+ runMode = 2;
282+ BaseCourse::Run();
283+ if (++counter >= 1000)
284+ {
285+ log("LookupGateStateThroughGateStop3\r\n");
286+ lookupGateState = LookupGateStateThroughGateStop3;
287+ counter = 0;
288+ }
289+}
290+
291+void LookupGate::RunStop3()
292+{
293+ runMode = 1;
294+ BaseCourse::Run();
295+ if (++counter >= 100)
296+ {
297+ log("LookupGateStateThroughUpridhtBody\r\n");
298+ lookupGateState = LookupGateStateUpridhtBody1st;
299+ counter = 0;
300+ }
301+}
302+
303+void LookupGate::UpridhtBody1st()
304+{
305+ runMode = 1;
306+ BaseCourse::Run();
307+ if(tail_control(76.0F))
308+ {
309+ log("LookupGateStateLineTraceRun\r\n");
310+ lookupGateState = LookupGateStateUpridhtBody2nd;
311+ }
312+}
313+
314+void LookupGate::UpridhtBody2nd()
315+{
316+ runMode = 1;
317+ BaseCourse::Run();
318+ if(tail_control(81.0F))
319+ {
320+ log("LookupGateStateLineTraceRun\r\n");
321+ lookupGateState = LookupGateStateLineTraceRun;
322+ }
323+}
324+
325+void LookupGate::ForwardRunFinal()
326+{
327+ runMode = 1;
328+ BaseCourse::Run();
329+}
330+
331+void LookupGate::SetPIDData()
332+{
333+ // PID制御に使用する定数を設定する
334+ PIDMgr->SetFixedData(0.5, 1, 0.03);
335+}
336+
337+bool LookupGate::tail_control(signed int angle)
338+{
339+ float pwm = (float)(angle - ev3_motor_get_counts(tail_motor)) * P_GAIN; /* 比例制御 */
340+ /* PWM出力飽和処理 */
341+ if (pwm > PWM_ABS_MAX_LOOKUP_GATE)
342+ {
343+ pwm = PWM_ABS_MAX_LOOKUP_GATE;
344+ }
345+ else if (pwm < -PWM_ABS_MAX_LOOKUP_GATE)
346+ {
347+ pwm = -PWM_ABS_MAX_LOOKUP_GATE;
348+ }
349+
350+ if (pwm == 0)
351+ {
352+ ev3_motor_stop(tail_motor, true);
353+ return true;
354+ }
355+ else
356+ {
357+ ev3_motor_set_power(tail_motor, (signed char)pwm);
358+ return false;
359+ }
360+}
361+
362+void LookupGate::calcMotorPower()
363+{
364+// log("%d\r\n", runMode);
365+ if(runMode == 0)
366+ {
367+ BaseCourse::calcMotorPower();
368+ }
369+ else if(runMode == 1)
370+ {
371+ int colorSensor = ev3_color_sensor_get_reflect(EV3_PORT_3);
372+ //log("%d,%d,\r\n", colorSensor,forward);
373+ float hensa = colorSensor - (LIGHT_WHITE_LOOKUP_GATE + 0 + LIGHT_BLACK_LOOKUP_GATE)/2;
374+ float wkP = hensa * KP;
375+ float turn = wkP;
376+ float turnR = 0;
377+ float turnL = 0;
378+ turn = turn * -1;
379+ if(forward > 0)
380+ {
381+ if (turn > 0)
382+ {
383+ turnR = 3;
384+ }
385+ else
386+ {
387+ turnL = 3;
388+ }
389+ //倒立運動なし計算
390+ pwm_L = forward + turnR;
391+ pwm_R = forward + turnL;
392+ }
393+ else if(forward < 0)
394+ {
395+ if (turn < 0)
396+ {
397+ turnR = -3;
398+ }
399+ else
400+ {
401+ turnL = -3;
402+ }
403+ //倒立運動なし計算
404+ pwm_L = forward + turnL;
405+ pwm_R = forward + turnR;
406+ }
407+ else if(forward == 0)
408+ {
409+ pwm_L = 0;
410+ pwm_R = 0;
411+ }
412+ }
413+ else if(runMode == 2)
414+ {
415+ pwm_L = forward;
416+ pwm_R = forward;
417+ }
418+}
419+
420+//*****************************************************************************
421+// 関数名 : sonar_alert
422+// 引数 : 無し
423+// 返り値 : 1(障害物あり)/0(障害物無し)
424+// 概要 : 超音波センサによる障害物検知
425+//*****************************************************************************
426+int LookupGate::sonar_alert(void)
427+{
428+ static unsigned int counter = 0;
429+ static int alert = 0;
430+ if (++counter == 40/4) /* 約40msec周期毎に障害物検知 */
431+ {
432+ distance = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
433+ if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0))
434+ {
435+ alert = 1; /* 障害物を検知 */
436+ }
437+ else
438+ {
439+ alert = 0; /* 障害物無し */
440+ }
441+ counter = 0;
442+ }
443+ else
444+ {
445+ distance = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
446+ }
447+ return alert;
448+}
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/LookupGate.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/LookupGate.h (revision 409)
@@ -0,0 +1,90 @@
1+#ifndef LOOKUPGATE_H
2+#define LOOKUPGATE_H
3+
4+#define P_GAIN 2.5F /* 完全停止用モータ制御比例係数 */
5+#define PWM_ABS_MAX_LOOKUP_GATE 20 /* 完全停止用モータ制御PWM絶対最大値 */
6+#define SONAR_ALERT_DISTANCE 19/* 超音波センサによる障害物検知距離[cm] */
7+#define LIGHT_WHITE_LOOKUP_GATE 17 /* 白色の光センサ値 */
8+#define LIGHT_BLACK_LOOKUP_GATE 2 /* 黒色の光センサ値 */
9+#define KP 1.0 /* P制御用係数*/
10+#define KI 1.5 /* I制御用係数*/
11+#define KD 1.5 /* D制御用係数*/
12+
13+#include "BaseCourse.h"
14+#include "ev3api.h"
15+
16+class LookupGate : public BaseCourse {
17+ public:
18+ LookupGate();
19+ virtual ~LookupGate();
20+
21+ void setForward();
22+ void runUntilSonorDetected();
23+ void runSonorDetected();
24+ void Run();
25+ void ForwardRun();
26+ void BackRun();
27+ void ForwardRunAgain();
28+ void throughGateReady1st();
29+ void throughGateReady2nd();
30+ void throughGateReady3rd();
31+ void throughGateReady4th();
32+ void throughGateReady5th();
33+ void throughGateReady6th();
34+ void RunStop1();
35+ void RunStop2();
36+ void RunStop3();
37+ void UpridhtBody1st();
38+ void UpridhtBody2nd();
39+ void ForwardRunFinal();
40+ void SetPIDData();
41+ void calcMotorPower();
42+ bool tail_control(signed int angle);
43+ virtual int sonar_alert(void);
44+ private:
45+ LookupGate(const LookupGate& orig){}
46+ bool m_stop;
47+
48+ private:
49+ enum LookupGateState
50+ {
51+ LookupGateStateBeforeSonorDetected,
52+ LookupGateStateAfterSonorDetected,
53+ LookupGateStateThroughGateReady1st,
54+ LookupGateStateThroughGateReady2nd,
55+ LookupGateStateThroughGateReady3rd,
56+ LookupGateStateThroughGateReady4th,
57+ LookupGateStateThroughGateReady5th,
58+ LookupGateStateThroughGateReady6th,
59+ LookupGateStateThroughGateForward,
60+ LookupGateStateThroughGateStop1,
61+ LookupGateStateThroughGateBack,
62+ LookupGateStateThroughGateStop2,
63+ LookupGateStateThroughGateForwardAgain,
64+ LookupGateStateThroughGateStop3,
65+ LookupGateStateUpridhtBody1st,
66+ LookupGateStateUpridhtBody2nd,
67+ LookupGateStateLineTraceRun,
68+ };
69+
70+ LookupGateState lookupGateState;
71+ int distanceWork;
72+ int runMode = 0;
73+ unsigned int distance = 0;
74+ signed char pwm = 0;
75+ unsigned int counter = 0;
76+ const sensor_port_t
77+ touch_sensor = EV3_PORT_1,
78+ sonar_sensor = EV3_PORT_2,
79+ color_sensor = EV3_PORT_3,
80+ gyro_sensor = EV3_PORT_4;
81+
82+ const motor_port_t
83+ left_motor = EV3_PORT_C,
84+ right_motor = EV3_PORT_B,
85+ tail_motor = EV3_PORT_A;
86+};
87+
88+
89+
90+#endif /* GARAGE_H */
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stairs.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stairs.cpp (revision 409)
@@ -0,0 +1,520 @@
1+/*
2+ * File: Stairs.cpp
3+ * Author: y.nakamura
4+ *
5+ * Created on 2016/08/07, 16:16
6+ */
7+#include "Stairs.h"
8+
9+Stairs::Stairs(): stairsState(StairsState1stStepDetection)
10+{
11+}
12+
13+Stairs::~Stairs()
14+{
15+ // TODO 変数の解放
16+
17+}
18+
19+/**
20+ * @brief 走行処理(オーバーライド)
21+ * @note
22+ * 階段走行時のモードに従い処理を分岐する
23+ *
24+ */
25+void Stairs::Run()
26+{
27+ switch(stairsState)
28+ {
29+ //段差検知モード
30+ case StairsState1stStepDetection:
31+ case StairsState2ndStepDetection:
32+ StepDetection();
33+ break;
34+
35+ //バック走行
36+ case StairsState1stBackward:
37+ case StairsState2ndBackward:
38+ Backward();
39+ break;
40+
41+ //昇段モード
42+ case StairsState1stStepUp:
43+ case StairsState2ndStepUp:
44+ StepUp();
45+ break;
46+
47+ // 昇段後位置調整&静止モード
48+ case StairsState1stStop:
49+ case StairsState2ndStop:
50+ Stop();
51+ break;
52+
53+ // スピンモード
54+ case StairsState1stSpin:
55+ case StairsState2ndSpin:
56+ Spin();
57+ break;
58+
59+ // ライン探索モード(前進)モード
60+ case StairsState1stSearchLineForward:
61+ case StairsState2ndSearchLineForward:
62+ case StairsStateAfterStepDownSearchLineForward:
63+ SearchLineForward();
64+ break;
65+
66+ // ライン探索モード(後進)モード
67+ case StairsState1stSearchLineBackward:
68+ case StairsState2ndSearchLineBackward:
69+ case StairsStateAfterStepDownSearchLineBackward:
70+ SearchLineBackward();
71+ break;
72+
73+ // ライン探索モード(進行方向角度変更)モード
74+ case StairsState1stSearchLineChangeAngle:
75+ case StairsState2ndSearchLineChangeAngle:
76+ case StairsStateAfterStepDownSearchLineChangeAngle:
77+ SearchLineChangeAngle();
78+ break;
79+
80+ // ライントレースモード
81+ case StairsStateBeforeStepDownLineTrace:
82+ case StairsStateAfterStepDownLineTrace:
83+ LineTrace();
84+ break;
85+
86+ // 降段モード
87+ case StairsStateStepDown:
88+ StepDown();
89+ break;
90+ }
91+}
92+
93+/**
94+ * @brief 段差検知
95+ * @note
96+ * ジャイロ値を元に段差を検知する
97+ * 段差検知したら次のモードに移行
98+ */
99+void Stairs::StepDetection()
100+{
101+ BaseCourse::Run();
102+ if(STEP_DETECTION_GYRO < std::abs(gyro))
103+ {
104+ changeNextState();
105+ }
106+}
107+
108+
109+/**
110+ * @brief バック走行
111+ * @note
112+ * 一定距離をバック走行して段差を昇るための助走距離分戻る
113+ * 一定距離を走ったら次のモードに移行
114+ */
115+void Stairs::Backward()
116+{
117+ BaseCourse::Run();
118+ //log("Backward %d, %d, %d, %d \r\n", wk_motor_ang_l, wk_motor_ang_r, motor_ang_l, motor_ang_r);
119+ if(-400 > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
120+ {
121+ gyro_offset = STEP_UP_GYRO_OFFSET;
122+ changeNextState();
123+ }
124+}
125+
126+
127+/**
128+ * @brief 昇段
129+ * @note
130+ * 一定距離を走行して段差を昇る
131+ * 段差を昇ったら次のモードに移行
132+ */
133+void Stairs::StepUp()
134+{
135+ BaseCourse::Run();
136+ if(550 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
137+ {
138+ gyro_offset = NORMAL_GYRO_OFFSET;
139+ changeNextState();
140+ }
141+}
142+
143+/**
144+ * @brief 静止+位置調整
145+ * @note
146+ * 段差を上った後のふらつきを止めて、スピンする場所に移動する
147+ * 一定時間経ったら次のモードに移行
148+ */
149+void Stairs::Stop()
150+{
151+ BaseCourse::Run();
152+ if(750 <= counter)
153+ {
154+ changeNextState();
155+ }
156+ counter += 1;
157+}
158+
159+/**
160+ * @brief スピン
161+ * @note
162+ * その場でスピンする
163+ * 一定角度スピンしたら次のモードに移行
164+ */
165+void Stairs::Spin()
166+{
167+ BaseCourse::Run();
168+ //log("Spin %d, %d, %d, %d \r\n", wk_motor_ang_l, wk_motor_ang_r, motor_ang_l, motor_ang_r);
169+ if (660 < motor_ang_r - wk_motor_ang_r &&
170+ -660 > motor_ang_l - wk_motor_ang_l)
171+ {
172+ changeNextState();
173+ }
174+}
175+
176+/**
177+ * @brief ライン探索+前進
178+ * @note
179+ * ライン探索しながら前進する
180+ * 一定距離前進したらライン探索モード(後進)モードに移行
181+ * ラインを発見したらしたら段差検知モードに移行
182+ */
183+void Stairs::SearchLineForward()
184+{
185+ BaseCourse::Run();
186+
187+ if (90 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
188+ {
189+ changeNextState();
190+ }
191+ if (judgeColor(COLOR_LIST_COUNT_SEARCHLINE, (LIGHT_WHITE + LIGHT_BLACK) / 2))
192+ {
193+ changeNextState(2);
194+ }
195+}
196+
197+/**
198+ * @brief ライン探索+後進
199+ * @note
200+ * ライン探索しながら後進する
201+ * 一定距離後進したらライン探索モード(進行方向角度変更)モードに移行
202+ * ラインを発見したらしたら段差検知モードに移行
203+ */
204+void Stairs::SearchLineBackward()
205+{
206+ BaseCourse::Run();
207+
208+ if (-90 >= (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
209+ {
210+ changeNextState();
211+ }
212+ if (judgeColor(COLOR_LIST_COUNT_SEARCHLINE, (LIGHT_WHITE + LIGHT_BLACK) / 2))
213+ {
214+ changeNextState(1);
215+ }
216+}
217+
218+/**
219+ * @brief ライン探索+進行方向角度変更
220+ * @note
221+ * ライン探索しながら進行方向角度を変更する
222+ * 一定角度回転したらライン探索モード(前進)モードに移行
223+ * ラインを発見したらしたら段差検知モードに移行
224+ */
225+void Stairs::SearchLineChangeAngle()
226+{
227+ BaseCourse::Run();
228+// log("Spin %d, %d, %d, %d \r\n", wk_motor_ang_l, wk_motor_ang_r, motor_ang_l, motor_ang_r);
229+ if (20 < motor_ang_r - wk_motor_ang_r &&
230+ -20 > motor_ang_l - wk_motor_ang_l)
231+ {
232+ changeNextState(-3);
233+ }
234+ if (judgeColor(COLOR_LIST_COUNT_SEARCHLINE, (LIGHT_WHITE + LIGHT_BLACK) / 2))
235+ {
236+ changeNextState();
237+ }
238+}
239+
240+/**
241+ * @brief ライントレース走行
242+ * @note
243+ * 一定距離を走行する
244+ * 一定距離を走行したら次のモードに移行
245+ */
246+void Stairs::LineTrace()
247+{
248+ BaseCourse::Run();
249+ if (90 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
250+ {
251+ changeNextState();
252+ }
253+}
254+
255+/**
256+ * @brief 降段
257+ * @note
258+ * 一定距離を走行して段差を降りる
259+ * 段差を降りたら次のモードに移行
260+ */
261+void Stairs::StepDown()
262+{
263+ BaseCourse::Run();
264+ BaseCourse::Run();
265+ if (300 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
266+ {
267+ changeNextState();
268+ }
269+}
270+
271+/**
272+ * @brief ライントレース走行
273+ * @note
274+ * ライン復帰を行い復帰後、一定距離走行する
275+ * 一定距離走行したらガレージモードに移行
276+ */
277+void Stairs::TraceRun()
278+{
279+ BaseCourse::Run();
280+ // TODO
281+}
282+
283+/**
284+ * @brief PID値の設定(オーバーライド)
285+ * @note
286+ * 階段走行時のPID値を指定する
287+ *
288+ */
289+void Stairs::SetPIDData()
290+{
291+ // TODO updatePidValueでやらないと意味無いかも。要確認
292+ PIDMgr->SetFixedData(2.6, 0, 0.1);
293+}
294+
295+/**
296+ * @brief 各センサーからの値を取得(オーバーライド)
297+ * @note
298+ * ベース処理呼出 + ログ出力
299+ *
300+ */
301+void Stairs::updateSensorValue()
302+{
303+ BaseCourse::updateSensorValue();
304+ log("%d, %d, %d, %d, %f, %f, %d,\r\n", gyro, wk_motor_ang_LR, motor_ang_r, motor_ang_l, forward, turn, stairsState);
305+}
306+
307+/**
308+ * @brief PID関係の各値更新(オーバーライド)
309+ * @note
310+ * ベースメソッドの呼出のみ
311+ *
312+ */
313+void Stairs::updatePidValue() {
314+ BaseCourse::updatePidValue();
315+}
316+
317+/**
318+ * @brief turn値の限界値制御(オーバーライド)
319+ * @note
320+ * 階段走行時のモードにturn値を上書きする
321+ *  または限界値を指定する
322+ *
323+ */
324+void Stairs::normalizeTurnValue() {
325+ switch(stairsState)
326+ {
327+ case StairsState1stBackward:
328+ case StairsState1stStepUp:
329+ case StairsState1stStop:
330+ case StairsState1stSearchLineForward:
331+ case StairsState1stSearchLineBackward:
332+ case StairsState2ndBackward:
333+ case StairsState2ndStepUp:
334+ case StairsState2ndStop:
335+ case StairsState2ndSearchLineForward:
336+ case StairsState2ndSearchLineBackward:
337+ case StairsStateStepDown:
338+ case StairsStateAfterStepDownSearchLineForward:
339+ case StairsStateAfterStepDownSearchLineBackward:
340+ turn = 0;
341+ break;
342+ case StairsState1stSpin:
343+ case StairsState2ndSpin:
344+ turn = -50;
345+ break;
346+ case StairsState1stSearchLineChangeAngle:
347+ case StairsState2ndSearchLineChangeAngle:
348+ case StairsStateAfterStepDownSearchLineChangeAngle:
349+ turn = -5;
350+ case StairsState1stStepDetection:
351+ case StairsState2ndStepDetection:
352+ case StairsStateBeforeStepDownLineTrace:
353+// case StairsStateLineTraceRun:
354+ case StairsStateAfterStepDownLineTrace:
355+ // turn値の限界値制御
356+ if (turn > 30)
357+ {
358+ turn = 30;
359+ }
360+ else if (turn < -30)
361+ {
362+ turn = -30;
363+ }
364+ break;
365+ }
366+}
367+
368+/**
369+ * @brief forward値の設定(オーバーライド)
370+ * @note
371+ * 階段走行時のモードにforward値を上書きする
372+ *
373+ */
374+void Stairs::setForward()
375+{
376+ forward = 0;
377+ switch(stairsState)
378+ {
379+ case StairsState1stStepDetection:
380+ case StairsState2ndStepDetection:
381+ forward = 20;
382+ break;
383+ case StairsState1stBackward:
384+ case StairsState2ndBackward:
385+ forward = -70;
386+ break;
387+ case StairsState1stStepUp:
388+ case StairsState2ndStepUp:
389+ //forward = 50;
390+ forward = 70;
391+ break;
392+ case StairsState1stStop:
393+ case StairsState2ndStop:
394+ forward = 0;
395+ if (560 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
396+ {
397+ forward = -50;
398+ }
399+ if (550 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
400+ {
401+ forward = -40;
402+ }
403+ if (540 < (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
404+ {
405+ forward = -30;
406+ }
407+ if (520 > (motor_ang_l + motor_ang_r) - wk_motor_ang_LR)
408+ {
409+ forward = 10;
410+ }
411+ break;
412+ case StairsState1stSpin:
413+ case StairsState2ndSpin:
414+ forward = 0;
415+ break;
416+ case StairsState1stSearchLineForward:
417+ case StairsState2ndSearchLineForward:
418+ case StairsStateAfterStepDownSearchLineForward:
419+ forward = 20;
420+ break;
421+ case StairsState1stSearchLineBackward:
422+ case StairsState2ndSearchLineBackward:
423+ case StairsStateAfterStepDownSearchLineBackward:
424+ forward = -20;
425+ break;
426+ case StairsState1stSearchLineChangeAngle:
427+ case StairsState2ndSearchLineChangeAngle:
428+ case StairsStateAfterStepDownSearchLineChangeAngle:
429+ forward = 0;
430+ break;
431+ case StairsStateBeforeStepDownLineTrace:
432+ case StairsStateAfterStepDownLineTrace:
433+ forward = 20;
434+ break;
435+ case StairsStateStepDown:
436+ forward = 20;
437+ break;
438+// case StairsStateLineTraceRun:
439+// forward = 10;
440+// break;
441+ }
442+
443+}
444+
445+/**
446+ * @brief 階段走行モード移行処理
447+ * @note
448+ * 階段走行モードを次のステートに切り替えて、計算用の変数値をリセットする
449+ *
450+ */
451+void Stairs::changeNextState()
452+{
453+ // モード移行
454+ changeNextState(0);
455+}
456+
457+/**
458+ * @brief 階段走行モード移行処理
459+ * @note
460+ * 階段走行モードを次のステートに切り替えて、計算用の変数値をリセットする
461+ * さらに指定のスキップ数分モードをスキップする
462+ *
463+ */
464+void Stairs::changeNextState(int skip)
465+{
466+ // モード単位で利用する変数群の初期化
467+ variableInit();
468+
469+ if (stairsState != StairsStateAfterStepDownLineTrace)
470+ {
471+ // モード移行
472+ stairsState = static_cast<StairsState>(stairsState + 1 + skip);
473+ }
474+ else
475+ {
476+ // TODO
477+ // ガレージモードに移行
478+ }
479+}
480+
481+void Stairs::variableInit()
482+{
483+ // 変数群の初期化
484+ wk_motor_ang_LR = motor_ang_l + motor_ang_r;
485+ wk_motor_ang_l = motor_ang_l;
486+ wk_motor_ang_r = motor_ang_r;
487+ counter = 0;
488+ m_colorCensorList.clear();
489+}
490+
491+/**
492+ * @brief ライン判定
493+ * @note
494+ * 指定回数連続して指定色より大きいカラーセンサー値が検出された場合にTrueを返却する
495+ */
496+bool Stairs::judgeColor(unsigned int listSize, int color)
497+{ // カラー値の履歴管理
498+ if ((unsigned int)m_colorCensorList.size() >= listSize)
499+ {
500+ m_colorCensorList.erase(m_colorCensorList.begin());
501+ }
502+ m_colorCensorList.push_back(colorSensor);
503+
504+ bool find = false;
505+ if (m_colorCensorList.size() >= listSize)
506+ {
507+ find = true;
508+ for (unsigned int i = 0; i <= m_colorCensorList.size() - 1; i++ )
509+ {
510+ if (m_colorCensorList[i] >= color)
511+ {
512+ find = false;
513+ break;
514+ }
515+ }
516+ }
517+ return find;
518+}
519+
520+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stairs.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stairs.h (revision 409)
@@ -0,0 +1,95 @@
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+
14+class Stairs : public BaseCourse {
15+public:
16+ Stairs();
17+ virtual ~Stairs();
18+ void setForward();
19+ void updateSensorValue();
20+ void updatePidValue();
21+ void SetPIDData();
22+ void normalizeTurnValue();
23+ void Run();
24+ void StepDetection();
25+ void Backward();
26+ void StepUp();
27+ void Spin();
28+ void Stop();
29+ void SearchLineForward();
30+ void SearchLineBackward();
31+ void SearchLineChangeAngle();
32+ void LineTrace();
33+ void StepDown();
34+ void TraceRun();
35+ void changeNextState();
36+ void changeNextState(int skip);
37+ bool judgeColor(unsigned int listSize, int color);
38+ void variableInit();
39+
40+private:
41+ Stairs(const Stairs& orig){}
42+ private:
43+
44+ const int COLOR_LIST_COUNT_SEARCHLINE = 3; // ライン探索時のカラー値累積カウント
45+ const int STEP_DETECTION_GYRO = 105; // 段差検知とするジャイロ値
46+ const int STEP_UP_GYRO_OFFSET = 0; // 段差上昇時のジャイロオフセット値
47+ const int NORMAL_GYRO_OFFSET = 0; // 通常時のジャイロオフセット値
48+
49+ /**
50+ * @enum StairsState
51+ * 階段走行の各種モード
52+ */
53+ enum StairsState
54+ {
55+ StairsState1stStepDetection, // 0 1段目 段差検知モード
56+
57+ StairsState1stBackward, // 1 1段目 段差検知モード
58+
59+ StairsState1stStepUp, // 2 1段目 昇段モード
60+ StairsState1stStop, // 3 1段目 昇段後位置調整&静止モード
61+ StairsState1stSpin, // 4 1段目 スピンモード
62+ StairsState1stSearchLineForward, // 5 1段目 ライン探索モード(前進)モード
63+ StairsState1stSearchLineBackward, // 6 1段目 ライン探索モード(後進)モード
64+ StairsState1stSearchLineChangeAngle, // 7 1段目 ライン探索モード(進行方向角度変更)モード
65+ StairsState2ndStepDetection, // 8 2段目 段差検知モード
66+
67+ StairsState2ndBackward, // 9 2段目 段差検知モード
68+
69+
70+ StairsState2ndStepUp, // 10 2段目 昇段モード
71+ StairsState2ndStop, // 11 2段目 昇段後位置調整&静止モード
72+ StairsState2ndSpin, // 12 2段目 スピンモード
73+ StairsState2ndSearchLineForward, // 13 2段目 ライン探索モード(前進)モード
74+ StairsState2ndSearchLineBackward, // 14 2段目 ライン探索モード(後進)モード
75+ StairsState2ndSearchLineChangeAngle, // 15 2段目 ライン探索モード(進行方向角度変更)モード
76+ StairsStateBeforeStepDownLineTrace, // 16 降段前ライントレースモード
77+ StairsStateStepDown, // 17 降段モード
78+ StairsStateAfterStepDownSearchLineForward, // 18 降段後 ライン探索モード(前進)モード
79+ StairsStateAfterStepDownSearchLineBackward, // 19 降段後 ライン探索モード(後進)モード
80+ StairsStateAfterStepDownSearchLineChangeAngle, // 20 降段後 ライン探索モード(進行方向角度変更)モード
81+ StairsStateAfterStepDownLineTrace, // 21 降段後ライントレースモード
82+ };
83+
84+ StairsState stairsState; // 階段走行の各種モード
85+ int32_t wk_motor_ang_LR; // モード単位のR+Lモーター回転角度累積ワーク
86+ int32_t wk_motor_ang_r; // モード単位のRモーター回転角度累積ワーク
87+ int32_t wk_motor_ang_l; // モード単位のLモーター回転角度累積ワーク
88+ int counter; // カウンター
89+ std::vector<int> m_colorCensorList; // カラーセンサー値累積用リスト
90+
91+
92+
93+};
94+
95+#endif /* STAIRS_H */
\ No newline at end of file
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalL.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalL.cpp (revision 409)
@@ -0,0 +1,145 @@
1+#include "StartToGoalL.h"
2+
3+StartToGoalL::StartToGoalL() {
4+ mode = 0;
5+}
6+
7+StartToGoalL::~StartToGoalL() {
8+}
9+
10+void StartToGoalL::SetPIDData() {
11+ // PID制御に使用する定数を設定する
12+ if(mode == 0) {
13+ // ストレート
14+ PIDMgr->SetFixedData(0.5, 0, 0.03);
15+ }
16+ else if(mode == 1) {
17+ // Lコース 第1カーブ
18+ PIDMgr->SetFixedData(2, 6, 0.12);
19+ }
20+ else if(mode == 2) {
21+ // Lコース 第2カーブ
22+ PIDMgr->SetFixedData(0.7, 6, 0.13);
23+ }
24+ else if(mode == 3) {
25+ // Lコース 第3カーブ
26+ PIDMgr->SetFixedData(0.7, 1, 0.11);
27+ }
28+ else if(mode == 4) {
29+ // Lコース 第3カーブ
30+ PIDMgr->SetFixedData(2, 6, 0.12);
31+ }
32+ else if(mode == 99) {
33+ // Lコース 第3カーブ
34+ PIDMgr->SetFixedData(0.7, 0, 0.03);
35+ }
36+}
37+
38+bool StartToGoalL::IsNextCourse()
39+{
40+ // 現在モード
41+ int currentMode = mode;
42+ // 左右モータ総合回転角平均
43+ double totalAvg = (motor_ang_r + motor_ang_l) / 2;
44+ if(totalAvg < 2800) {
45+ // ストレート(スタート?第1カーブ前)
46+ mode = 0;
47+ }
48+ else if(totalAvg < 3100) {
49+ // ストレート(スピードダウン)
50+ mode = 99;
51+ }
52+ else if(totalAvg < 4500) {
53+ // 第1カーブ
54+ mode = 1;
55+ }
56+ else if(totalAvg < 5900) {
57+ // ストレート(第1カーブ後から第2カーブ前)
58+ mode = 0;
59+ }
60+ else if(totalAvg < 7100) {
61+ // 第2カーブ
62+ mode = 2;
63+ }
64+ else if(totalAvg < 7800) {
65+ // ストレート(第2カーブ後?第3カーブ前) 
66+ mode = 0;
67+ }
68+ else if(totalAvg < 8100) {
69+ // スピードダウン
70+ mode = 99;
71+ }
72+ else if (totalAvg < 10000) {
73+ // 第3カーブ
74+ mode = 3;
75+ }
76+ else if (totalAvg < 10500){
77+ // 第3カーブ後?ゴール(ストレート)
78+ mode = 4;
79+ }
80+ else if (totalAvg < 11500){
81+ // 第3カーブ後?ゴール(ストレート)
82+ mode = 0;
83+ }
84+ else {
85+ // ゴール 次の走行クラス(難所)へ切り替え
86+ return true;
87+ }
88+ // モードが変更された場合、現在の左右モータ総回転角値を退避する
89+ if (currentMode != mode) {
90+ tmpR = motor_ang_r;
91+ tmpL = motor_ang_l;
92+ }
93+
94+ log("%f, %d, %f, %d\r\n", totalAvg, mode, forward, volt);
95+ return false;
96+}
97+
98+void StartToGoalL::setForward() {
99+ if(mode == 0) {
100+ // 左右モータの回転角から徐々にスピードアップ
101+ double avg = ((motor_ang_r - tmpR) + (motor_ang_l - tmpL)) / 2;
102+ if(avg < 50) {
103+ if (forward <= 50) forward = 50;
104+ }
105+ else if(avg < 100) {
106+ if (forward <= 100) forward = 100;
107+ }
108+ else if(avg < 300) {
109+ if (forward <= 130) forward = 130;
110+ }
111+ else {
112+ forward = 170;
113+ }
114+ }
115+ if(mode == 1) {
116+ forward = 70;
117+ }
118+ else if(mode == 2) {
119+ forward = 110;
120+ }
121+ else if(mode == 3) {
122+ forward = 100;
123+ }
124+ else if(mode == 4) {
125+ forward = 70;
126+ }
127+ else if(mode == 99) {
128+ // 左右モータの回転角から徐々にスピードダウン
129+ double avg = ((motor_ang_r - tmpR) + (motor_ang_l - tmpL)) / 2;
130+ if(avg < 50) {
131+ if (forward >= 170) forward = 170;
132+ }
133+ else if(avg < 100) {
134+ if (forward >= 130) forward = 130;
135+ }
136+ else if(avg < 200){
137+ if (forward >= 100) forward = 100;
138+ }
139+ else{
140+ forward = 80;
141+ }
142+ }
143+}
144+
145+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalL.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalL.h (revision 409)
@@ -0,0 +1,19 @@
1+#ifndef STARTTOGOALL_H
2+#define STARTTOGOALL_H
3+
4+#include "BaseCourse.h"
5+
6+class StartToGoalL : public BaseCourse{
7+public:
8+ StartToGoalL();
9+ virtual ~StartToGoalL();
10+
11+ void SetPIDData();
12+ bool IsNextCourse();
13+ void setForward();
14+private:
15+ int mode = 0;
16+};
17+
18+#endif
19+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalR.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalR.cpp (revision 409)
@@ -0,0 +1,159 @@
1+#include "StartToGoalR.h"
2+
3+StartToGoalR::StartToGoalR() {
4+ mode = 0;
5+}
6+
7+StartToGoalR::~StartToGoalR() {
8+}
9+
10+void StartToGoalR::SetPIDData() {
11+ // PID制御に使用する定数を設定する
12+ if(mode == 0) {
13+ // ストレート
14+ PIDMgr->SetFixedData(0.5, 0, 0.03);
15+ }
16+ else if(mode == 1) {
17+ // Rコース 第1カーブ
18+ PIDMgr->SetFixedData(2, 6, 0.08);
19+ }
20+ else if(mode == 2) {
21+ // Rコース 第2カーブ
22+ PIDMgr->SetFixedData(2.6, 7, 0.1);
23+ }
24+ else if(mode == 3) {
25+ // Rコース 第3カーブ
26+ PIDMgr->SetFixedData(1.18, 6, 0.15);
27+ }
28+ else if(mode == 4) {
29+ // Rコース 第2カーブ
30+ PIDMgr->SetFixedData(0.8, 1, 0.04);
31+ }
32+ else if(mode == 99) {
33+ // スピードダウン
34+ PIDMgr->SetFixedData(0.7, 1, 0.03);
35+ }
36+}
37+
38+bool StartToGoalR::IsNextCourse()
39+{
40+ // 現在モード
41+ int currentMode = mode;
42+ // 左右モータ総合回転角平均
43+ double totalAvg = (motor_ang_r + WK + motor_ang_l + WK) / 2;
44+ if(totalAvg < 2900) {
45+ // ストレート(スタート?第1カーブ前)
46+ mode = 0;
47+ }
48+ else if(totalAvg < 3100) {
49+ // スピードダウン
50+ mode = 99;
51+ }
52+ else if(totalAvg < 4100) {
53+ // 第1カーブ
54+ mode = 1;
55+ }
56+ else if(totalAvg < 5900) {
57+ // ストレート(第1カーブ後から第2カーブ前)
58+ mode = 0;
59+ }
60+ else if(totalAvg < 6500) {
61+ // スピードダウン
62+ mode = 99;
63+ }
64+// else if(totalAvg < 7000) {
65+// // 第2カーブ(始め部分)
66+// mode = 2;
67+// }
68+// else if(totalAvg < 7430) {
69+// // 第2カーブ(中間部分)
70+// mode = 4;
71+// }
72+ else if(totalAvg < 7800) {
73+ // 第2カーブ(終わり部分)
74+ mode = 2;
75+ }
76+ else if(totalAvg < 8800) {
77+ // ストレート(第2カーブ後から第3カーブ前) 
78+ mode = 0;
79+ }
80+ else if(totalAvg < 9100) {
81+ // スピードダウン 
82+ mode = 99;
83+ }
84+ else if (totalAvg < 10700) {
85+ // 第3カーブ
86+ mode = 3;
87+ }
88+ else if (totalAvg < 12000){
89+ // ストレート(第3カーブ後からゴール)
90+ mode = 0;
91+ }
92+ else {
93+ // 完了 次の走行クラス(難所)へ切り替え
94+ return true;
95+ }
96+
97+ // モードが変更された場合、現在の左右モータ総回転角値を退避する
98+ if (currentMode != mode) {
99+ tmpR = motor_ang_r + WK;
100+ tmpL = motor_ang_l + WK;
101+ }
102+
103+ log("%f, %d, %f, %d\r\n", totalAvg, mode, forward, volt);
104+ return false;
105+}
106+
107+void StartToGoalR::setForward() {
108+ if(mode == 0) {
109+ // ストレート
110+ // 左右モータの回転角から徐々にスピードアップ
111+ double avg = ((motor_ang_r + WK - tmpR) + (motor_ang_l + WK - tmpL)) / 2;
112+ if(avg < 50) {
113+ if (forward <= 50) forward = 50;
114+ }
115+ else if(avg < 100) {
116+ if (forward <= 100) forward = 100;
117+ }
118+ else if(avg < 300) {
119+ if (forward <= 130) forward = 130;
120+ }
121+ else {
122+ forward = 150;
123+ }
124+ }
125+ else if(mode == 1) {
126+ // 第1カーブ
127+ forward = 70;
128+ }
129+ else if(mode == 2) {
130+ // 第2カーブ
131+ forward = 55;
132+ }
133+ else if(mode == 3) {
134+ // 第3カーブ
135+ forward = 100;
136+ }
137+ else if(mode == 4) {
138+ // 第2カーブ中間部分
139+ forward = 60;
140+ }
141+ else if(mode == 99) {
142+ // 左右モータの回転角から徐々にスピードダウン
143+ double avg = ((motor_ang_r - tmpR) + (motor_ang_l - tmpL)) / 2;
144+ if(avg < 50) {
145+ if (forward >= 150) forward = 150;
146+ }
147+ else if(avg < 100) {
148+ if (forward >= 130) forward = 130;
149+ }
150+ else if(avg < 200){
151+ if (forward >= 100) forward = 100;
152+ }
153+ else{
154+ forward = 80;
155+ }
156+ }
157+}
158+
159+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalR.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/StartToGoalR.h (revision 409)
@@ -0,0 +1,21 @@
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+};
19+
20+#endif
21+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stopper.cpp (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stopper.cpp (revision 409)
@@ -0,0 +1,79 @@
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\r\n");
22+
23+ // 停止と尻尾下し
24+ Run();
25+ m_tailDownCompleted = TailDown();
26+ }
27+ else
28+ {
29+ log("route2\r\n");
30+
31+ // 尻尾に車体を預ける
32+ if(m_tailMountCount < 25)
33+ {
34+ ev3_motor_set_power(right_motor, 50);
35+ ev3_motor_set_power(left_motor, 50);
36+ ++m_tailMountCount;
37+ }
38+ else if(m_tailMountCount < 250)
39+ {
40+ ev3_motor_set_power(right_motor, 0);
41+ ev3_motor_set_power(left_motor, 0);
42+ ev3_motor_set_power(tail_motor, 0);
43+ ++m_tailMountCount;
44+ }
45+ else
46+ {
47+ m_tailDownCompleted = false;
48+ m_tailMountCount = 0;
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+ log("angle:%d, ", (int)ev3_motor_get_counts(tail_motor));
75+
76+ const float targetAngle = 92.0F;
77+
78+ return CommonApp::tail_control(targetAngle);
79+}
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stopper.h (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/course/Stopper.h (revision 409)
@@ -0,0 +1,25 @@
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+};
23+
24+#endif /* STOPPER_H */
25+
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/configurations.xml (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/configurations.xml (revision 409)
@@ -0,0 +1,126 @@
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>LookupGate.cpp</in>
13+ <in>LookupGate.h</in>
14+ <in>Stairs.cpp</in>
15+ <in>Stairs.h</in>
16+ <in>StartToGoalL.cpp</in>
17+ <in>StartToGoalL.h</in>
18+ <in>StartToGoalR.cpp</in>
19+ <in>StartToGoalR.h</in>
20+ <in>Stopper.cpp</in>
21+ <in>Stopper.h</in>
22+ </df>
23+ <in>BaseCourse.cpp</in>
24+ <in>BaseCourse.h</in>
25+ <in>CommonApp.cpp</in>
26+ <in>CommonApp.h</in>
27+ <in>PIDDataManager.cpp</in>
28+ <in>PIDDataManager.h</in>
29+ <in>app.cpp</in>
30+ </df>
31+ <logicalFolder name="ExternalFiles"
32+ displayName="重要なファイル"
33+ projectFiles="false"
34+ kind="IMPORTANT_FILES_FOLDER">
35+ <itemPath>../Makefile</itemPath>
36+ </logicalFolder>
37+ </logicalFolder>
38+ <sourceFolderFilter>^(nbproject)$</sourceFolderFilter>
39+ <sourceRootList>
40+ <Elem>.</Elem>
41+ </sourceRootList>
42+ <projectmakefile>../Makefile</projectmakefile>
43+ <confs>
44+ <conf name="Default" type="0">
45+ <toolsSet>
46+ <compilerSet>MinGW|MinGW</compilerSet>
47+ <dependencyChecking>false</dependencyChecking>
48+ <rebuildPropChanged>false</rebuildPropChanged>
49+ </toolsSet>
50+ <codeAssistance>
51+ </codeAssistance>
52+ <makefileType>
53+ <makeTool>
54+ <buildCommandWorkingDir>..</buildCommandWorkingDir>
55+ <buildCommand>${MAKE} -f Makefile mod=calib_cpp PWD=${CURDIR}</buildCommand>
56+ <cleanCommand>${MAKE} -f Makefile clean</cleanCommand>
57+ <executablePath></executablePath>
58+ <cTool>
59+ <incDir>
60+ <pElem>../common/lib/libcpp-ev3/include</pElem>
61+ <pElem>../include</pElem>
62+ <pElem>../../target/ev3_gcc</pElem>
63+ <pElem>../../target/ev3_gcc/api/include</pElem>
64+ <pElem>../../include</pElem>
65+ <pElem>../../arch</pElem>
66+ <pElem>../../arch/arm_gcc/am1808</pElem>
67+ </incDir>
68+ </cTool>
69+ <ccTool>
70+ <incDir>
71+ <pElem>../common/lib/libcpp-ev3/include</pElem>
72+ <pElem>../include</pElem>
73+ <pElem>../../target/ev3_gcc</pElem>
74+ <pElem>../../target/ev3_gcc/api/include</pElem>
75+ <pElem>../../include</pElem>
76+ <pElem>../../arch</pElem>
77+ <pElem>../../arch/arm_gcc/am1808</pElem>
78+ </incDir>
79+ </ccTool>
80+ </makeTool>
81+ </makefileType>
82+ <item path="BaseCourse.cpp" ex="false" tool="1" flavor2="0">
83+ </item>
84+ <item path="BaseCourse.h" ex="false" tool="3" flavor2="0">
85+ </item>
86+ <item path="CommonApp.cpp" ex="false" tool="1" flavor2="0">
87+ </item>
88+ <item path="CommonApp.h" ex="false" tool="3" flavor2="0">
89+ </item>
90+ <item path="PIDDataManager.cpp" ex="false" tool="1" flavor2="0">
91+ </item>
92+ <item path="PIDDataManager.h" ex="false" tool="3" flavor2="0">
93+ </item>
94+ <item path="app.cpp" ex="false" tool="1" flavor2="0">
95+ </item>
96+ <item path="balancer/balancer.c" ex="false" tool="0" flavor2="2">
97+ </item>
98+ <item path="balancer/balancer_param.c" ex="false" tool="0" flavor2="2">
99+ </item>
100+ <item path="course/Garage.cpp" ex="false" tool="1" flavor2="0">
101+ </item>
102+ <item path="course/Garage.h" ex="false" tool="3" flavor2="0">
103+ </item>
104+ <item path="course/LookupGate.cpp" ex="false" tool="1" flavor2="0">
105+ </item>
106+ <item path="course/LookupGate.h" ex="false" tool="3" flavor2="0">
107+ </item>
108+ <item path="course/Stairs.cpp" ex="false" tool="1" flavor2="0">
109+ </item>
110+ <item path="course/Stairs.h" ex="false" tool="3" flavor2="0">
111+ </item>
112+ <item path="course/StartToGoalL.cpp" ex="false" tool="1" flavor2="0">
113+ </item>
114+ <item path="course/StartToGoalL.h" ex="false" tool="3" flavor2="0">
115+ </item>
116+ <item path="course/StartToGoalR.cpp" ex="false" tool="1" flavor2="0">
117+ </item>
118+ <item path="course/StartToGoalR.h" ex="false" tool="3" flavor2="0">
119+ </item>
120+ <item path="course/Stopper.cpp" ex="false" tool="1" flavor2="0">
121+ </item>
122+ <item path="course/Stopper.h" ex="false" tool="3" flavor2="0">
123+ </item>
124+ </conf>
125+ </confs>
126+</configurationDescriptor>
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/private/configurations.xml (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/private/configurations.xml (revision 409)
@@ -0,0 +1,83 @@
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.h</in>
8+ <in>balancer_param.c</in>
9+ <in>balancer_private.h</in>
10+ </df>
11+ <df name="course">
12+ <in>Garage.cpp</in>
13+ <in>Garage.h</in>
14+ <in>LookupGate.cpp</in>
15+ <in>LookupGate.h</in>
16+ <in>Stairs.cpp</in>
17+ <in>Stairs.h</in>
18+ <in>StartToGoalL.cpp</in>
19+ <in>StartToGoalL.h</in>
20+ <in>StartToGoalR.cpp</in>
21+ <in>StartToGoalR.h</in>
22+ <in>Stopper.cpp</in>
23+ <in>Stopper.h</in>
24+ </df>
25+ <in>BaseCourse.cpp</in>
26+ <in>BaseCourse.h</in>
27+ <in>CommonApp.cpp</in>
28+ <in>CommonApp.h</in>
29+ <in>PIDDataManager.cpp</in>
30+ <in>PIDDataManager.h</in>
31+ <in>app.cpp</in>
32+ <in>app.h</in>
33+ </df>
34+ </logicalFolder>
35+ <projectmakefile>../Makefile</projectmakefile>
36+ <confs>
37+ <conf name="Default" type="0">
38+ <toolsSet>
39+ <developmentServer>localhost</developmentServer>
40+ <platform>3</platform>
41+ </toolsSet>
42+ <compile>
43+ <compiledirpicklist>
44+ <compiledirpicklistitem>.</compiledirpicklistitem>
45+ <compiledirpicklistitem>${AUTO_FOLDER}</compiledirpicklistitem>
46+ </compiledirpicklist>
47+ <compiledir>${AUTO_FOLDER}</compiledir>
48+ <compilecommandpicklist>
49+ <compilecommandpicklistitem>${MAKE} ${ITEM_NAME}.o</compilecommandpicklistitem>
50+ <compilecommandpicklistitem>${AUTO_COMPILE}</compilecommandpicklistitem>
51+ </compilecommandpicklist>
52+ <compilecommand>${AUTO_COMPILE}</compilecommand>
53+ </compile>
54+ <dbx_gdbdebugger version="1">
55+ <gdb_pathmaps>
56+ </gdb_pathmaps>
57+ <gdb_interceptlist>
58+ <gdbinterceptoptions gdb_all="false" gdb_unhandled="true" gdb_unexpected="true"/>
59+ </gdb_interceptlist>
60+ <gdb_options>
61+ <DebugOptions>
62+ </DebugOptions>
63+ </gdb_options>
64+ <gdb_buildfirst gdb_buildfirst_overriden="false" gdb_buildfirst_old="false"/>
65+ </dbx_gdbdebugger>
66+ <nativedebugger version="1">
67+ <engine>gdb</engine>
68+ </nativedebugger>
69+ <runprofile version="9">
70+ <runcommandpicklist>
71+ <runcommandpicklistitem>"${OUTPUT_PATH}"</runcommandpicklistitem>
72+ </runcommandpicklist>
73+ <runcommand>"${OUTPUT_PATH}"</runcommand>
74+ <rundir></rundir>
75+ <buildfirst>false</buildfirst>
76+ <terminal-type>0</terminal-type>
77+ <remove-instrumentation>0</remove-instrumentation>
78+ <environment>
79+ </environment>
80+ </runprofile>
81+ </conf>
82+ </confs>
83+</configurationDescriptor>
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/private/private.xml (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/private/private.xml (revision 409)
@@ -0,0 +1,11 @@
1+<?xml version="1.0" encoding="UTF-8"?>
2+<project-private xmlns="http://www.netbeans.org/ns/project-private/1">
3+ <data xmlns="http://www.netbeans.org/ns/make-project-private/1">
4+ <activeConfTypeElem>0</activeConfTypeElem>
5+ <activeConfIndexElem>0</activeConfIndexElem>
6+ </data>
7+ <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
8+ <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
9+ <group/>
10+ </open-files>
11+</project-private>
--- trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/project.xml (nonexistent)
+++ trunk/src/hrp2_beta7/sdk/workspace/calib_cpp/nbproject/project.xml (revision 409)
@@ -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>sample_cpp2</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>
Show on old repository browser