siqiang pan
2
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 49:7224132f4b0e
- Parent:
- 48:3003ea51c619
- Child:
- 50:ee78382fd399
--- a/main.cpp Mon Aug 31 01:22:48 2020 +0000
+++ b/main.cpp Wed Sep 02 05:35:17 2020 +0000
@@ -21,29 +21,53 @@
#define FAST 1 // 速い
#define VERYFAST 2 // とても速い
-#define SLOW 0.8
+
+
+/* // 右回り37.21[s]
+#define S 0.9
+#define VF 1.2
#define REVERSE 0.5
#define MBED04 0.781//Mbed04号機の左右のモーター速度比(R:L = 1: 0.781)
#define MBED05 0.953//Mbed05号機の左右のモーター速度比(R:L = 1: 0.953)
-#define MSR0 0.4
-#define MSR1 0.5
-#define MSR2 0.6
-#define MSR3 MSR0*SLOW
-#define MSR4 MSR1*SLOW
-#define MSR5 MSR2*SLOW
-#define MSR6 0.7
-#define MSR7 0.8
-#define MSR8 0.9
-#define MSL0 MSR0*MBED05
-#define MSL1 MSR1*MBED05
-#define MSL2 MSR2*MBED05
-#define MSL3 MSR3*MBED05
-#define MSL4 MSR4*MBED05
-#define MSL5 MSR5*MBED05
-#define MSL6 MSR6*MBED05
-#define MSL7 MSR7*MBED05
-#define MSL8 MSR8*MBED05
+#define MBED07 1
+// MSR = Motor Speed Right
+// MSL = Motor Speed Left
+#define MSR0 0.5 // 基準速度Normal
+#define MSR1 0.6 // 基準速度Fast
+#define MSR2 0.7 // 基準速度VeryFast
+*/
+
+
+
+#define S 0.9
+#define VF 1
+#define REVERSE 0.5
+
+#define MBED04 0.781 //Mbed04号機の左右のモーター速度比(R:L = 1: 0.781)
+#define MBED05 0.953 //Mbed05号機の左右のモーター速度比(R:L = 1: 0.953)
+#define MBED07 1
+// MSR = Motor Speed Right
+// MSL = Motor Speed Left
+#define MSR0 0.55 // 基準速度Normal
+#define MSR1 0.6 // 基準速度Fast
+#define MSR2 0.65 // 基準速度VeryFast
+
+#define MSR3 MSR0*S // 低速旋回(Normalの時)
+#define MSR4 MSR1*S*S // 低速旋回(Fastの時)
+#define MSR5 MSR2*S*S*S // 低速旋回(VeryFastの時)
+#define MSR6 MSR0*VF // 高速(Normalの時)
+#define MSR7 MSR1*VF // 高速(Fastの時)
+#define MSR8 MSR2*VF // 高速(VeryFastの時)
+#define MSL0 MSR0*MBED07
+#define MSL1 MSR1*MBED07
+#define MSL2 MSR2*MBED07
+#define MSL3 MSR3*MBED07
+#define MSL4 MSR4*MBED07
+#define MSL5 MSR5*MBED07
+#define MSL6 MSR6*MBED07
+#define MSL7 MSR7*MBED07
+#define MSL8 MSR8*MBED07
/* 操作モード定義 */
enum MODE{
@@ -56,6 +80,8 @@
LINE_TRACE, // 6：ライントレース
AVOIDANCE, // 7：障害物回避
SPEED, // 8：スピード制御
+ LT_R, // 9:低速右折(ライントレース時)
+ LT_L // 10:低速左折(ライントレース時)
};
/* ピン配置 */
@@ -111,9 +137,9 @@
int flag_t = 0; // バックライトタイマーフラグ
/* trace用変数 */
-int sensArray[32] = {0,6,2,4,1,2,1,4, // ライントレースセンサパターン
- 3,6,1,6,1,1,1,6, // 0:前回動作継続
- 7,1,7,1,3,1,1,1, // 1:高速前進
+int sensArray[32] = {0,6,2,4,1,4,2,4, // ライントレースセンサパターン
+ 3,4,1,6,3,1,1,6, // 0:前回動作継続
+ 7,1,5,1,5,1,1,1, // 1:高速前進
5,1,7,1,5,1,7,1}; // 2:低速右折
// 3:低速左折
// 4:中速右折
@@ -335,6 +361,20 @@
motorL1 = LOW; // 左前進モーターOFF
motorL2 = LOW; // 左後退モーターOFF
break;
+ /* 低速右折 */
+ case LT_R:
+ motorR1 = motorSpeedR2[flag_sp]; // 右前進モーターON
+ motorR2 = LOW; // 右後退モーターOFF
+ motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
+ motorL2 = LOW; // 左後退モーターOFF
+ break;
+ /* 低速左折 */
+ case LT_L:
+ motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
+ motorR2 = LOW; // 右後退モーターOFF
+ motorL1 = motorSpeedL2[flag_sp]; // 左前進モーターON
+ motorL2 = LOW; // 左後退モーターOFF
+ break;
}
if(flag_sp > VERYFAST){ // スピード変更フラグが2より大きいなら
flag_sp %= 3; // スピード変更フラグ調整
@@ -353,6 +393,12 @@
}
/* ライントレーススレッド */
+//void trace(){
+// // PID用
+// while(1){
+// ThisThread::sleep_for(3);
+// }
+//}
void trace(){
while(1){
/* 各センサー値読み取り */
@@ -361,7 +407,6 @@
int sensor3 = ss3;
int sensor4 = ss4;
int sensor5 = ss5;
- //pc.printf("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5);
int sensD = 0;
/* センサー値の決定 */
@@ -381,12 +426,14 @@
case 2:
flag_sp = flag_sp % 3 + 3;
beforeRun = run;
- run = RIGHT; // 低速で右折
+// run = RIGHT; // 低速で右折
+ run = LT_R; // 低速で右折
break;
case 3:
flag_sp = flag_sp % 3 + 3;
beforeRun = run;
- run = LEFT; // 低速で左折
+// run = LEFT; // 低速で左折
+ run = LT_L; // 低速で左折
break;
case 4:
flag_sp = flag_sp % 3;
@@ -786,10 +833,7 @@
esp.attach(&callback);
servreq=0;
}
- run = beforeRun;
ThisThread::sleep_for(100);
- beforeRun = run;
- run = STOP;
}
}
// Static WEB page
@@ -1065,10 +1109,7 @@
//****
//output at command when 2000
if(((i%2047)==0) && (i>0)) {
- run = beforeRun;
ThisThread::sleep_for(100);
- beforeRun = run;
- run = STOP;
sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl-2048*(i/2047))>2048?2048:(bufl-2048*(i/2047))); // send IPD link channel and buffer character length.
timeout=600;
getcount=50;
@@ -1131,10 +1172,7 @@
// Reads and processes GET and POST web data
void ReadWebData()
{
- run = beforeRun;
ThisThread::sleep_for(200);
- beforeRun = run;
- run = STOP;
esp.attach(NULL);
ount=0;
DataRX=0;