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

can出来ない～

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@10:6d60c0927770, 2019-03-06 (annotated)

Committer:
yuki0701
Date:
Wed Mar 06 06:19:53 2019 +0000
Revision:
10:6d60c0927770
Parent:
9:337fe0747940 
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 //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
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 9:337fe0747940 44 //printf("T1 = %d\n\r",T1);
yuki0701 8:4bdfac5b52e1 45 if(T1 == 0) {
yuki0701 8:4bdfac5b52e1 46
yuki0701 8:4bdfac5b52e1 47 //スタート位置からみかんの木まで移動
yuki0701 9:337fe0747940 48 printf("T1 = 0\n\r");
yuki0701 10:6d60c0927770 49 wait(0.5);
yuki0701 8:4bdfac5b52e1 50 T1++;
yuki0701 8:4bdfac5b52e1 51 //printf("t = %d\n\r",T1);
yuki0701 8:4bdfac5b52e1 52 }
yuki0701 8:4bdfac5b52e1 53
yuki0701 8:4bdfac5b52e1 54 if(T1 == 1) {
yuki0701 8:4bdfac5b52e1 55 while(1) {
yuki0701 9:337fe0747940 56 printf("T1 = 1\n\r");
yuki0701 9:337fe0747940 57 wait(0.5);
yuki0701 10:6d60c0927770 58 if(T1 > 1) {
yuki0701 8:4bdfac5b52e1 59 break;
yuki0701 8:4bdfac5b52e1 60 }
yuki0701 8:4bdfac5b52e1 61 }
yuki0701 8:4bdfac5b52e1 62 }
yuki0701 8:4bdfac5b52e1 63
yuki0701 8:4bdfac5b52e1 64 if(T1 == 2) {
yuki0701 8:4bdfac5b52e1 65
yuki0701 8:4bdfac5b52e1 66 //みかんの木から三宝置き場まで移動
yuki0701 9:337fe0747940 67 printf("T1 = 2\n\r");
yuki0701 8:4bdfac5b52e1 68 T1++;
yuki0701 8:4bdfac5b52e1 69 }
yuki0701 8:4bdfac5b52e1 70
yuki0701 8:4bdfac5b52e1 71 if(T1 == 3) {
yuki0701 8:4bdfac5b52e1 72 while(1) {
yuki0701 9:337fe0747940 73 printf("T1 = 3\n\r");
yuki0701 10:6d60c0927770 74 wait(0.5);
yuki0701 10:6d60c0927770 75 if(T1 > 3) {
yuki0701 9:337fe0747940 76
yuki0701 8:4bdfac5b52e1 77 break;
yuki0701 8:4bdfac5b52e1 78 }
yuki0701 8:4bdfac5b52e1 79 }
yuki0701 8:4bdfac5b52e1 80 }
yuki0701 8:4bdfac5b52e1 81
yuki0701 8:4bdfac5b52e1 82 if(T1 == 4) {
yuki0701 8:4bdfac5b52e1 83
yuki0701 8:4bdfac5b52e1 84 //三宝置き場からりんごの木まで移動
yuki0701 10:6d60c0927770 85 printf("T1 = 4\n\r");
yuki0701 10:6d60c0927770 86 wait(0.5);
yuki0701 8:4bdfac5b52e1 87 T1++;
yuki0701 8:4bdfac5b52e1 88 }
yuki0701 8:4bdfac5b52e1 89
yuki0701 8:4bdfac5b52e1 90 if(T1 == 5) {
yuki0701 8:4bdfac5b52e1 91 while(1) {
yuki0701 10:6d60c0927770 92 printf("T1 = 5\n\r");
yuki0701 10:6d60c0927770 93 wait(0.5);
yuki0701 10:6d60c0927770 94 if(T1 > 5) {
yuki0701 8:4bdfac5b52e1 95 break;
yuki0701 8:4bdfac5b52e1 96 }
yuki0701 8:4bdfac5b52e1 97 }
yuki0701 8:4bdfac5b52e1 98 }
yuki0701 8:4bdfac5b52e1 99
yuki0701 8:4bdfac5b52e1 100 if(T1 == 6) {
yuki0701 8:4bdfac5b52e1 101
yuki0701 8:4bdfac5b52e1 102 //りんごの木からお供え台まで移動
yuki0701 10:6d60c0927770 103 printf("T1 = 6\n\r");
yuki0701 10:6d60c0927770 104 wait(0.5);
yuki0701 8:4bdfac5b52e1 105 T1++;
yuki0701 8:4bdfac5b52e1 106 }
yuki0701 8:4bdfac5b52e1 107 #endif
yuki0701 8:4bdfac5b52e1 108
yuki0701 8:4bdfac5b52e1 109 #ifdef HARUROBO_MAIN_MODE
la00noix 6:26724c287387 110 while(1) {
la00noix 6:26724c287387 111
yuki0701 8:4bdfac5b52e1 112 id1_value[0] = 1;
la00noix 6:26724c287387 113 switch(id1_value[0]) {
la00noix 0:c61c6e4775ca 114 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 115 case 1:
la00noix 6:26724c287387 116
yuki0701 8:4bdfac5b52e1 117 id1_value[6] = 1;
la00noix 6:26724c287387 118 switch(id1_value[6]) {
la00noix 6:26724c287387 119 case 0:
la00noix 6:26724c287387 120 //-----right mode-------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 121
la00noix 6:26724c287387 122 break;
la00noix 6:26724c287387 123 case 1:
la00noix 6:26724c287387 124 //-----left mode--------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 125
yuki0701 8:4bdfac5b52e1 126 go_waitmode = 0;
la00noix 6:26724c287387 127 if(go_waitmode == 0) {
la00noix 0:c61c6e4775ca 128
yuki0701 8:4bdfac5b52e1 129
yuki0701 9:337fe0747940 130
la00noix 6:26724c287387 131 //can_start();
la00noix 6:26724c287387 132 gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
la00noix 6:26724c287387 133 purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 9:337fe0747940 134 purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
yuki0701 9:337fe0747940 135 gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
la00noix 6:26724c287387 136 MaxonControl(0,0,0,0);
yuki0701 9:337fe0747940 137 set_cond(2,0,-1243,1,1076);
yuki0701 9:337fe0747940 138 pos_correction(-1672,1400,0,0,0);
yuki0701 9:337fe0747940 139 enc_correction(1,1);
la00noix 6:26724c287387 140 wait(0.5);
la00noix 6:26724c287387 141
yuki0701 9:337fe0747940 142 gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
yuki0701 9:337fe0747940 143 purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
la00noix 6:26724c287387 144 purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
la00noix 6:26724c287387 145 gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);
la00noix 6:26724c287387 146 gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
la00noix 6:26724c287387 147 MaxonControl(0,0,0,0);
la00noix 6:26724c287387 148 pos_correction(-2962,4500,-90,1,1);
la00noix 6:26724c287387 149
la00noix 6:26724c287387 150 set_cond(0,0,-2462,0,0);
yuki0701 8:4bdfac5b52e1 151 gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 152 MaxonControl(0,0,0,0);
yuki0701 9:337fe0747940 153 pos_correction(-2850,4500,-90,0,1);
yuki0701 9:337fe0747940 154 enc_correction(1,0);
la00noix 6:26724c287387 155 wait(0.5);
yuki0701 1:3c11e07da92a 156
la00noix 6:26724c287387 157 gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 158 purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 159 purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 160 purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 161 gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 162 set_cond(2,1,-3500,1,6324);
yuki0701 8:4bdfac5b52e1 163 gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 164 MaxonControl(0,0,0,0);
yuki0701 9:337fe0747940 165 pos_correction(-2700,6647,-90,0,0);
yuki0701 9:337fe0747940 166 enc_correction(1,1);
la00noix 6:26724c287387 167 wait(0.5);
la00noix 6:26724c287387 168
la00noix 6:26724c287387 169 gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90);
la00noix 6:26724c287387 170 gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:337fe0747940 171 purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
yuki0701 9:337fe0747940 172 gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
la00noix 6:26724c287387 173 set_cond(2,1,1243,1,4000);
yuki0701 9:337fe0747940 174 gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
la00noix 6:26724c287387 175 MaxonControl(0,0,0,0);
yuki0701 9:337fe0747940 176 pos_correction(-519,4500,-180,0,0);
yuki0701 8:4bdfac5b52e1 177 MaxonControl(0,0,0,0);
yuki0701 9:337fe0747940 178
la00noix 6:26724c287387 179 go_waitmode = 1;
yuki0701 1:3c11e07da92a 180
la00noix 6:26724c287387 181 } else if(go_waitmode == 1) {
la00noix 6:26724c287387 182
la00noix 6:26724c287387 183 MaxonControl(0,0,0,0);
la00noix 6:26724c287387 184
la00noix 6:26724c287387 185 }
la00noix 6:26724c287387 186 break;
la00noix 6:26724c287387 187 }
la00noix 6:26724c287387 188 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 189 case 0:
la00noix 6:26724c287387 190
la00noix 6:26724c287387 191 calc_xy(0,1,1);
la00noix 6:26724c287387 192 ashi_led();
la00noix 6:26724c287387 193 MaxonControl(0,0,0,0);
la00noix 7:44ce34007499 194 go_waitmode = 0;
la00noix 6:26724c287387 195
la00noix 6:26724c287387 196 break;
la00noix 6:26724c287387 197 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 198 case 2:
la00noix 6:26724c287387 199
la00noix 6:26724c287387 200 ManualOut(250,100,500,200);
la00noix 7:44ce34007499 201 go_waitmode = 0;
la00noix 6:26724c287387 202
la00noix 6:26724c287387 203 break;
la00noix 0:c61c6e4775ca 204 }
la00noix 0:c61c6e4775ca 205 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 206 }
yuki0701 8:4bdfac5b52e1 207 #endif
yuki0701 8:4bdfac5b52e1 208 }
yuki0701 8:4bdfac5b52e1 209