3/20 2:04

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp

Committer:
la00noix
Date:
2019-02-26
Revision:
2:7cba05e70367
Parent:
1:3c11e07da92a
Child:
3:8a0faa3b08c3

File content as of revision 2:7cba05e70367:

#include "EC.h"
#include "R1370P.h"
#include "move4wheel.h"
#include "mbed.h"
#include "math.h"
#include "PathFollowing.h"
#include "movement.h"
#include "maxonsetting.h"
#include "manual.h"
#include "can.h"

#define PI 3.141592

//#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義

#define HARUROBO_TEST_MODE //テスト自動プログラム(練習・動作確認用)使用時に定義
//#define HARUROBO_RIGHT_MODE //本番用自動プログラム(右側フィールド)使用時に定義
//#define HARUROBO_LEFT_MODE //本番用自動プログラム(左側フィールド)使用時に定義

int go_waitmode = 0;

//-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
//DigitalOut canread_led(LED1);  //canread -> on                //can.cpp
//DigitalOut cansend_led(LED2);  //cansend -> on                //can.cpp
//DigitalOut debug_led(LED3);    //maxon debug programme -> on  //maxonsetting.cpp

//////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////

int main(){
    
    UserLoopSetting_maxon();
    UserLoopSetting_sensor();
    UserLoopSetting_can();
      
#ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示

      printf("ソースファイル名 : %s¥n", __FILE__);
      printf("作成日付 : %s¥n", __DATE__);
      printf("作成時刻 : %s¥n", __TIME__);
  
#endif

while(1){
    
    id1_value[0] = 1;
    
    switch(id1_value[0]){
            
//-----auto mode----------------------------------------------------------------------------------------------------------------------//
        case 1:

//-----テスト自動プログラム(練習・動作確認用--------------------------------------------------------------------//
#ifdef HARUROBO_TEST_MODE
        
        if(go_waitmode == 0){
           
           //can_start();
           //set_cond(2,1,-700,1,-700);
           gogo_straight(1,1,-2962,3500,-2962,3000,200,1000,5,0.1,10,0.1,600,0);
           purecurve(7,1,1,-2962,3000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
           purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
           gogo_straight(1,1,-1672,2000,-1672,1500,1000,200,5,0.1,10,0.1,600,0);
           MaxonControl(0,0,0,0);
           set_cond(2,0,-1243,1,1076);
           pos_correction(-1672,1500,0,0,0);
           wait(0.5);

           gogo_straight(1,1,-1672,1500,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
           purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
           purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,90);
           gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,90);
           gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,90);
           MaxonControl(0,0,0,0);
           pos_correction(-2962,4500,90,1,1);
      
           set_cond(0,0,-2462,0,0);
           gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,90);
           MaxonControl(0,0,0,0);
           pos_correction(-2850,4500,90,0,1);
           wait(0.5);
      
           gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,90);
           purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,90);
           purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,90);
           purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,90);
           gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,90);
           set_cond(1,0,0,0,7000);
           gogo_straight(1,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,90);
           MaxonControl(0,0,0,0);
           pos_correction(-2700,6647,90,1,0);
           wait(0.5);
      
           gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,90);
           gogo_straight(1,1,-2500,6647,-1243,6647,1000,1000,5,0.1,10,0.1,800,90);
           purecurve(8,1,1,-1243,6647,-519,6000,9,1000,5,0.1,10,0.1,600,180);
           gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,180);
           set_cond(2,0,0,1,4000);
           gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,180);
           MaxonControl(0,0,0,0);
           pos_correction(-519,4500,180,0,0);

           go_waitmode = 1;
        
        }else if(go_waitmode == 1){
            
        MaxonControl(0,0,0,0);
        
        }
        
        
#endif 
//--------------------------------------------------------------------------------------------------------//




//-----本番用自動プログラム(右側フィールド用)-------------------------------------------------------------------//
#ifdef HARUROBO_RIGHT_MODE

        //ここに本番用自動プログラム(右側フィールド用)を書く

#endif 
//---------------------------------------------------------------------------------------------------------//



//-----本番用自動プログラム(左側フィールド用)-------------------------------------------------------------------//
#ifdef HARUROBO_LEFT_MODE //

        //ここに本番用自動プログラム(左側フィールド用)を書く

#endif 
//---------------------------------------------------------------------------------------------------------//
        MaxonControl(0,0,0,0);
        break;
            
//-----wait mode----------------------------------------------------------------------------------------------------------------------//    
        case 0:
        
        calc_xy(0,1,1);
        ashi_led();
        MaxonControl(0,0,0,0);
        printf("now_angle: %f, now_x: %f, now_y: %f\n\r",now_angle,now_x,now_y);
        
        break;
            
//-----manual mode--------------------------------------------------------------------------------------------------------------------//
        case 2:

        ManualOut(250,100,500,200);
                        
        break;
//------------------------------------------------------------------------------------------------------------------------------------//
    }
}
}