3/6
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
main.cpp@8:4bdfac5b52e1, 2019-03-05 (annotated)
- Committer:
- yuki0701
- Date:
- Tue Mar 05 04:29:49 2019 +0000
- Revision:
- 8:4bdfac5b52e1
- Parent:
- 7:44ce34007499
- Child:
- 9:337fe0747940
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
la00noix | 0:c61c6e4775ca | 1 | #include "EC.h" |
la00noix | 0:c61c6e4775ca | 2 | #include "R1370P.h" |
la00noix | 0:c61c6e4775ca | 3 | #include "move4wheel.h" |
la00noix | 0:c61c6e4775ca | 4 | #include "mbed.h" |
la00noix | 0:c61c6e4775ca | 5 | #include "math.h" |
la00noix | 0:c61c6e4775ca | 6 | #include "PathFollowing.h" |
la00noix | 0:c61c6e4775ca | 7 | #include "movement.h" |
la00noix | 0:c61c6e4775ca | 8 | #include "maxonsetting.h" |
la00noix | 0:c61c6e4775ca | 9 | #include "manual.h" |
la00noix | 0:c61c6e4775ca | 10 | #include "can.h" |
la00noix | 0:c61c6e4775ca | 11 | |
la00noix | 0:c61c6e4775ca | 12 | #define PI 3.141592 |
la00noix | 0:c61c6e4775ca | 13 | |
la00noix | 0:c61c6e4775ca | 14 | //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義 |
yuki0701 | 8:4bdfac5b52e1 | 15 | #define HARUROBO_TEST_MODE |
yuki0701 | 8:4bdfac5b52e1 | 16 | //#define HARUROBO_MAIN_MODE |
la00noix | 0:c61c6e4775ca | 17 | |
la00noix | 0:c61c6e4775ca | 18 | int go_waitmode = 0; |
la00noix | 0:c61c6e4775ca | 19 | |
la00noix | 0:c61c6e4775ca | 20 | //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// |
la00noix | 6:26724c287387 | 21 | //DigitalOut cansend_led(LED1); //canread -> on //can.cpp |
la00noix | 6:26724c287387 | 22 | //DigitalOut canread_led(LED2); //cansend -> on //can.cpp |
la00noix | 0:c61c6e4775ca | 23 | //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp |
la00noix | 0:c61c6e4775ca | 24 | |
la00noix | 0:c61c6e4775ca | 25 | //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// |
la00noix | 0:c61c6e4775ca | 26 | |
la00noix | 6:26724c287387 | 27 | int main() |
la00noix | 6:26724c287387 | 28 | { |
la00noix | 0:c61c6e4775ca | 29 | UserLoopSetting_maxon(); |
la00noix | 0:c61c6e4775ca | 30 | UserLoopSetting_sensor(); |
la00noix | 0:c61c6e4775ca | 31 | UserLoopSetting_can(); |
la00noix | 6:26724c287387 | 32 | |
la00noix | 0:c61c6e4775ca | 33 | #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 |
la00noix | 0:c61c6e4775ca | 34 | |
la00noix | 6:26724c287387 | 35 | printf("ソースファイル名 : %s¥n", __FILE__); |
la00noix | 6:26724c287387 | 36 | printf("作成日付 : %s¥n", __DATE__); |
la00noix | 6:26724c287387 | 37 | printf("作成時刻 : %s¥n", __TIME__); |
la00noix | 6:26724c287387 | 38 | |
la00noix | 0:c61c6e4775ca | 39 | #endif |
la00noix | 0:c61c6e4775ca | 40 | |
yuki0701 | 8:4bdfac5b52e1 | 41 | #ifdef HARUROBO_TEST_MODE |
yuki0701 | 8:4bdfac5b52e1 | 42 | T1 = 0; |
yuki0701 | 8:4bdfac5b52e1 | 43 | printf("start\n\r"); |
yuki0701 | 8:4bdfac5b52e1 | 44 | printf("T1 = %d\n\r",T1); |
yuki0701 | 8:4bdfac5b52e1 | 45 | if(T1 == 0) { |
yuki0701 | 8:4bdfac5b52e1 | 46 | |
yuki0701 | 8:4bdfac5b52e1 | 47 | //スタート位置からみかんの木まで移動 |
yuki0701 | 8:4bdfac5b52e1 | 48 | printf("t = 0"); |
yuki0701 | 8:4bdfac5b52e1 | 49 | T1++; |
yuki0701 | 8:4bdfac5b52e1 | 50 | //printf("t = %d\n\r",T1); |
yuki0701 | 8:4bdfac5b52e1 | 51 | } |
yuki0701 | 8:4bdfac5b52e1 | 52 | |
yuki0701 | 8:4bdfac5b52e1 | 53 | if(T1 == 1) { |
yuki0701 | 8:4bdfac5b52e1 | 54 | while(1) { |
yuki0701 | 8:4bdfac5b52e1 | 55 | //printf("wait\n\r"); |
yuki0701 | 8:4bdfac5b52e1 | 56 | if(T1 == 2) { |
yuki0701 | 8:4bdfac5b52e1 | 57 | break; |
yuki0701 | 8:4bdfac5b52e1 | 58 | } |
yuki0701 | 8:4bdfac5b52e1 | 59 | } |
yuki0701 | 8:4bdfac5b52e1 | 60 | } |
yuki0701 | 8:4bdfac5b52e1 | 61 | |
yuki0701 | 8:4bdfac5b52e1 | 62 | if(T1 == 2) { |
yuki0701 | 8:4bdfac5b52e1 | 63 | |
yuki0701 | 8:4bdfac5b52e1 | 64 | //みかんの木から三宝置き場まで移動 |
yuki0701 | 8:4bdfac5b52e1 | 65 | printf("t = 2"); |
yuki0701 | 8:4bdfac5b52e1 | 66 | T1++; |
yuki0701 | 8:4bdfac5b52e1 | 67 | } |
yuki0701 | 8:4bdfac5b52e1 | 68 | |
yuki0701 | 8:4bdfac5b52e1 | 69 | if(T1 == 3) { |
yuki0701 | 8:4bdfac5b52e1 | 70 | while(1) { |
yuki0701 | 8:4bdfac5b52e1 | 71 | if(T1 == 4) { |
yuki0701 | 8:4bdfac5b52e1 | 72 | break; |
yuki0701 | 8:4bdfac5b52e1 | 73 | } |
yuki0701 | 8:4bdfac5b52e1 | 74 | } |
yuki0701 | 8:4bdfac5b52e1 | 75 | } |
yuki0701 | 8:4bdfac5b52e1 | 76 | |
yuki0701 | 8:4bdfac5b52e1 | 77 | if(T1 == 4) { |
yuki0701 | 8:4bdfac5b52e1 | 78 | |
yuki0701 | 8:4bdfac5b52e1 | 79 | //三宝置き場からりんごの木まで移動 |
yuki0701 | 8:4bdfac5b52e1 | 80 | printf("t = 4"); |
yuki0701 | 8:4bdfac5b52e1 | 81 | T1++; |
yuki0701 | 8:4bdfac5b52e1 | 82 | } |
yuki0701 | 8:4bdfac5b52e1 | 83 | |
yuki0701 | 8:4bdfac5b52e1 | 84 | if(T1 == 5) { |
yuki0701 | 8:4bdfac5b52e1 | 85 | while(1) { |
yuki0701 | 8:4bdfac5b52e1 | 86 | if(T1 == 6) { |
yuki0701 | 8:4bdfac5b52e1 | 87 | break; |
yuki0701 | 8:4bdfac5b52e1 | 88 | } |
yuki0701 | 8:4bdfac5b52e1 | 89 | } |
yuki0701 | 8:4bdfac5b52e1 | 90 | } |
yuki0701 | 8:4bdfac5b52e1 | 91 | |
yuki0701 | 8:4bdfac5b52e1 | 92 | if(T1 == 6) { |
yuki0701 | 8:4bdfac5b52e1 | 93 | |
yuki0701 | 8:4bdfac5b52e1 | 94 | //りんごの木からお供え台まで移動 |
yuki0701 | 8:4bdfac5b52e1 | 95 | printf("t = 6"); |
yuki0701 | 8:4bdfac5b52e1 | 96 | T1++; |
yuki0701 | 8:4bdfac5b52e1 | 97 | } |
yuki0701 | 8:4bdfac5b52e1 | 98 | #endif |
yuki0701 | 8:4bdfac5b52e1 | 99 | |
yuki0701 | 8:4bdfac5b52e1 | 100 | #ifdef HARUROBO_MAIN_MODE |
la00noix | 6:26724c287387 | 101 | while(1) { |
la00noix | 6:26724c287387 | 102 | |
yuki0701 | 8:4bdfac5b52e1 | 103 | id1_value[0] = 1; |
la00noix | 6:26724c287387 | 104 | switch(id1_value[0]) { |
la00noix | 0:c61c6e4775ca | 105 | //-----auto mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 106 | case 1: |
la00noix | 6:26724c287387 | 107 | |
yuki0701 | 8:4bdfac5b52e1 | 108 | id1_value[6] = 1; |
la00noix | 6:26724c287387 | 109 | switch(id1_value[6]) { |
la00noix | 6:26724c287387 | 110 | case 0: |
la00noix | 6:26724c287387 | 111 | //-----right mode-------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 112 | |
la00noix | 6:26724c287387 | 113 | break; |
la00noix | 6:26724c287387 | 114 | case 1: |
la00noix | 6:26724c287387 | 115 | //-----left mode--------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 116 | |
yuki0701 | 8:4bdfac5b52e1 | 117 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 118 | if(go_waitmode == 0) { |
la00noix | 0:c61c6e4775ca | 119 | |
yuki0701 | 8:4bdfac5b52e1 | 120 | |
yuki0701 | 8:4bdfac5b52e1 | 121 | |
la00noix | 6:26724c287387 | 122 | //can_start(); |
la00noix | 6:26724c287387 | 123 | gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); |
la00noix | 6:26724c287387 | 124 | purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
la00noix | 6:26724c287387 | 125 | purecurve(8,1,1,-2317,2500,-1610,2000,9,1000,5,0.1,10,0.1,600,0); |
la00noix | 6:26724c287387 | 126 | gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0); |
la00noix | 6:26724c287387 | 127 | MaxonControl(0,0,0,0); |
yuki0701 | 8:4bdfac5b52e1 | 128 | set_cond(2,0,-1243,1,800); |
la00noix | 6:26724c287387 | 129 | pos_correction(-1610,1250,0,1,0); |
la00noix | 6:26724c287387 | 130 | wait(0.5); |
la00noix | 6:26724c287387 | 131 | |
la00noix | 6:26724c287387 | 132 | gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0); |
la00noix | 6:26724c287387 | 133 | purecurve(3,1,1,-1610,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
la00noix | 6:26724c287387 | 134 | purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); |
la00noix | 6:26724c287387 | 135 | gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); |
la00noix | 6:26724c287387 | 136 | gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); |
la00noix | 6:26724c287387 | 137 | MaxonControl(0,0,0,0); |
la00noix | 6:26724c287387 | 138 | pos_correction(-2962,4500,-90,1,1); |
la00noix | 6:26724c287387 | 139 | |
la00noix | 6:26724c287387 | 140 | set_cond(0,0,-2462,0,0); |
yuki0701 | 8:4bdfac5b52e1 | 141 | gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 142 | MaxonControl(0,0,0,0); |
la00noix | 6:26724c287387 | 143 | pos_correction(-2850,4500,-90,1,1); |
la00noix | 6:26724c287387 | 144 | wait(0.5); |
yuki0701 | 1:3c11e07da92a | 145 | |
la00noix | 6:26724c287387 | 146 | gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 147 | purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 148 | purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 149 | purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 150 | gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 151 | set_cond(2,1,-3500,1,6324); |
yuki0701 | 8:4bdfac5b52e1 | 152 | gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 153 | MaxonControl(0,0,0,0); |
la00noix | 6:26724c287387 | 154 | pos_correction(-2700,6647,-90,1,1); |
la00noix | 6:26724c287387 | 155 | wait(0.5); |
la00noix | 6:26724c287387 | 156 | |
la00noix | 6:26724c287387 | 157 | gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 158 | gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90); |
la00noix | 6:26724c287387 | 159 | purecurve(8,1,1,-1000,6647,-350,6000,9,1000,5,0.1,10,0.1,600,-180); |
la00noix | 6:26724c287387 | 160 | gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180); |
la00noix | 6:26724c287387 | 161 | set_cond(2,1,1243,1,4000); |
yuki0701 | 8:4bdfac5b52e1 | 162 | gogo_straight(0,0,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180); |
la00noix | 6:26724c287387 | 163 | MaxonControl(0,0,0,0); |
la00noix | 6:26724c287387 | 164 | pos_correction(-400,4500,-180,1,1); |
yuki0701 | 8:4bdfac5b52e1 | 165 | MaxonControl(0,0,0,0); |
yuki0701 | 8:4bdfac5b52e1 | 166 | |
la00noix | 6:26724c287387 | 167 | go_waitmode = 1; |
yuki0701 | 1:3c11e07da92a | 168 | |
la00noix | 6:26724c287387 | 169 | } else if(go_waitmode == 1) { |
la00noix | 6:26724c287387 | 170 | |
la00noix | 6:26724c287387 | 171 | MaxonControl(0,0,0,0); |
la00noix | 6:26724c287387 | 172 | |
la00noix | 6:26724c287387 | 173 | } |
la00noix | 6:26724c287387 | 174 | break; |
la00noix | 6:26724c287387 | 175 | } |
la00noix | 6:26724c287387 | 176 | //-----wait mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 177 | case 0: |
la00noix | 6:26724c287387 | 178 | |
la00noix | 6:26724c287387 | 179 | calc_xy(0,1,1); |
la00noix | 6:26724c287387 | 180 | ashi_led(); |
la00noix | 6:26724c287387 | 181 | MaxonControl(0,0,0,0); |
la00noix | 7:44ce34007499 | 182 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 183 | |
la00noix | 6:26724c287387 | 184 | break; |
la00noix | 6:26724c287387 | 185 | //-----manual mode--------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 186 | case 2: |
la00noix | 6:26724c287387 | 187 | |
la00noix | 6:26724c287387 | 188 | ManualOut(250,100,500,200); |
la00noix | 7:44ce34007499 | 189 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 190 | |
la00noix | 6:26724c287387 | 191 | break; |
la00noix | 0:c61c6e4775ca | 192 | } |
la00noix | 0:c61c6e4775ca | 193 | //------------------------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 194 | } |
yuki0701 | 8:4bdfac5b52e1 | 195 | #endif |
yuki0701 | 8:4bdfac5b52e1 | 196 | } |
yuki0701 | 8:4bdfac5b52e1 | 197 |