LAURUS
output only raw data (acceleration, anguler rate, geomagnetism, air pressure)
Dependencies: mbed SDFileSystem ConfigFile
Revision 34:4bda9af9a0cd, committed 2015-08-06
- Comitter:
- ojan
- Date:
- Thu Aug 06 15:45:48 2015 +0000
- Parent:
- 33:56163d4e2c53
- Commit message:
- LAURUS_Program_v2.9.0; ; + add RULE3_1; + add "volatile" to some variables like "flag"
Changed in this revision
| BufferedSerial/BufferedSerial.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 56163d4e2c53 -r 4bda9af9a0cd BufferedSerial/BufferedSerial.cpp
--- a/BufferedSerial/BufferedSerial.cpp Sun Jul 19 02:21:43 2015 +0000
+++ b/BufferedSerial/BufferedSerial.cpp Thu Aug 06 15:45:48 2015 +0000
@@ -159,7 +159,7 @@
bool BufferedSerial::checkCTS(void)
{
- static bool _send_flg=true;
+ volatile static bool _send_flg=true;
if(_send_flg) {
_send_flg=!_cts;
} else {
diff -r 56163d4e2c53 -r 4bda9af9a0cd main.cpp
--- a/main.cpp Sun Jul 19 02:21:43 2015 +0000
+++ b/main.cpp Thu Aug 06 15:45:48 2015 +0000
@@ -16,19 +16,30 @@
//#define RULE2
//#define RULE3
//#define RULE4
+//#define RULE3_1
#define SERVO_DEBUG
//#define DIRECTION_DEBUG
//#define SPIRAL_DEBUG
#ifdef DIRECTION_DEBUG
-const float TargetDirection = 0.0f; // 指定方向に飛ぶ
+const float TargetDirection = -179.0f; // 指定方向に飛ぶ
+
+#elif RULE3_1
+const float BetaMin = 0.2f * RAD_TO_DEG; // β最小値
+const float BetaVar = 0.3f * RAD_TO_DEG; // β変化量
+const float BetaMax = 1.0f * RAD_TO_DEG; // β最大値
+const float Span = 10.0f; // βの値が変わる距離間隔（Span[m]ごとにβの値がBetaVarだけ変化）
+
+#elif RULE4
+const float BorderHeight = 10.0f; // 制御則3と2を切り替える高度の閾値(m)
#endif
+
const float dt = 0.01f; // 割り込み周期(s)
const float ServoMax = 0.0046f; // サーボの最大パルス長(s)
const float ServoMin = 0.0012f; // サーボの最小パルス長(s)
const float Stabilize = 0.0f; // 滑空安定時の引っ張り量
const float Turn = 25.0f; // 旋回時の引っ張り量
-const float PullRate = 5.0f; // 紐の引っ張り速度
+const float PullRate = 7.5f; // 紐の引っ張り速度
const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
const float BorderSpiral = 40.0f; // スパイラル検知角度
const short BorderOpt = 30000; // 光センサーの閾値
@@ -39,7 +50,6 @@
const float Alpha = 30.0f; // 目標方向と自分の進行方向との差の閾値(deg)（制御則1&2&3の定数
const float Beta = 60.0f; // 目標方向と自分の進行方向との間に取るべき角度差(deg)（制御則3の定数
const float BorderDistance = 5.0f; // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
-const float BorderHeight = 10.0f; // 制御則3と2を切り替える高度の閾値(m)
enum Direction {LEFT, RIGHT, NEUTRAL};
@@ -67,8 +77,8 @@
Timer timer; // 時間計測用タイマー
Direction dir = NEUTRAL; // 旋回方向
-int lps_cnt = 0; // 気圧センサ読み取りカウント
-bool INT_flag = true; // 割り込み可否フラグ
+volatile int lps_cnt = 0; // 気圧センサ読み取りカウント
+volatile bool INT_flag = true; // 割り込み可否フラグ
/* こちらの変数群はメインループでは参照しない */
Vector raw_acc(3); // 加速度(m/s^2) 生
Vector raw_gyro(3); // 角速度(deg/s) 生
@@ -102,8 +112,8 @@
Vector r_u(3); // 世界座標に固定された、上向きのベクトル（Z軸）
Vector r_l(3); // 世界座標に固定された、西向きのベクトル（Y軸）
-float pull_L = 0.0f; // 左サーボの引っ張り量（目標値）(mm：0~PullMax)
-float pull_R = 0.0f; // 右サーボの引っ張り量（目標値）(mm：0~PullMax)
+float pull_L = 10.0f; // 左サーボの引っ張り量（目標値）(mm：0~PullMax)
+float pull_R = 10.0f; // 右サーボの引っ張り量（目標値）(mm：0~PullMax)
float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
@@ -111,12 +121,15 @@
float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
-int step = 0; // シーケンス制御のステップ
+volatile int step = 0; // シーケンス制御のステップ
char data[512] = {}; // 送信データ用配
int loopTime = 0; // 1ループに掛かる時間（デバッグ用
float sv = 0.0f; // サーボ電源電圧
float lv = 0.0f; // ロジック電源電圧
+#ifdef RULE3_1
+float beta = 0.0f; // β変数 （制御則3_1で使用
+#endif
/** config.txt **
* #から始めるのはコメント行
* #イコールの前後に空白を入れない
@@ -242,13 +255,17 @@
if(xbee.readable()) {
char cmd = xbee.getc();
if(cmd == 'w') {
- pull_R += 5.0f;
+ pull_R = 20.0f;
+ pull_L = 0.0f;
} else if(cmd == 's'){
- pull_R -= 5.0f;
+ pull_R = 10.0f;
+ pull_L = 10.0f;
} else if(cmd == 'a'){
- pull_L += 5.0f;
+ pull_R = 0.0f;
+ pull_L = 20.0f;
} else if(cmd == 'd'){
- pull_L -= 5.0f;
+ pull_R = 10.0f;
+ pull_L = 10.0f;
}
}
#else
@@ -261,7 +278,7 @@
// データをmicroSDに保存し、XBeeでPCへ送信する
- sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
+ sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %.1f,%.1f\r\n",
UTC_t, ss, yaw, pitch, roll,
press, gms.longitude, gms.latitude,
vrt_acc, height, Distance(target_p, p),
@@ -355,7 +372,7 @@
*/
void ControlRoutine()
{
- static int count = 0;
+ volatile static int count = 0;
static float target_lng;
static float target_lat;
static float target_X;
@@ -416,6 +433,16 @@
if(theta > 180.0f) theta -= 360.0f;
else if(theta < -180.0f) theta += 360.0f;
+#elif RULE3_1
+ float d = Distance(target_p, p);
+ beta = BetaMax - BetaVar * (int)(d / Span);
+ if(beta < BetaMin) beta = BetaMin;
+
+ theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG + beta;
+
+ if(theta > 180.0f) theta -= 360.0f;
+ else if(theta < -180.0f) theta += 360.0f;
+
#else
theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG;
#endif
@@ -787,9 +814,9 @@
*/
bool Thrown()
{
- static int opt_count = 0;
- static int g_count = 0;
- static int para_count = 0;
+ volatile static int opt_count = 0;
+ volatile static int g_count = 0;
+ volatile static int para_count = 0;
#ifndef SPIRAL_DEBUG
/*if(optSensor.read_u16() > BorderOpt) opt_count++;
@@ -816,8 +843,8 @@
void DataUpdate()
{
// サーボ用変数（引き量）
- static float pL = pull_L;
- static float pR = pull_R;
+ static float pL = 0.0f;
+ static float pR = 0.0f;
// 引き量が目標値から一定以上離れていれば、引き量を一定の速度で更新する
if(pull_L - pL > PullRate * dt) {