ロボステ6期 / Mbed 2 deprecated NHK2020-main１

あ

Dependencies:   uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P

main.cpp@0:b87fd8dd4322, 2019-03-19 (annotated)

Committer:
la00noix
Date:
Tue Mar 19 13:20:23 2019 +0000
Revision:
0:b87fd8dd4322
Child:
1:26fc1b2f1c42
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:b87fd8dd4322 1 #include "EC.h"
la00noix 0:b87fd8dd4322 2 #include "R1370P.h"
la00noix 0:b87fd8dd4322 3 #include "move4wheel.h"
la00noix 0:b87fd8dd4322 4 #include "mbed.h"
la00noix 0:b87fd8dd4322 5 #include "math.h"
la00noix 0:b87fd8dd4322 6 #include "PathFollowing.h"
la00noix 0:b87fd8dd4322 7 #include "movement.h"
la00noix 0:b87fd8dd4322 8 #include "maxonsetting.h"
la00noix 0:b87fd8dd4322 9 #include "manual.h"
la00noix 0:b87fd8dd4322 10 #include "can.h"
la00noix 0:b87fd8dd4322 11
la00noix 0:b87fd8dd4322 12 #define PI 3.141592
la00noix 0:b87fd8dd4322 13
la00noix 0:b87fd8dd4322 14 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
la00noix 0:b87fd8dd4322 15
la00noix 0:b87fd8dd4322 16 int go_waitmode = 0;
la00noix 0:b87fd8dd4322 17
la00noix 0:b87fd8dd4322 18 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
la00noix 0:b87fd8dd4322 19 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
la00noix 0:b87fd8dd4322 20 //DigitalOut canread_led(LED2); //canread -> on //can.cpp
la00noix 0:b87fd8dd4322 21 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
la00noix 0:b87fd8dd4322 22
la00noix 0:b87fd8dd4322 23 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
la00noix 0:b87fd8dd4322 24
la00noix 0:b87fd8dd4322 25 int main()
la00noix 0:b87fd8dd4322 26 {
la00noix 0:b87fd8dd4322 27 UserLoopSetting_maxon();
la00noix 0:b87fd8dd4322 28 UserLoopSetting_sensor();
la00noix 0:b87fd8dd4322 29 UserLoopSetting_can();
la00noix 0:b87fd8dd4322 30
la00noix 0:b87fd8dd4322 31 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
la00noix 0:b87fd8dd4322 32
la00noix 0:b87fd8dd4322 33 printf("ソースファイル名 : %s¥n", __FILE__);
la00noix 0:b87fd8dd4322 34 printf("作成日付 : %s¥n", __DATE__);
la00noix 0:b87fd8dd4322 35 printf("作成時刻 : %s¥n", __TIME__);
la00noix 0:b87fd8dd4322 36
la00noix 0:b87fd8dd4322 37 #endif
la00noix 0:b87fd8dd4322 38
la00noix 0:b87fd8dd4322 39 while(1) {
la00noix 0:b87fd8dd4322 40
la00noix 0:b87fd8dd4322 41 switch(id1_value[0]) {
la00noix 0:b87fd8dd4322 42 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 0:b87fd8dd4322 43 case 1:
la00noix 0:b87fd8dd4322 44
la00noix 0:b87fd8dd4322 45 switch(id1_value[6]) {
la00noix 0:b87fd8dd4322 46 case 0:
la00noix 0:b87fd8dd4322 47 //-----right mode-------------------------------------------------------------------------------------------------------------//
la00noix 0:b87fd8dd4322 48 flag = 0;
la00noix 0:b87fd8dd4322 49 if(T1 == 0) { //スタート位置から少し移動
la00noix 0:b87fd8dd4322 50
la00noix 0:b87fd8dd4322 51 wait(0.5);
la00noix 0:b87fd8dd4322 52 T1++;
la00noix 0:b87fd8dd4322 53 }
la00noix 0:b87fd8dd4322 54 if(T1 == 1) { //みかんの木まで移動して掴む
la00noix 0:b87fd8dd4322 55
la00noix 0:b87fd8dd4322 56 wait(0.5);
la00noix 0:b87fd8dd4322 57 T1++;
la00noix 0:b87fd8dd4322 58 }
la00noix 0:b87fd8dd4322 59
la00noix 0:b87fd8dd4322 60 if(T1 == 2) { //みかんを持ち上げる
la00noix 0:b87fd8dd4322 61 while(1) {
la00noix 0:b87fd8dd4322 62 wait(0.1);
la00noix 0:b87fd8dd4322 63 if(T1 == 3) {
la00noix 0:b87fd8dd4322 64 break;
la00noix 0:b87fd8dd4322 65 }
la00noix 0:b87fd8dd4322 66 }
la00noix 0:b87fd8dd4322 67 }
la00noix 0:b87fd8dd4322 68 if(T1 == 3) { //みかんの木から三宝置き場まで移動
la00noix 0:b87fd8dd4322 69
la00noix 0:b87fd8dd4322 70 wait(0.5);
la00noix 0:b87fd8dd4322 71 T1++;
la00noix 0:b87fd8dd4322 72 }
la00noix 0:b87fd8dd4322 73 if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる
la00noix 0:b87fd8dd4322 74 while(1) {
la00noix 0:b87fd8dd4322 75 wait(0.1);
la00noix 0:b87fd8dd4322 76 if(T1 == 5) {
la00noix 0:b87fd8dd4322 77 break;
la00noix 0:b87fd8dd4322 78 }
la00noix 0:b87fd8dd4322 79 }
la00noix 0:b87fd8dd4322 80 }
la00noix 0:b87fd8dd4322 81 if(T1 == 5) { //三宝置き場からりんごの木まで移動
la00noix 0:b87fd8dd4322 82
la00noix 0:b87fd8dd4322 83 wait(0.5);
la00noix 0:b87fd8dd4322 84 T1++;
la00noix 0:b87fd8dd4322 85 }
la00noix 0:b87fd8dd4322 86 if(T1 == 6) { //りんごを持ち上げる
la00noix 0:b87fd8dd4322 87 while(1) {
la00noix 0:b87fd8dd4322 88 wait(0.1);
la00noix 0:b87fd8dd4322 89 if(T1 == 7) {
la00noix 0:b87fd8dd4322 90 break;
la00noix 0:b87fd8dd4322 91 }
la00noix 0:b87fd8dd4322 92 }
la00noix 0:b87fd8dd4322 93 }
la00noix 0:b87fd8dd4322 94 if(T1 == 7) { //りんごの木からお供え台まで移動
la00noix 0:b87fd8dd4322 95
la00noix 0:b87fd8dd4322 96 wait(0.5);
la00noix 0:b87fd8dd4322 97 T1++;
la00noix 0:b87fd8dd4322 98 }
la00noix 0:b87fd8dd4322 99 if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く
la00noix 0:b87fd8dd4322 100 while(1) {
la00noix 0:b87fd8dd4322 101 printf("ashi finished\n\r");
la00noix 0:b87fd8dd4322 102 MaxonControl(0,0,0,0);
la00noix 0:b87fd8dd4322 103 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
la00noix 0:b87fd8dd4322 104 if(id1_value[6] != flag)break;
la00noix 0:b87fd8dd4322 105 }
la00noix 0:b87fd8dd4322 106 }
la00noix 0:b87fd8dd4322 107 break;
la00noix 0:b87fd8dd4322 108 case 1:
la00noix 0:b87fd8dd4322 109 //-----left mode--------------------------------------------------------------------------------------------------------------//
la00noix 0:b87fd8dd4322 110 flag = 1;
la00noix 0:b87fd8dd4322 111
la00noix 0:b87fd8dd4322 112 break;
la00noix 0:b87fd8dd4322 113 }
la00noix 0:b87fd8dd4322 114 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 0:b87fd8dd4322 115 case 0:
la00noix 0:b87fd8dd4322 116
la00noix 0:b87fd8dd4322 117 calc_xy(0,1,1);
la00noix 0:b87fd8dd4322 118 ashi_led();
la00noix 0:b87fd8dd4322 119 MaxonControl(0,0,0,0);
la00noix 0:b87fd8dd4322 120 go_waitmode = 0;
la00noix 0:b87fd8dd4322 121
la00noix 0:b87fd8dd4322 122 break;
la00noix 0:b87fd8dd4322 123 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 0:b87fd8dd4322 124 case 2:
la00noix 0:b87fd8dd4322 125
la00noix 0:b87fd8dd4322 126 ManualOut(250,100,500,200);
la00noix 0:b87fd8dd4322 127 go_waitmode = 0;
la00noix 0:b87fd8dd4322 128
la00noix 0:b87fd8dd4322 129 break;
la00noix 0:b87fd8dd4322 130 }
la00noix 0:b87fd8dd4322 131 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:b87fd8dd4322 132 }
la00noix 0:b87fd8dd4322 133 }