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

改良版位置補正プログラム動作未確認

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@9:63924280272d, 2019-03-07 (annotated)

Committer:
yuki0701
Date:
Thu Mar 07 05:47:18 2019 +0000
Revision:
9:63924280272d
Parent:
8:2ba338b4590e 
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 9:63924280272d 15 #define HARUROBO_TEST_MODE
yuki0701 9:63924280272d 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 9:63924280272d 41 #ifdef HARUROBO_TEST_MODE
yuki0701 9:63924280272d 42
yuki0701 9:63924280272d 43 gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
yuki0701 9:63924280272d 44 purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 9:63924280272d 45 purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
yuki0701 9:63924280272d 46 gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
yuki0701 9:63924280272d 47 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 48 set_cond(2,0,-1243,1,1076);
yuki0701 9:63924280272d 49 pos_correction(-1672,1400,0,0,0);
yuki0701 9:63924280272d 50 enc_correction(1,1);
yuki0701 9:63924280272d 51 wait(0.5);
yuki0701 9:63924280272d 52
yuki0701 9:63924280272d 53 gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
yuki0701 9:63924280272d 54 purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 9:63924280272d 55 purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
yuki0701 9:63924280272d 56 gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);
yuki0701 9:63924280272d 57 gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
yuki0701 9:63924280272d 58 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 59 pos_correction(-2962,4500,-90,1,1);
yuki0701 9:63924280272d 60
yuki0701 9:63924280272d 61 set_cond(0,0,-2462,0,0);
yuki0701 9:63924280272d 62 gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 63 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 64 pos_correction(-2850,4500,-90,0,1);
yuki0701 9:63924280272d 65 enc_correction(1,0);
yuki0701 9:63924280272d 66 wait(0.5);
yuki0701 9:63924280272d 67
yuki0701 9:63924280272d 68 gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 69 purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 70 purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 71 purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 72 gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 73 set_cond(2,1,-3500,1,6324);
yuki0701 9:63924280272d 74 gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 75 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 76 pos_correction(-2700,6647,-90,0,0);
yuki0701 9:63924280272d 77 enc_correction(1,1);
yuki0701 9:63924280272d 78 wait(0.5);
yuki0701 9:63924280272d 79
yuki0701 9:63924280272d 80 gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 81 gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90);
yuki0701 9:63924280272d 82 purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
yuki0701 9:63924280272d 83 gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
yuki0701 9:63924280272d 84 set_cond(2,1,1243,1,4000);
yuki0701 9:63924280272d 85 gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
yuki0701 9:63924280272d 86 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 87 pos_correction(-519,4500,-180,0,0);
yuki0701 9:63924280272d 88 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 89
yuki0701 9:63924280272d 90 #endif
yuki0701 9:63924280272d 91
yuki0701 9:63924280272d 92 #ifdef HARUROBO_MAIN_MODE
la00noix 6:26724c287387 93 while(1) {
la00noix 6:26724c287387 94
la00noix 6:26724c287387 95 switch(id1_value[0]) {
la00noix 0:c61c6e4775ca 96 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 97 case 1:
la00noix 6:26724c287387 98
la00noix 6:26724c287387 99 switch(id1_value[6]) {
la00noix 6:26724c287387 100 case 0:
la00noix 6:26724c287387 101 //-----right mode-------------------------------------------------------------------------------------------------------------//
la00noix 8:2ba338b4590e 102 if(T1 == 0) {
la00noix 6:26724c287387 103
la00noix 8:2ba338b4590e 104 //スタート位置からみかんの木まで移動
la00noix 8:2ba338b4590e 105 wait(0.5);
la00noix 8:2ba338b4590e 106 T1++;
la00noix 8:2ba338b4590e 107
la00noix 8:2ba338b4590e 108 }
la00noix 8:2ba338b4590e 109 if(T1 == 1) {
la00noix 8:2ba338b4590e 110 while(1) {
la00noix 8:2ba338b4590e 111 wait(0.5);
la00noix 8:2ba338b4590e 112 if(T1 == 2) {
la00noix 8:2ba338b4590e 113 break;
la00noix 8:2ba338b4590e 114 }
la00noix 8:2ba338b4590e 115 }
la00noix 8:2ba338b4590e 116 }
la00noix 8:2ba338b4590e 117 if(T1 == 2) {
la00noix 8:2ba338b4590e 118
la00noix 8:2ba338b4590e 119 //みかんの木から三宝置き場まで移動
la00noix 8:2ba338b4590e 120 T1++;
la00noix 8:2ba338b4590e 121 }
la00noix 8:2ba338b4590e 122 if(T1 == 3) {
la00noix 8:2ba338b4590e 123 while(1) {
la00noix 8:2ba338b4590e 124 wait(0.5);
la00noix 8:2ba338b4590e 125 if(T1 == 4) {
la00noix 8:2ba338b4590e 126 break;
la00noix 8:2ba338b4590e 127 }
la00noix 8:2ba338b4590e 128 }
la00noix 8:2ba338b4590e 129 }
la00noix 8:2ba338b4590e 130 if(T1 == 4) {
la00noix 8:2ba338b4590e 131
la00noix 8:2ba338b4590e 132 //三宝置き場からりんごの木まで移動
la00noix 8:2ba338b4590e 133 wait(0.5);
la00noix 8:2ba338b4590e 134 T1++;
la00noix 8:2ba338b4590e 135 }
la00noix 8:2ba338b4590e 136 if(T1 == 5) {
la00noix 8:2ba338b4590e 137 while(1) {
la00noix 8:2ba338b4590e 138
la00noix 8:2ba338b4590e 139 wait(0.5);
la00noix 8:2ba338b4590e 140 if(T1 == 6) {
la00noix 8:2ba338b4590e 141 break;
la00noix 8:2ba338b4590e 142 }
la00noix 8:2ba338b4590e 143 }
la00noix 8:2ba338b4590e 144 }
la00noix 8:2ba338b4590e 145 if(T1 == 6) {
la00noix 8:2ba338b4590e 146
la00noix 8:2ba338b4590e 147 //りんごの木からお供え台まで移動
la00noix 8:2ba338b4590e 148 wait(0.5);
la00noix 8:2ba338b4590e 149 T1++;
la00noix 8:2ba338b4590e 150 }
la00noix 6:26724c287387 151 break;
la00noix 6:26724c287387 152 case 1:
la00noix 6:26724c287387 153 //-----left mode--------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 154
la00noix 6:26724c287387 155 break;
la00noix 6:26724c287387 156 }
la00noix 6:26724c287387 157 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 158 case 0:
la00noix 6:26724c287387 159
la00noix 6:26724c287387 160 calc_xy(0,1,1);
la00noix 6:26724c287387 161 ashi_led();
la00noix 6:26724c287387 162 MaxonControl(0,0,0,0);
la00noix 7:44ce34007499 163 go_waitmode = 0;
la00noix 6:26724c287387 164
la00noix 6:26724c287387 165 break;
la00noix 6:26724c287387 166 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 167 case 2:
la00noix 6:26724c287387 168
la00noix 6:26724c287387 169 ManualOut(250,100,500,200);
la00noix 7:44ce34007499 170 go_waitmode = 0;
la00noix 6:26724c287387 171
la00noix 6:26724c287387 172 break;
la00noix 0:c61c6e4775ca 173 }
la00noix 0:c61c6e4775ca 174 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 175 }
yuki0701 9:63924280272d 176
yuki0701 9:63924280272d 177 #endif
yuki0701 9:63924280272d 178
la00noix 0:c61c6e4775ca 179 }