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

3/20 2:04

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@9:7667dcfc7ce5, 2019-03-13 (annotated)

Committer:
la00noix
Date:
Wed Mar 13 03:24:49 2019 +0000
Revision:
9:7667dcfc7ce5
Parent:
8:2ba338b4590e
Child:
10:c741191360de
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;
la00noix 9:7667dcfc7ce5 49 if(T1 == 0) { //スタート位置からみかんの木まで移動
la00noix 9:7667dcfc7ce5 50 gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 51 purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 52 purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 53 set_cond(2,0,-1050,1,1076);
la00noix 9:7667dcfc7ce5 54 gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 55 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 56 pos_correction(-1672,1400,0,0,0);
la00noix 9:7667dcfc7ce5 57 enc_correction(1,1);
la00noix 8:2ba338b4590e 58 wait(0.5);
la00noix 8:2ba338b4590e 59 T1++;
la00noix 8:2ba338b4590e 60 }
la00noix 8:2ba338b4590e 61 if(T1 == 1) {
la00noix 8:2ba338b4590e 62 while(1) {
la00noix 9:7667dcfc7ce5 63 wait(0.1);
la00noix 8:2ba338b4590e 64 if(T1 == 2) {
la00noix 8:2ba338b4590e 65 break;
la00noix 8:2ba338b4590e 66 }
la00noix 8:2ba338b4590e 67 }
la00noix 8:2ba338b4590e 68 }
la00noix 9:7667dcfc7ce5 69 if(T1 == 2) { //みかんの木から三宝置き場まで移動
la00noix 9:7667dcfc7ce5 70 gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 71 purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 72 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 73 gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);
la00noix 9:7667dcfc7ce5 74 gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
la00noix 9:7667dcfc7ce5 75 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 76 pos_correction(-2962,4500,-90,1,1);
la00noix 9:7667dcfc7ce5 77 set_cond(2,0,-2462,0,6000);
la00noix 9:7667dcfc7ce5 78 gogo_straight(0,0,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 79 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 80 pos_correction(-2850,4500,-90,0,0);
la00noix 9:7667dcfc7ce5 81 enc_correction(1,1);
la00noix 9:7667dcfc7ce5 82 wait(0.5);
la00noix 8:2ba338b4590e 83 T1++;
la00noix 8:2ba338b4590e 84 }
la00noix 8:2ba338b4590e 85 if(T1 == 3) {
la00noix 8:2ba338b4590e 86 while(1) {
la00noix 9:7667dcfc7ce5 87 wait(0.1);
la00noix 8:2ba338b4590e 88 if(T1 == 4) {
la00noix 8:2ba338b4590e 89 break;
la00noix 8:2ba338b4590e 90 }
la00noix 8:2ba338b4590e 91 }
la00noix 8:2ba338b4590e 92 }
la00noix 9:7667dcfc7ce5 93 if(T1 == 4) { //三宝置き場からりんごの木まで移動
la00noix 9:7667dcfc7ce5 94 gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 95 purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 96 purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 97 purecurve(3,1,1,-1700,6000,-2257,6550,9,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 98 gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 99 set_cond(2,1,-3500,1,6050);
la00noix 9:7667dcfc7ce5 100 gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 101 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 102 pos_correction(-2700,6600,-90,0,0);
la00noix 9:7667dcfc7ce5 103 enc_correction(1,1);
la00noix 8:2ba338b4590e 104 wait(0.5);
la00noix 8:2ba338b4590e 105 T1++;
la00noix 8:2ba338b4590e 106 }
la00noix 8:2ba338b4590e 107 if(T1 == 5) {
la00noix 8:2ba338b4590e 108 while(1) {
la00noix 9:7667dcfc7ce5 109 wait(0.1);
la00noix 8:2ba338b4590e 110 if(T1 == 6) {
la00noix 8:2ba338b4590e 111 break;
la00noix 8:2ba338b4590e 112 }
la00noix 8:2ba338b4590e 113 }
la00noix 8:2ba338b4590e 114 }
la00noix 9:7667dcfc7ce5 115 if(T1 == 6) { //りんごの木からお供え台まで移動
la00noix 9:7667dcfc7ce5 116 gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 117 gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 118 gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 119 purecurve(8,1,1,-1000,6550,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
la00noix 9:7667dcfc7ce5 120 gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
la00noix 9:7667dcfc7ce5 121 set_cond(2,1,-1000,1,4000);
la00noix 9:7667dcfc7ce5 122 gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
la00noix 9:7667dcfc7ce5 123 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 124 pos_correction(-519,4500,-180,0,0);
la00noix 9:7667dcfc7ce5 125 enc_correction(1,1);
la00noix 9:7667dcfc7ce5 126 MaxonControl(0,0,0,0);
la00noix 8:2ba338b4590e 127 T1++;
la00noix 8:2ba338b4590e 128 }
la00noix 9:7667dcfc7ce5 129 if(T1 == 7) {
la00noix 9:7667dcfc7ce5 130 while(1) {
la00noix 9:7667dcfc7ce5 131 printf("ashi finished\n\r");
la00noix 9:7667dcfc7ce5 132 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 133 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
la00noix 9:7667dcfc7ce5 134 if(id1_value[6] != flag)break;
la00noix 9:7667dcfc7ce5 135 }
la00noix 9:7667dcfc7ce5 136 }
la00noix 6:26724c287387 137 break;
la00noix 6:26724c287387 138 case 1:
la00noix 6:26724c287387 139 //-----left mode--------------------------------------------------------------------------------------------------------------//
la00noix 9:7667dcfc7ce5 140 flag = 1;
la00noix 6:26724c287387 141
la00noix 6:26724c287387 142 break;
la00noix 6:26724c287387 143 }
la00noix 6:26724c287387 144 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 145 case 0:
la00noix 6:26724c287387 146
la00noix 6:26724c287387 147 calc_xy(0,1,1);
la00noix 6:26724c287387 148 ashi_led();
la00noix 6:26724c287387 149 MaxonControl(0,0,0,0);
la00noix 7:44ce34007499 150 go_waitmode = 0;
la00noix 6:26724c287387 151
la00noix 6:26724c287387 152 break;
la00noix 6:26724c287387 153 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 154 case 2:
la00noix 6:26724c287387 155
la00noix 6:26724c287387 156 ManualOut(250,100,500,200);
la00noix 7:44ce34007499 157 go_waitmode = 0;
la00noix 6:26724c287387 158
la00noix 6:26724c287387 159 break;
la00noix 0:c61c6e4775ca 160 }
la00noix 0:c61c6e4775ca 161 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 162 }
la00noix 0:c61c6e4775ca 163 }