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

3/20 2:04

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@2:7cba05e70367, 2019-02-26 (annotated)

Committer:
la00noix
Date:
Tue Feb 26 05:10:27 2019 +0000
Revision:
2:7cba05e70367
Parent:
1:3c11e07da92a
Child:
3:8a0faa3b08c3
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 #define HARUROBO_TEST_MODE //テスト自動プログラム（練習・動作確認用）使用時に定義
la00noix 0:c61c6e4775ca 17 //#define HARUROBO_RIGHT_MODE //本番用自動プログラム（右側フィールド）使用時に定義
la00noix 0:c61c6e4775ca 18 //#define HARUROBO_LEFT_MODE //本番用自動プログラム（左側フィールド）使用時に定義
la00noix 0:c61c6e4775ca 19
la00noix 0:c61c6e4775ca 20 int go_waitmode = 0;
la00noix 0:c61c6e4775ca 21
la00noix 0:c61c6e4775ca 22 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
la00noix 0:c61c6e4775ca 23 //DigitalOut canread_led(LED1); //canread -> on //can.cpp
la00noix 0:c61c6e4775ca 24 //DigitalOut cansend_led(LED2); //cansend -> on //can.cpp
la00noix 0:c61c6e4775ca 25 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
la00noix 0:c61c6e4775ca 26
la00noix 0:c61c6e4775ca 27 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
la00noix 0:c61c6e4775ca 28
la00noix 0:c61c6e4775ca 29 int main(){
la00noix 0:c61c6e4775ca 30
la00noix 0:c61c6e4775ca 31 UserLoopSetting_maxon();
la00noix 0:c61c6e4775ca 32 UserLoopSetting_sensor();
la00noix 0:c61c6e4775ca 33 UserLoopSetting_can();
la00noix 0:c61c6e4775ca 34
la00noix 0:c61c6e4775ca 35 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
la00noix 0:c61c6e4775ca 36
la00noix 0:c61c6e4775ca 37 printf("ソースファイル名 : %s¥n", __FILE__);
la00noix 0:c61c6e4775ca 38 printf("作成日付 : %s¥n", __DATE__);
la00noix 0:c61c6e4775ca 39 printf("作成時刻 : %s¥n", __TIME__);
la00noix 0:c61c6e4775ca 40
la00noix 0:c61c6e4775ca 41 #endif
la00noix 0:c61c6e4775ca 42
la00noix 0:c61c6e4775ca 43 while(1){
la00noix 0:c61c6e4775ca 44
la00noix 2:7cba05e70367 45 id1_value[0] = 1;
yuki0701 1:3c11e07da92a 46
la00noix 0:c61c6e4775ca 47 switch(id1_value[0]){
la00noix 0:c61c6e4775ca 48
la00noix 0:c61c6e4775ca 49 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 50 case 1:
la00noix 0:c61c6e4775ca 51
la00noix 0:c61c6e4775ca 52 //-----テスト自動プログラム（練習・動作確認用--------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 53 #ifdef HARUROBO_TEST_MODE
la00noix 0:c61c6e4775ca 54
la00noix 0:c61c6e4775ca 55 if(go_waitmode == 0){
yuki0701 1:3c11e07da92a 56
la00noix 2:7cba05e70367 57 //can_start();
la00noix 2:7cba05e70367 58 //set_cond(2,1,-700,1,-700);
yuki0701 1:3c11e07da92a 59 gogo_straight(1,1,-2962,3500,-2962,3000,200,1000,5,0.1,10,0.1,600,0);
yuki0701 1:3c11e07da92a 60 purecurve(7,1,1,-2962,3000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 1:3c11e07da92a 61 purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
yuki0701 1:3c11e07da92a 62 gogo_straight(1,1,-1672,2000,-1672,1500,1000,200,5,0.1,10,0.1,600,0);
yuki0701 1:3c11e07da92a 63 MaxonControl(0,0,0,0);
yuki0701 1:3c11e07da92a 64 set_cond(2,0,-1243,1,1076);
yuki0701 1:3c11e07da92a 65 pos_correction(-1672,1500,0,0,0);
yuki0701 1:3c11e07da92a 66 wait(0.5);
yuki0701 1:3c11e07da92a 67
yuki0701 1:3c11e07da92a 68 gogo_straight(1,1,-1672,1500,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
yuki0701 1:3c11e07da92a 69 purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 1:3c11e07da92a 70 purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,90);
yuki0701 1:3c11e07da92a 71 gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,90);
yuki0701 1:3c11e07da92a 72 gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,90);
yuki0701 1:3c11e07da92a 73 MaxonControl(0,0,0,0);
yuki0701 1:3c11e07da92a 74 pos_correction(-2962,4500,90,1,1);
yuki0701 1:3c11e07da92a 75
yuki0701 1:3c11e07da92a 76 set_cond(0,0,-2462,0,0);
yuki0701 1:3c11e07da92a 77 gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 78 MaxonControl(0,0,0,0);
yuki0701 1:3c11e07da92a 79 pos_correction(-2850,4500,90,0,1);
yuki0701 1:3c11e07da92a 80 wait(0.5);
yuki0701 1:3c11e07da92a 81
yuki0701 1:3c11e07da92a 82 gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 83 purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 84 purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 85 purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 86 gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 87 set_cond(1,0,0,0,7000);
yuki0701 1:3c11e07da92a 88 gogo_straight(1,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 89 MaxonControl(0,0,0,0);
yuki0701 1:3c11e07da92a 90 pos_correction(-2700,6647,90,1,0);
yuki0701 1:3c11e07da92a 91 wait(0.5);
yuki0701 1:3c11e07da92a 92
yuki0701 1:3c11e07da92a 93 gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 94 gogo_straight(1,1,-2500,6647,-1243,6647,1000,1000,5,0.1,10,0.1,800,90);
yuki0701 1:3c11e07da92a 95 purecurve(8,1,1,-1243,6647,-519,6000,9,1000,5,0.1,10,0.1,600,180);
yuki0701 1:3c11e07da92a 96 gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,180);
yuki0701 1:3c11e07da92a 97 set_cond(2,0,0,1,4000);
yuki0701 1:3c11e07da92a 98 gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,180);
yuki0701 1:3c11e07da92a 99 MaxonControl(0,0,0,0);
yuki0701 1:3c11e07da92a 100 pos_correction(-519,4500,180,0,0);
yuki0701 1:3c11e07da92a 101
yuki0701 1:3c11e07da92a 102 go_waitmode = 1;
la00noix 0:c61c6e4775ca 103
la00noix 0:c61c6e4775ca 104 }else if(go_waitmode == 1){
la00noix 0:c61c6e4775ca 105
la00noix 0:c61c6e4775ca 106 MaxonControl(0,0,0,0);
la00noix 0:c61c6e4775ca 107
la00noix 0:c61c6e4775ca 108 }
la00noix 0:c61c6e4775ca 109
la00noix 0:c61c6e4775ca 110
la00noix 0:c61c6e4775ca 111 #endif
la00noix 0:c61c6e4775ca 112 //--------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 113
la00noix 0:c61c6e4775ca 114
la00noix 0:c61c6e4775ca 115
la00noix 0:c61c6e4775ca 116
la00noix 0:c61c6e4775ca 117 //-----本番用自動プログラム（右側フィールド用）-------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 118 #ifdef HARUROBO_RIGHT_MODE
la00noix 0:c61c6e4775ca 119
la00noix 0:c61c6e4775ca 120 //ここに本番用自動プログラム（右側フィールド用）を書く
la00noix 0:c61c6e4775ca 121
la00noix 0:c61c6e4775ca 122 #endif
la00noix 0:c61c6e4775ca 123 //---------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 124
la00noix 0:c61c6e4775ca 125
la00noix 0:c61c6e4775ca 126
la00noix 0:c61c6e4775ca 127 //-----本番用自動プログラム（左側フィールド用）-------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 128 #ifdef HARUROBO_LEFT_MODE //
la00noix 0:c61c6e4775ca 129
la00noix 0:c61c6e4775ca 130 //ここに本番用自動プログラム（左側フィールド用）を書く
la00noix 0:c61c6e4775ca 131
la00noix 0:c61c6e4775ca 132 #endif
la00noix 0:c61c6e4775ca 133 //---------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 134 MaxonControl(0,0,0,0);
la00noix 0:c61c6e4775ca 135 break;
la00noix 0:c61c6e4775ca 136
la00noix 0:c61c6e4775ca 137 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 138 case 0:
la00noix 0:c61c6e4775ca 139
la00noix 0:c61c6e4775ca 140 calc_xy(0,1,1);
la00noix 0:c61c6e4775ca 141 ashi_led();
la00noix 0:c61c6e4775ca 142 MaxonControl(0,0,0,0);
yuki0701 1:3c11e07da92a 143 printf("now_angle: %f, now_x: %f, now_y: %f\n\r",now_angle,now_x,now_y);
la00noix 0:c61c6e4775ca 144
la00noix 0:c61c6e4775ca 145 break;
la00noix 0:c61c6e4775ca 146
la00noix 0:c61c6e4775ca 147 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 148 case 2:
la00noix 0:c61c6e4775ca 149
la00noix 0:c61c6e4775ca 150 ManualOut(250,100,500,200);
la00noix 0:c61c6e4775ca 151
la00noix 0:c61c6e4775ca 152 break;
la00noix 0:c61c6e4775ca 153 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 154 }
la00noix 0:c61c6e4775ca 155 }
la00noix 0:c61c6e4775ca 156 }