robostep8th / Mbed 2 deprecated harurobo_ashi_com7

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@11:8eda88c96bb0, 2020-12-26 (annotated)

Committer:
yuki0701
Date:
Sat Dec 26 09:00:06 2020 +0000
Revision:
11:8eda88c96bb0
Parent:
10:cf5c159e729a 
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
yuki0701 11:8eda88c96bb0 20 /*------------運動時の各速度の設定----------*/
yuki0701 11:8eda88c96bb0 21 double st_speed = 500; // 移動開始時の速度
yuki0701 11:8eda88c96bb0 22 double max_speed = 2000; // 直線移動時の最大速度
yuki0701 11:8eda88c96bb0 23 double mid_speed = 600; // 直線移動時の中間速度
yuki0701 11:8eda88c96bb0 24 double turn_speed = 1000; // 旋回時の速度
yuki0701 11:8eda88c96bb0 25 double end_speed = 200; // 停止直前の速度
yuki0701 11:8eda88c96bb0 26 double fruit_speed = 200; // 果物を取る時の速度
yuki0701 11:8eda88c96bb0 27 /*--------------------------------------*/
yuki0701 11:8eda88c96bb0 28
yuki0701 11:8eda88c96bb0 29
yuki0701 11:8eda88c96bb0 30
la00noix 0:c61c6e4775ca 31 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
la00noix 6:26724c287387 32 //DigitalOut cansend_led(LED1); //canread -> on //can.cpp
la00noix 6:26724c287387 33 //DigitalOut canread_led(LED2); //cansend -> on //can.cpp
la00noix 0:c61c6e4775ca 34 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
la00noix 0:c61c6e4775ca 35
la00noix 0:c61c6e4775ca 36 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
la00noix 0:c61c6e4775ca 37
la00noix 6:26724c287387 38 int main()
la00noix 6:26724c287387 39 {
la00noix 0:c61c6e4775ca 40 UserLoopSetting_maxon();
la00noix 0:c61c6e4775ca 41 UserLoopSetting_sensor();
la00noix 0:c61c6e4775ca 42 UserLoopSetting_can();
la00noix 6:26724c287387 43
la00noix 0:c61c6e4775ca 44 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
la00noix 0:c61c6e4775ca 45
la00noix 6:26724c287387 46 printf("ソースファイル名 : %s¥n", __FILE__);
la00noix 6:26724c287387 47 printf("作成日付 : %s¥n", __DATE__);
la00noix 6:26724c287387 48 printf("作成時刻 : %s¥n", __TIME__);
la00noix 6:26724c287387 49
la00noix 0:c61c6e4775ca 50 #endif
la00noix 0:c61c6e4775ca 51
yuki0701 9:63924280272d 52 #ifdef HARUROBO_TEST_MODE
yuki0701 9:63924280272d 53
yuki0701 10:cf5c159e729a 54 id1_value[0] = 1;
yuki0701 10:cf5c159e729a 55
yuki0701 10:cf5c159e729a 56
yuki0701 11:8eda88c96bb0 57 while(1){
yuki0701 11:8eda88c96bb0 58 //int m1=0,m2=0,m3=0,m4=0;
yuki0701 11:8eda88c96bb0 59 int m1=4095,m2=4095,m3=4095,m4=4095;
yuki0701 11:8eda88c96bb0 60 MaxonControl(m1,m2,m3,m4);
yuki0701 11:8eda88c96bb0 61 debug_printf("m1=%d m2=%d m3=%d m4=%d\n\r",m1,m2,m3,m4);
yuki0701 11:8eda88c96bb0 62 }
yuki0701 9:63924280272d 63
yuki0701 11:8eda88c96bb0 64 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
yuki0701 11:8eda88c96bb0 65 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:8eda88c96bb0 66 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:8eda88c96bb0 67 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:8eda88c96bb0 68 set_cond(2,0,-1050,1,1076);
yuki0701 11:8eda88c96bb0 69 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
yuki0701 9:63924280272d 70 MaxonControl(0,0,0,0);
yuki0701 11:8eda88c96bb0 71 pos_correction(-1672,1600,0,0,0,25);
yuki0701 10:cf5c159e729a 72 enc_correction(1,1);
yuki0701 11:8eda88c96bb0 73 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
yuki0701 11:8eda88c96bb0 74 MaxonControl(0,0,0,0);
yuki0701 11:8eda88c96bb0 75
yuki0701 11:8eda88c96bb0 76 //wait(3);
yuki0701 9:63924280272d 77
yuki0701 11:8eda88c96bb0 78 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:8eda88c96bb0 79 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:8eda88c96bb0 80 purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,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 11:8eda88c96bb0 81 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90);
yuki0701 11:8eda88c96bb0 82 gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90);
yuki0701 9:63924280272d 83 MaxonControl(0,0,0,0);
yuki0701 11:8eda88c96bb0 84 set_cond(2,0,-2462,0,6000);
yuki0701 11:8eda88c96bb0 85 pos_correction(-2962,4837.5,-90,0,0,10);
yuki0701 11:8eda88c96bb0 86 gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 87 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 88 enc_correction(1,1);
yuki0701 11:8eda88c96bb0 89
yuki0701 11:8eda88c96bb0 90 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 91 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 92 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 93 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 94 gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 95 MaxonControl(0,0,0,0);
yuki0701 11:8eda88c96bb0 96 set_cond(1,0,0,1,6038);
yuki0701 11:8eda88c96bb0 97 pos_correction(-2500,6610,-90,1,0,8);
yuki0701 11:8eda88c96bb0 98 set_cond(2,1,-3162,1,6038);
yuki0701 11:8eda88c96bb0 99 gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 100 MaxonControl(0,0,0,0);
yuki0701 11:8eda88c96bb0 101 enc_correction(1,1);
yuki0701 9:63924280272d 102
yuki0701 11:8eda88c96bb0 103 set_cond(1,0,0,0,6962);
yuki0701 11:8eda88c96bb0 104 gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 105 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 106
yuki0701 11:8eda88c96bb0 107 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:8eda88c96bb0 108 enc_correction(0,1);
yuki0701 11:8eda88c96bb0 109 //set_cond(2,0,-19,0,6962);
yuki0701 11:8eda88c96bb0 110 purecurve(8,1,1,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正
yuki0701 11:8eda88c96bb0 111 //enc_correction(1,0);
yuki0701 11:8eda88c96bb0 112 set_cond(0,0,-19,0,0);
yuki0701 11:8eda88c96bb0 113 gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
yuki0701 11:8eda88c96bb0 114 gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
yuki0701 10:cf5c159e729a 115 set_cond(2,1,-1000,1,4000);
yuki0701 11:8eda88c96bb0 116 gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180);
yuki0701 9:63924280272d 117 MaxonControl(0,0,0,0);
yuki0701 11:8eda88c96bb0 118 pos_correction(-620,4370,-180,0,0,30);
yuki0701 10:cf5c159e729a 119 enc_correction(1,1);
yuki0701 9:63924280272d 120 MaxonControl(0,0,0,0);
yuki0701 9:63924280272d 121
yuki0701 11:8eda88c96bb0 122
yuki0701 11:8eda88c96bb0 123
yuki0701 10:cf5c159e729a 124 //以下自動で初期位置に戻るプログラム
yuki0701 10:cf5c159e729a 125 wait(3);
yuki0701 11:8eda88c96bb0 126
yuki0701 11:8eda88c96bb0 127 gogo_straight(1,1,-620,4500,-620,5000,200,1000,5,0.1,10,0.1,600,-180);
yuki0701 11:8eda88c96bb0 128 gogo_straight(1,1,-620,5000,-620,6000,1000,1000,5,0.1,10,0.1,600,-180);
yuki0701 11:8eda88c96bb0 129 gogo_straight(1,1,-620,6000,-620,6600,1000,200,5,0.1,10,0.1,600,-180);
yuki0701 10:cf5c159e729a 130 MaxonControl(0,0,0,0);
yuki0701 10:cf5c159e729a 131 set_cond(2,0,0,0,7000);
yuki0701 11:8eda88c96bb0 132 //pos_correction(-519,6600,-180,0,0,30);
yuki0701 11:8eda88c96bb0 133 calc_xy(-180,0,0);
yuki0701 10:cf5c159e729a 134 enc_correction(1,1);
yuki0701 11:8eda88c96bb0 135 gogo_straight(1,1,-620,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180);
yuki0701 10:cf5c159e729a 136 purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180);
yuki0701 10:cf5c159e729a 137 purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180);
yuki0701 10:cf5c159e729a 138 gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180);
yuki0701 10:cf5c159e729a 139 MaxonControl(0,0,0,0);
yuki0701 10:cf5c159e729a 140 set_cond(2,1,-3500,0,6000);
yuki0701 11:8eda88c96bb0 141 //pos_correction(-2962,5500,-180,0,0,30);
yuki0701 10:cf5c159e729a 142 enc_correction(1,1);
yuki0701 11:8eda88c96bb0 143 calc_xy(-180,0,0);
yuki0701 10:cf5c159e729a 144 gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90);
yuki0701 10:cf5c159e729a 145 gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0);
yuki0701 10:cf5c159e729a 146 gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0);
yuki0701 10:cf5c159e729a 147 MaxonControl(0,0,0,0);
yuki0701 10:cf5c159e729a 148 set_cond(2,1,-3500,1,2000);
yuki0701 11:8eda88c96bb0 149 pos_correction(-3112,3500,0,0,0,15);
yuki0701 10:cf5c159e729a 150 enc_correction(1,1);
yuki0701 10:cf5c159e729a 151 MaxonControl(0,0,0,0);
yuki0701 11:8eda88c96bb0 152
yuki0701 9:63924280272d 153 #endif
yuki0701 9:63924280272d 154
yuki0701 9:63924280272d 155 #ifdef HARUROBO_MAIN_MODE
la00noix 6:26724c287387 156 while(1) {
la00noix 6:26724c287387 157
la00noix 6:26724c287387 158 switch(id1_value[0]) {
la00noix 0:c61c6e4775ca 159 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 160 case 1:
la00noix 6:26724c287387 161
la00noix 6:26724c287387 162 switch(id1_value[6]) {
la00noix 6:26724c287387 163 case 0:
la00noix 6:26724c287387 164 //-----right mode-------------------------------------------------------------------------------------------------------------//
la00noix 8:2ba338b4590e 165 if(T1 == 0) {
la00noix 6:26724c287387 166
la00noix 8:2ba338b4590e 167 //スタート位置からみかんの木まで移動
la00noix 8:2ba338b4590e 168 wait(0.5);
la00noix 8:2ba338b4590e 169 T1++;
la00noix 8:2ba338b4590e 170
la00noix 8:2ba338b4590e 171 }
la00noix 8:2ba338b4590e 172 if(T1 == 1) {
la00noix 8:2ba338b4590e 173 while(1) {
la00noix 8:2ba338b4590e 174 wait(0.5);
la00noix 8:2ba338b4590e 175 if(T1 == 2) {
la00noix 8:2ba338b4590e 176 break;
la00noix 8:2ba338b4590e 177 }
la00noix 8:2ba338b4590e 178 }
la00noix 8:2ba338b4590e 179 }
la00noix 8:2ba338b4590e 180 if(T1 == 2) {
la00noix 8:2ba338b4590e 181
la00noix 8:2ba338b4590e 182 //みかんの木から三宝置き場まで移動
la00noix 8:2ba338b4590e 183 T1++;
la00noix 8:2ba338b4590e 184 }
la00noix 8:2ba338b4590e 185 if(T1 == 3) {
la00noix 8:2ba338b4590e 186 while(1) {
la00noix 8:2ba338b4590e 187 wait(0.5);
la00noix 8:2ba338b4590e 188 if(T1 == 4) {
la00noix 8:2ba338b4590e 189 break;
la00noix 8:2ba338b4590e 190 }
la00noix 8:2ba338b4590e 191 }
la00noix 8:2ba338b4590e 192 }
la00noix 8:2ba338b4590e 193 if(T1 == 4) {
la00noix 8:2ba338b4590e 194
la00noix 8:2ba338b4590e 195 //三宝置き場からりんごの木まで移動
la00noix 8:2ba338b4590e 196 wait(0.5);
la00noix 8:2ba338b4590e 197 T1++;
la00noix 8:2ba338b4590e 198 }
la00noix 8:2ba338b4590e 199 if(T1 == 5) {
la00noix 8:2ba338b4590e 200 while(1) {
la00noix 8:2ba338b4590e 201
la00noix 8:2ba338b4590e 202 wait(0.5);
la00noix 8:2ba338b4590e 203 if(T1 == 6) {
la00noix 8:2ba338b4590e 204 break;
la00noix 8:2ba338b4590e 205 }
la00noix 8:2ba338b4590e 206 }
la00noix 8:2ba338b4590e 207 }
la00noix 8:2ba338b4590e 208 if(T1 == 6) {
la00noix 8:2ba338b4590e 209
la00noix 8:2ba338b4590e 210 //りんごの木からお供え台まで移動
la00noix 8:2ba338b4590e 211 wait(0.5);
la00noix 8:2ba338b4590e 212 T1++;
la00noix 8:2ba338b4590e 213 }
la00noix 6:26724c287387 214 break;
la00noix 6:26724c287387 215 case 1:
la00noix 6:26724c287387 216 //-----left mode--------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 217
la00noix 6:26724c287387 218 break;
la00noix 6:26724c287387 219 }
la00noix 6:26724c287387 220 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 221 case 0:
la00noix 6:26724c287387 222
la00noix 6:26724c287387 223 calc_xy(0,1,1);
la00noix 6:26724c287387 224 ashi_led();
la00noix 6:26724c287387 225 MaxonControl(0,0,0,0);
la00noix 7:44ce34007499 226 go_waitmode = 0;
la00noix 6:26724c287387 227
la00noix 6:26724c287387 228 break;
la00noix 6:26724c287387 229 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 230 case 2:
la00noix 6:26724c287387 231
la00noix 6:26724c287387 232 ManualOut(250,100,500,200);
la00noix 7:44ce34007499 233 go_waitmode = 0;
la00noix 6:26724c287387 234
la00noix 6:26724c287387 235 break;
la00noix 0:c61c6e4775ca 236 }
la00noix 0:c61c6e4775ca 237 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 238 }
yuki0701 9:63924280272d 239
yuki0701 9:63924280272d 240 #endif
yuki0701 9:63924280272d 241
la00noix 0:c61c6e4775ca 242 }