Maiko Matsumoto
6/18 wallbot
Fork of
BLE_WallbotBLE_Challenge_byYUTAKA
by 
Yutaka Yoshida
Diff: main.cpp
- Revision:
- 5:29f7f535d4b2
- Parent:
- 4:951fabc31ba6
- Child:
- 6:213041054f97
diff -r 951fabc31ba6 -r 29f7f535d4b2 main.cpp
--- a/main.cpp Mon Jun 11 06:51:49 2018 +0000
+++ b/main.cpp Mon Jun 11 07:37:33 2018 +0000
@@ -46,6 +46,9 @@
int challenge_mode = 0;
char bValue = 0;
+// Wallbot State
+int stt_Mode;
+
int get_line(int num);
/* RCBController Service */
@@ -355,67 +358,151 @@
{
ModeLed = 0;
wait(1);
+
+ int cnt_loop = 0;
+ float pre_ax = 0;
+ float thre_bump = 1.0;
+
while(sw1 != 0)
{
-#if 0
- int line = get_line(0) ? 0 : 1;
- line |= get_line(1) ? 0 : 2;
- line |= get_line(2) ? 0 : 4;
- line |= get_line(3) ? 0 : 8;
-#else
- int line = get_line(0) ? 1 : 0;
- line |= get_line(1) ? 2 : 0;
- line |= get_line(2) ? 4 : 0;
- line |= get_line(3) ? 8 : 0;
-#endif
+ Wait(0.1);
+
+ // Get value
+
+ // Store pre frame value
+ pre_ax = ax;
+
+
+
+//#if 0
+// int line = get_line(0) ? 0 : 1;
+// line |= get_line(1) ? 0 : 2;
+// line |= get_line(2) ? 0 : 4;
+// line |= get_line(3) ? 0 : 8;
+//#else
+// int line = get_line(0) ? 1 : 0;
+// line |= get_line(1) ? 2 : 0;
+// line |= get_line(2) ? 4 : 0;
+// line |= get_line(3) ? 8 : 0;
+//#endif
-#if DBG
- pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
-#endif
+//#if DBG
+// pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
+//#endif
//改造中のソース
#if 1
- switch(line)
+ switch(stt_Mode)
{
- case 1: //直進モード上から下へ ○○○● とりあえず書いてみたパターン
- if(ay<0){
- left = 1.0;
- right = 1.0;
+ case 1: // Up forward
+ pc.printf("Mode 1");
+
+ if(ay < -0.2){
+ left = 0.9;
+ right = 1.0;
}
- else if(ay>0){
+ else if(ay > 0.2){
+ left = 1.0;
+ right = 0.9;
+ }
+ else
+ {
left = 1.0;
right = 1.0;
}
+
+ // Judge Bump
+ if(ax - pre_ax < -thre_bump)
+ {
+ stt_Mode = 2;
+ pc.printf(" 1 -> 2 ");
+ }
+
+ break;
+ case 2:
+ pc.printf("Mode 2");
+ if(cnt_loop < 10)
+ {
+ left = -1.0;
+ right = -1.0;
+ cnt_loop++;
+ }
+ else{
+ cnt_loop = 0;
+ stt_Mode = 3;
+ pc.printf(" 2 -> 3");
+ }
break;
- case 3: //直進モード下から上へ ○○●●
- left = 1.0;
- right = -0.5;
- break;
- case 2: //回転モード上から下へ○○●○
- left = 1.0;
- right = 0.5;
- break;
- case 6: //回転モード下から上へ○●●○
- left = 1.0;
- right = 1.0;
+ case 3: //
+ pc.printf("Mode 3");
+ if(ax < 9.8 && ay > 0.1 && ay < -0.1)
+ {
+ left = 1.0;
+ right = 0;
+ }
+ else
+ {
+ stt_Mode = 4;
+ pc.printf(" 3 - > 4");
+ }
+
break;
- case 4: //後進モード上から下へ○●○○
- left = 0.5;
- right = 1.0;
+
+ case 4: // Down forward
+ pc.printf("Mode 4");
+ if(ay < -0.2){
+ left = 1.0;
+ right = 0.9;
+ }
+ else if(ay > 0.2){
+ left = 0.9;
+ right = 1.0;
+ }
+ else
+ {
+ left = 1.0;
+ right = 1.0;
+ }
+
+ // Judge Bump
+ if(ax - pre_ax < -thre_bump)
+ {
+ stt_Mode = 5;
+ }
+
break;
- case 12: //後進モード下から上へ●●○○
- left = -0.5;
- right = 1.0;
+ case 5:
+ pc.printf("Mode 5");
+ if(cnt_loop < 10)
+ {
+ left = -1.0;
+ right = -1.0;
+ cnt_loop++;
+ }
+ else{
+ cnt_loop = 0;
+ stt_Mode = 6;
+ pc.printf(" 5 -> 6 \n");
+ }
+
break;
- case 8: // ●○○○
- left = -1.0;
- right = 1.0;
+ case 6: //
+ pc.printf("Mode 6");
+ if(ax < -9.8 && ay > 0.1 && ay < -0.1)
+ {
+ left = 0;
+ right = 1.0;
+ }
+ else
+ {
+ stt_Mode = 1;
+ pc.printf(" 6 -> 1 \n");
+ }
+
break;
default:
- left = 1.0;
- right = 1.0;
break;
}
#endif
@@ -512,6 +599,15 @@
ble.addService(RCBControllerService);
+ // Wallbot State Initialize
+ // Up Straight : 1
+ // Up Back : 2
+ // Up Rotate : 3
+ // Down Straight : 4
+ // Down Back : 5
+ // Down Rotate : 6
+ stt_Mode = 1;
+
while (true) {
if(sw1 == 0)
{