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

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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "EC.h"
00002 #include "R1370P.h"
00003 #include "move4wheel.h"
00004 #include "mbed.h"
00005 #include "math.h"
00006 #include "PathFollowing.h"
00007 #include "movement.h"
00008 #include "maxonsetting.h"
00009 #include "manual.h"
00010 #include "can.h"
00011 
00012 #define PI 3.141592
00013 
00014 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
00015 #define HARUROBO_TEST_MODE
00016 //#define HARUROBO_MAIN_MODE
00017 
00018 int go_waitmode = 0;
00019 
00020 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
00021 //DigitalOut cansend_led(LED1);  //canread -> on                //can.cpp
00022 //DigitalOut canread_led(LED2);  //cansend -> on                //can.cpp
00023 //DigitalOut debug_led(LED3);    //maxon debug programme -> on  //maxonsetting.cpp
00024 
00025 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
00026 
00027 int main()
00028 {
00029     UserLoopSetting_maxon();
00030     UserLoopSetting_sensor();
00031     UserLoopSetting_can();
00032 
00033 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
00034 
00035     printf("ソースファイル名 : %s¥n", __FILE__);
00036     printf("作成日付 : %s¥n", __DATE__);
00037     printf("作成時刻 : %s¥n", __TIME__);
00038 
00039 #endif
00040 
00041 #ifdef HARUROBO_TEST_MODE
00042 
00043     id1_value[0] = 1;
00044     
00045     //while(1);
00046 
00047     gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
00048     purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
00049     purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
00050     set_cond(2,0,-1050,1,1076);
00051     gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
00052     MaxonControl(0,0,0,0);
00053     pos_correction(-1672,1400,0,0,0);
00054     enc_correction(1,1);
00055     wait(0.5);
00056 
00057     gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
00058     purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
00059     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);
00060     gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);    
00061     gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
00062     MaxonControl(0,0,0,0);
00063     pos_correction(-2962,4500,-90,1,1);
00064     set_cond(2,0,-2462,0,6000);
00065     gogo_straight(0,0,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
00066     MaxonControl(0,0,0,0);
00067     pos_correction(-2850,4500,-90,0,0);
00068     enc_correction(1,1);
00069     wait(0.5);
00070 
00071     gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
00072     purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
00073     purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
00074     purecurve(3,1,1,-1700,6000,-2257,6550,9,1000,5,0.1,10,0.1,800,-90);
00075     gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90);
00076     set_cond(2,1,-3500,1,6050);
00077     gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90);
00078     MaxonControl(0,0,0,0);
00079     pos_correction(-2700,6600,-90,0,0);
00080     enc_correction(1,1);
00081     wait(0.5);
00082 
00083     gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90);
00084     gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90);
00085     gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90);
00086     purecurve(8,1,1,-1000,6550,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
00087     gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
00088     set_cond(2,1,-1000,1,4000);
00089     gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
00090     MaxonControl(0,0,0,0);
00091     pos_correction(-519,4500,-180,0,0);
00092     enc_correction(1,1);
00093     MaxonControl(0,0,0,0);
00094 
00095 //以下自動で初期位置に戻るプログラム
00096     wait(3);
00097     
00098     gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180);
00099     gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180);
00100     gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180);
00101     MaxonControl(0,0,0,0);
00102     set_cond(2,0,0,0,7000);
00103     pos_correction(-519,6600,-180,0,0);
00104     enc_correction(1,1);
00105     gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180);
00106     purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180);
00107     purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180);
00108     gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180);
00109     MaxonControl(0,0,0,0);
00110     set_cond(2,1,-3500,0,6000);
00111     pos_correction(-2962,5500,-180,0,0);
00112     enc_correction(1,1);
00113     gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90);
00114     gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0);
00115     gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0);
00116     MaxonControl(0,0,0,0);
00117     set_cond(2,1,-3500,1,2000);
00118     pos_correction(-3000,3500,0,0,0);
00119     enc_correction(1,1);
00120     MaxonControl(0,0,0,0);
00121     
00122 #endif
00123 
00124 #ifdef HARUROBO_MAIN_MODE
00125     while(1) {
00126 
00127         switch(id1_value[0]) {
00128 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
00129             case 1:
00130 
00131                 switch(id1_value[6]) {
00132                     case 0:
00133                         //-----right mode-------------------------------------------------------------------------------------------------------------//
00134                         if(T1 == 0) {
00135 
00136                             //スタート位置からみかんの木まで移動
00137                             wait(0.5);
00138                             T1++;
00139 
00140                         }
00141                         if(T1 == 1) {
00142                             while(1) {
00143                                 wait(0.5);
00144                                 if(T1 == 2) {
00145                                     break;
00146                                 }
00147                             }
00148                         }
00149                         if(T1 == 2) {
00150 
00151                             //みかんの木から三宝置き場まで移動
00152                             T1++;
00153                         }
00154                         if(T1 == 3) {
00155                             while(1) {
00156                                 wait(0.5);
00157                                 if(T1 == 4) {
00158                                     break;
00159                                 }
00160                             }
00161                         }
00162                         if(T1 == 4) {
00163 
00164                             //三宝置き場からりんごの木まで移動
00165                             wait(0.5);
00166                             T1++;
00167                         }
00168                         if(T1 == 5) {
00169                             while(1) {
00170 
00171                                 wait(0.5);
00172                                 if(T1 == 6) {
00173                                     break;
00174                                 }
00175                             }
00176                         }
00177                         if(T1 == 6) {
00178 
00179                             //りんごの木からお供え台まで移動
00180                             wait(0.5);
00181                             T1++;
00182                         }
00183                         break;
00184                     case 1:
00185                         //-----left mode--------------------------------------------------------------------------------------------------------------//
00186 
00187                         break;
00188                 }
00189 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
00190             case 0:
00191 
00192                 calc_xy(0,1,1);
00193                 ashi_led();
00194                 MaxonControl(0,0,0,0);
00195                 go_waitmode = 0;
00196 
00197                 break;
00198 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
00199             case 2:
00200 
00201                 ManualOut(250,100,500,200);
00202                 go_waitmode = 0;
00203 
00204                 break;
00205         }
00206 //------------------------------------------------------------------------------------------------------------------------------------//
00207     }
00208 
00209 #endif
00210 
00211 }
Generated on Wed Jul 13 2022 03:13:25 by  doxygen 1.7.2