春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_ashi_3_13_13h_24m

3/13 13時24分 up

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@10:c741191360de, 2019-03-13 (annotated)

Committer:
yuki0701
Date:
Wed Mar 13 04:23:18 2019 +0000
Revision:
10:c741191360de
Parent:
9:7667dcfc7ce5 
a

Who changed what in which revision?

UserRevisionLine numberNew 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 //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
la00noix 0:c61c6e4775ca 15
la00noix 0:c61c6e4775ca 16 int go_waitmode = 0;
la00noix 0:c61c6e4775ca 17
la00noix 0:c61c6e4775ca 18 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
la00noix 9:7667dcfc7ce5 19 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
la00noix 9:7667dcfc7ce5 20 //DigitalOut canread_led(LED2); //canread -> on //can.cpp
la00noix 0:c61c6e4775ca 21 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
la00noix 0:c61c6e4775ca 22
la00noix 0:c61c6e4775ca 23 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
la00noix 0:c61c6e4775ca 24
la00noix 6:26724c287387 25 int main()
la00noix 6:26724c287387 26 {
la00noix 0:c61c6e4775ca 27 UserLoopSetting_maxon();
la00noix 0:c61c6e4775ca 28 UserLoopSetting_sensor();
la00noix 0:c61c6e4775ca 29 UserLoopSetting_can();
la00noix 6:26724c287387 30
la00noix 0:c61c6e4775ca 31 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
la00noix 0:c61c6e4775ca 32
la00noix 6:26724c287387 33 printf("ソースファイル名 : %s¥n", __FILE__);
la00noix 6:26724c287387 34 printf("作成日付 : %s¥n", __DATE__);
la00noix 6:26724c287387 35 printf("作成時刻 : %s¥n", __TIME__);
la00noix 6:26724c287387 36
la00noix 0:c61c6e4775ca 37 #endif
la00noix 0:c61c6e4775ca 38
la00noix 6:26724c287387 39 while(1) {
la00noix 6:26724c287387 40
la00noix 6:26724c287387 41 switch(id1_value[0]) {
la00noix 0:c61c6e4775ca 42 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 43 case 1:
la00noix 6:26724c287387 44
la00noix 6:26724c287387 45 switch(id1_value[6]) {
la00noix 6:26724c287387 46 case 0:
la00noix 6:26724c287387 47 //-----right mode-------------------------------------------------------------------------------------------------------------//
la00noix 9:7667dcfc7ce5 48 flag = 0;
yuki0701 10:c741191360de 49 UserLoopSetting_enc_right();
yuki0701 10:c741191360de 50 if(T1 == 0) { //スタート位置からみかんの木まで移動
yuki0701 10:c741191360de 51 gogo_straight(1,1,2962,3500,2962,2900,200,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 52 purecurve(6,1,1,2962,2900,2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 53 purecurve(5,1,1,2317,2500,1672,2000,9,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 54 set_cond(2,1,1050,1,1076);
yuki0701 10:c741191360de 55 gogo_straight(0,0,1672,2000,1672,1400,1000,200,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 56 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 57 pos_correction(1672,1400,0,0,0);
yuki0701 10:c741191360de 58 enc_correction(1,1);
yuki0701 10:c741191360de 59 wait(0.5);
yuki0701 10:c741191360de 60 T1++;
yuki0701 10:c741191360de 61 }
yuki0701 10:c741191360de 62 if(T1 == 1) {
yuki0701 10:c741191360de 63 while(1) {
yuki0701 10:c741191360de 64 wait(0.1);
yuki0701 10:c741191360de 65 if(T1 == 2) {
yuki0701 10:c741191360de 66 break;
yuki0701 10:c741191360de 67 }
yuki0701 10:c741191360de 68 }
yuki0701 10:c741191360de 69 }
yuki0701 10:c741191360de 70 if(T1 == 2) { //みかんの木から三宝置き場まで移動
yuki0701 10:c741191360de 71 gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 72 purecurve(2,1,1,1672,2000,2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 73 purecurve(1,1,1,2317,2500,2962,3000,9,1000,5,0.1,10,0.1,600,90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
yuki0701 10:c741191360de 74 gogo_straight(1,1,2962,3000,2962,4000,1000,1000,5,0.1,10,0.1,600,90);
yuki0701 10:c741191360de 75 gogo_straight(1,1,2962,4000,2962,4500,1000,200,5,0.1,10,0.1,600,90);
yuki0701 10:c741191360de 76 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 77 pos_correction(2962,4500,90,1,1);
yuki0701 10:c741191360de 78 set_cond(2,1,2462,0,6000);
yuki0701 10:c741191360de 79 gogo_straight(0,0,2962,4500,2850,4500,200,200,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 80 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 81 pos_correction(2850,4500,90,0,0);
yuki0701 10:c741191360de 82 enc_correction(1,1);
yuki0701 10:c741191360de 83 wait(0.5);
yuki0701 10:c741191360de 84 T1++;
yuki0701 10:c741191360de 85 }
yuki0701 10:c741191360de 86 if(T1 == 3) {
yuki0701 10:c741191360de 87 while(1) {
yuki0701 10:c741191360de 88 wait(0.1);
yuki0701 10:c741191360de 89 if(T1 == 4) {
yuki0701 10:c741191360de 90 break;
yuki0701 10:c741191360de 91 }
yuki0701 10:c741191360de 92 }
yuki0701 10:c741191360de 93 }
yuki0701 10:c741191360de 94 if(T1 == 4) { //三宝置き場からりんごの木まで移動
yuki0701 10:c741191360de 95 gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 96 purecurve(3,1,1,2850,5150,2257,5500,9,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 97 purecurve(4,1,1,2257,5500,1700,6000,9,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 98 purecurve(2,1,1,1700,6000,2257,6550,9,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 99 gogo_straight(1,1,2257,6550,2500,6550,1000,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 100 set_cond(2,0,3500,1,6050);
yuki0701 10:c741191360de 101 gogo_straight(0,0,2500,6550,2700,6600,1000,200,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 102 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 103 pos_correction(2700,6600,90,0,0);
yuki0701 10:c741191360de 104 enc_correction(1,1);
yuki0701 10:c741191360de 105 wait(0.5);
yuki0701 10:c741191360de 106 T1++;
yuki0701 10:c741191360de 107 }
yuki0701 10:c741191360de 108 if(T1 == 5) {
yuki0701 10:c741191360de 109 while(1) {
yuki0701 10:c741191360de 110 wait(0.1);
yuki0701 10:c741191360de 111 if(T1 == 6) {
yuki0701 10:c741191360de 112 break;
yuki0701 10:c741191360de 113 }
yuki0701 10:c741191360de 114 }
yuki0701 10:c741191360de 115 }
yuki0701 10:c741191360de 116 if(T1 == 6) { //りんごの木からお供え台まで移動
yuki0701 10:c741191360de 117 gogo_straight(1,1,2700,6600,2500,6550,200,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 118 gogo_straight(1,1,2500,6550,1300,6550,1000,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 119 gogo_straight(1,1,1300,6550,1000,6500,1000,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 120 purecurve(5,1,1,1000,6550,519,6000,9,1000,5,0.1,10,0.1,600,180);
yuki0701 10:c741191360de 121 gogo_straight(1,1,519,6000,519,4700,1000,1000,5,0.1,10,0.1,600,180);
yuki0701 10:c741191360de 122 set_cond(2,0,1000,1,4000);
yuki0701 10:c741191360de 123 gogo_straight(0,0,519,4700,519,4500,1000,200,5,0.1,10,0.1,800,180);
yuki0701 10:c741191360de 124 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 125 pos_correction(519,4500,180,0,0);
yuki0701 10:c741191360de 126 enc_correction(1,1);
yuki0701 10:c741191360de 127 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 128 T1++;
yuki0701 10:c741191360de 129 }
yuki0701 10:c741191360de 130 if(T1 == 7) {
yuki0701 10:c741191360de 131 while(1) {
yuki0701 10:c741191360de 132 printf("ashi finished\n\r");
yuki0701 10:c741191360de 133 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 134 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
yuki0701 10:c741191360de 135 if(id1_value[6] != flag)break;
yuki0701 10:c741191360de 136 }
yuki0701 10:c741191360de 137 }
yuki0701 10:c741191360de 138 break;
yuki0701 10:c741191360de 139
yuki0701 10:c741191360de 140 case 1:
yuki0701 10:c741191360de 141 //-----left mode--------------------------------------------------------------------------------------------------------------//
yuki0701 10:c741191360de 142 flag = 1;
yuki0701 10:c741191360de 143 UserLoopSetting_enc_left();
la00noix 9:7667dcfc7ce5 144 if(T1 == 0) { //スタート位置からみかんの木まで移動
la00noix 9:7667dcfc7ce5 145 gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 146 purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 147 purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 148 set_cond(2,0,-1050,1,1076);
la00noix 9:7667dcfc7ce5 149 gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 150 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 151 pos_correction(-1672,1400,0,0,0);
la00noix 9:7667dcfc7ce5 152 enc_correction(1,1);
la00noix 8:2ba338b4590e 153 wait(0.5);
la00noix 8:2ba338b4590e 154 T1++;
la00noix 8:2ba338b4590e 155 }
la00noix 8:2ba338b4590e 156 if(T1 == 1) {
la00noix 8:2ba338b4590e 157 while(1) {
la00noix 9:7667dcfc7ce5 158 wait(0.1);
la00noix 8:2ba338b4590e 159 if(T1 == 2) {
la00noix 8:2ba338b4590e 160 break;
la00noix 8:2ba338b4590e 161 }
la00noix 8:2ba338b4590e 162 }
la00noix 8:2ba338b4590e 163 }
la00noix 9:7667dcfc7ce5 164 if(T1 == 2) { //みかんの木から三宝置き場まで移動
la00noix 9:7667dcfc7ce5 165 gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 166 purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 167 purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
la00noix 9:7667dcfc7ce5 168 gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);
la00noix 9:7667dcfc7ce5 169 gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
la00noix 9:7667dcfc7ce5 170 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 171 pos_correction(-2962,4500,-90,1,1);
la00noix 9:7667dcfc7ce5 172 set_cond(2,0,-2462,0,6000);
la00noix 9:7667dcfc7ce5 173 gogo_straight(0,0,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 174 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 175 pos_correction(-2850,4500,-90,0,0);
la00noix 9:7667dcfc7ce5 176 enc_correction(1,1);
la00noix 9:7667dcfc7ce5 177 wait(0.5);
la00noix 8:2ba338b4590e 178 T1++;
la00noix 8:2ba338b4590e 179 }
la00noix 8:2ba338b4590e 180 if(T1 == 3) {
la00noix 8:2ba338b4590e 181 while(1) {
la00noix 9:7667dcfc7ce5 182 wait(0.1);
la00noix 8:2ba338b4590e 183 if(T1 == 4) {
la00noix 8:2ba338b4590e 184 break;
la00noix 8:2ba338b4590e 185 }
la00noix 8:2ba338b4590e 186 }
la00noix 8:2ba338b4590e 187 }
la00noix 9:7667dcfc7ce5 188 if(T1 == 4) { //三宝置き場からりんごの木まで移動
la00noix 9:7667dcfc7ce5 189 gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 190 purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 191 purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 192 purecurve(3,1,1,-1700,6000,-2257,6550,9,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 193 gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 194 set_cond(2,1,-3500,1,6050);
la00noix 9:7667dcfc7ce5 195 gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 196 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 197 pos_correction(-2700,6600,-90,0,0);
la00noix 9:7667dcfc7ce5 198 enc_correction(1,1);
la00noix 8:2ba338b4590e 199 wait(0.5);
la00noix 8:2ba338b4590e 200 T1++;
la00noix 8:2ba338b4590e 201 }
la00noix 8:2ba338b4590e 202 if(T1 == 5) {
la00noix 8:2ba338b4590e 203 while(1) {
la00noix 9:7667dcfc7ce5 204 wait(0.1);
la00noix 8:2ba338b4590e 205 if(T1 == 6) {
la00noix 8:2ba338b4590e 206 break;
la00noix 8:2ba338b4590e 207 }
la00noix 8:2ba338b4590e 208 }
la00noix 8:2ba338b4590e 209 }
la00noix 9:7667dcfc7ce5 210 if(T1 == 6) { //りんごの木からお供え台まで移動
la00noix 9:7667dcfc7ce5 211 gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 212 gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 213 gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 214 purecurve(8,1,1,-1000,6550,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
la00noix 9:7667dcfc7ce5 215 gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
la00noix 9:7667dcfc7ce5 216 set_cond(2,1,-1000,1,4000);
la00noix 9:7667dcfc7ce5 217 gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
la00noix 9:7667dcfc7ce5 218 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 219 pos_correction(-519,4500,-180,0,0);
la00noix 9:7667dcfc7ce5 220 enc_correction(1,1);
la00noix 9:7667dcfc7ce5 221 MaxonControl(0,0,0,0);
la00noix 8:2ba338b4590e 222 T1++;
la00noix 8:2ba338b4590e 223 }
la00noix 9:7667dcfc7ce5 224 if(T1 == 7) {
la00noix 9:7667dcfc7ce5 225 while(1) {
la00noix 9:7667dcfc7ce5 226 printf("ashi finished\n\r");
la00noix 9:7667dcfc7ce5 227 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 228 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
la00noix 9:7667dcfc7ce5 229 if(id1_value[6] != flag)break;
la00noix 9:7667dcfc7ce5 230 }
la00noix 9:7667dcfc7ce5 231 }
la00noix 6:26724c287387 232
la00noix 6:26724c287387 233 break;
la00noix 6:26724c287387 234 }
la00noix 6:26724c287387 235 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 236 case 0:
la00noix 6:26724c287387 237
la00noix 6:26724c287387 238 calc_xy(0,1,1);
la00noix 6:26724c287387 239 ashi_led();
la00noix 6:26724c287387 240 MaxonControl(0,0,0,0);
la00noix 7:44ce34007499 241 go_waitmode = 0;
la00noix 6:26724c287387 242
la00noix 6:26724c287387 243 break;
la00noix 6:26724c287387 244 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 245 case 2:
la00noix 6:26724c287387 246
la00noix 6:26724c287387 247 ManualOut(250,100,500,200);
la00noix 7:44ce34007499 248 go_waitmode = 0;
la00noix 6:26724c287387 249
la00noix 6:26724c287387 250 break;
la00noix 0:c61c6e4775ca 251 }
la00noix 0:c61c6e4775ca 252 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 253 }
la00noix 0:c61c6e4775ca 254 }