siqiang pan
2
Dependencies: RemoteIR TextLCD
Revision 73:8ce014cb07f1, committed 2020-09-28
- Comitter:
- faker_71
- Date:
- Mon Sep 28 00:52:36 2020 +0000
- Parent:
- 72:d389adc0c780
- Commit message:
- 0928
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r d389adc0c780 -r 8ce014cb07f1 main.cpp
--- a/main.cpp Fri Sep 11 07:51:34 2020 +0000
+++ b/main.cpp Mon Sep 28 00:52:36 2020 +0000
@@ -227,6 +227,7 @@
void changeSpeed();
void avoidance(/*void const *argument*/);
void trace(/*void const *argument*/);
+void trace_avoid();
void watchsurrounding3();
void watchsurrounding5();
int watch();
@@ -235,9 +236,9 @@
void display();
void lcdBacklight(void const *argument);
void SendCMD(),getreply(),ReadWebData(),startserver(),sendpage(),SendWEB(),sendcheck();
-void wifi(/*void const *argument*/);
+//void wifi(/*void const *argument*/);
Thread *deco_thread; // decodeIRをスレッド化 ：+3
-Thread *wifi_thread;
+//Thread *wifi_thread;
Thread *motor_thread; // motorをスレッド化 ：+2
RtosTimer bTimer(lcdBacklight, osTimerPeriodic); // lcdBacklightをタイマー割り込みで設定
Thread *avoi_thread; // avoidanceをスレッド化：+2
@@ -253,7 +254,7 @@
deco_thread -> set_priority(osPriorityRealtime);
motor_thread = new Thread(motor);
motor_thread -> set_priority(osPriorityHigh);
- wifi_thread -> set_priority(osPriorityRealtime);
+ //wifi_thread -> set_priority(osPriorityRealtime);
display();
}
@@ -480,8 +481,147 @@
// ThisThread::sleep_for(3);
// }
//}
+
+void tran_RL(int tr){
+ if (tr==0){
+ //left
+ motorR1 = 0.2; // 右前進モーターON
+ motorR2 = 0; // 右後退モーターOFF
+ motorL1 = 0; // 左前進モーターON
+ motorL2 = 0.2; // 左後退モーターOFF
+ }else if(tr==3){
+ motorR1 = 0; // 右前進モーターON
+ motorR2 = 0; // 右後退モーターOFF
+ motorL1 = 0; // 左前進モーターON
+ motorL2 = 0; // 左後退モーターOFF
+ }else{
+ //right
+ motorR1 = 0; // 右前進モーターON
+ motorR2 = 0.2; // 右後退モーターOFF
+ motorL1 = 0.2; // 左前進モーターON
+ motorL2 = 0; // 左後退モーターOFF
+ }
+}
+
+
+int fond=0;
+int DistanceMid,DistanceRM,DistanceLM,nowDistance;
+int turn;//0 left 1 right
+int mid_po=1400;
+//0917
+void trace_avoid(){
+ run=100;
+ //servo.pulsewidth_us(1450);
+ //ThisThread::sleep_for(100);
+ DistanceMid = watch();
+ if ((DistanceMid<20) && (DistanceMid>0)){
+ //run = STOP;
+ tran_RL(3);
+ led1!=led1;
+ led2!=led2;
+ led3!=led3;
+ //左前,右前
+ servo.pulsewidth_us(mid_po+300);
+ ThisThread::sleep_for(300);
+ DistanceLM=watch();
+ servo.pulsewidth_us(mid_po-300);//右qian
+ ThisThread::sleep_for(300);
+ DistanceRM=watch();
+ if(DistanceRM>DistanceLM){
+
+ //go through left side
+ turn=1;
+ servo.pulsewidth_us(mid_po+800);
+ ThisThread::sleep_for(300);
+ while(watch()>20 || watch()<0){
+ led1=!led1;
+ tran_RL(turn);
+ }
+ }else{
+ //go through right side+900
+ turn=0;
+ servo.pulsewidth_us(mid_po-830);//左直角
+ ThisThread::sleep_for(300);
+ while(watch()>25 || watch()<0){
+ tran_RL(turn);
+ }
+ }
+
+ //preDistance=watch();
+ while(1){
+ nowDistance=watch();
+ //if(watch()>nowDistance){
+ // nowDistance=watch();
+ ThisThread::sleep_for(10);
+ if(watch()>nowDistance){
+ nowDistance=watch();
+ ThisThread::sleep_for(10);
+
+ if(watch()>nowDistance){
+ tran_RL(3);
+ break;
+ }
+ }
+ //}
+ }
+ while(!fond){
+ while(watch()>0 && watch()<30){
+ pc.printf("++go through+++++++++++++++++%d\r\n",watch());
+
+ int sensor3 = ss3;
+ //直行
+ motorR1 = 0.2; // 右前進モーターON
+ motorR2 = 0; // 右後退モーターOFF
+ motorL1 = 0.2; // 左前進モーターON
+ motorL2 = 0; // 左後退モーターOFF
+ //sen3黑
+ if(sensor3==1){
+ fond=1;
+ tran_RL(3);
+ servo.pulsewidth_us(1450);
+ ThisThread::sleep_for(300000);
+
+ tran_RL(turn);
+ break;
+ }
+ if(watch()>40){
+ ThisThread::sleep_for(10);
+ if (watch()>40){
+ ThisThread::sleep_for(10);
+ if (watch()>40){
+
+ pc.printf("out is ok^^^^^^^^^^^^^^^^^^^^^^%d\r\n",watch());
+ }
+ }
+
+ }
+ }
+ while(!fond){
+ //缓慢右转
+ tran_RL(!turn);
+ nowDistance=watch();
+ ThisThread::sleep_for(10);
+ if(watch()>nowDistance){
+ nowDistance=watch();
+ ThisThread::sleep_for(10);
+ if(watch()>nowDistance){
+ //nowDistance=watch();
+ break;
+ /* if(watch()>nowDistance){
+ tran_RL(3);
+ break;
+ }*/
+ }
+ }
+ }
+ }
+ }
+}
+
+
void trace(){
while(1){
+
/* 各センサー値読み取り */
int sensor1 = ss1;
int sensor2 = ss2;
@@ -489,6 +629,8 @@
int sensor4 = ss4;
int sensor5 = ss5;
int sensD = 0;
+ //led1=0;
+ //trace_avoid();
/* センサー値の決定 */
if(sensor1 > 0) sensD |= 0x10;
@@ -665,6 +807,42 @@
}
/* 距離計測関数 */
+int watchpsq(){
+ do{
+ trig = 0;
+ ThisThread::sleep_for(5); // 5ms待つ
+ trig = 1;
+ ThisThread::sleep_for(15); // 15ms待つ
+ trig = 0;
+ timer.start();
+ t1=timer.read_ms();
+ while(echo.read() == 0 && t1<10){
+ t1=timer.read_ms();
+ //led1 = 1;
+ }
+ timer.stop();
+ timer.reset();
+ }while(t1 >= 10);
+ timer.start(); // 距離計測タイマースタート
+ while(echo.read() == 1){
+ }
+ timer.stop(); // 距離計測タイマーストップ
+ DT = (int)(timer.read_us()*0.01657); // 距離計算
+ /*
+ if(timer.read_ms() > 1000){
+ DT = -1;
+ break;
+ }
+ */
+
+ if(DT > 600){
+ DT = 600;
+ }
+ timer.reset();
+ //led1 = 0;
+ return DT;
+}
+/* 距離計測関数 */
int watch(){
do{
do{
@@ -897,9 +1075,9 @@
if(avoi_thread->get_state() != Thread::Deleted){
avoi_thread->terminate();
}
- if(wifi_thread->get_state() != Thread::Deleted){
- wifi_thread->terminate();
- }
+ //if(wifi_thread->get_state() != Thread::Deleted){
+ // wifi_thread->terminate();
+ //}
if(deco_thread != NULL){
deco_thread->terminate();
}
@@ -1557,9 +1735,9 @@
for(int i=0;i<10;i++){
b10[i] = (int)(((battery.read() * 3.3 - MIN_V)/0.7)*10+0.5)*10;
};
- wifi_thread = new Thread(wifi);
- wifi_thread -> set_priority(osPriorityHigh);
-// setup();
+ //wifi_thread = new Thread(wifi);
+ //wifi_thread -> set_priority(osPriorityHigh);
+ setup();
avoi_thread = new Thread(avoidance);
avoi_thread->terminate();
trace_thread = new Thread(trace);