ロボステ6期
あ
Dependencies: uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P
main.cpp
- Committer:
- yuki0701
- Date:
- 2019-12-11
- Revision:
- 2:820dcd23c8e3
- Parent:
- 1:26fc1b2f1c42
File content as of revision 2:820dcd23c8e3:
#include "EC.h"
#include "R1370P.h"
#include "move4wheel.h"
#include "mbed.h"
#include "math.h"
#include "PathFollowing.h"
#include "movement.h"
#include "manual.h"
#include "can.h"
#define PI 3.141592
//#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
int go_waitmode = 0;
/*------------運動時の各速度の設定----------*/
double st_speed = 500; // 移動開始時の速度
double max_speed = 2500; // 直線移動時の最大速度
double mid_speed = 2000; // 直線移動時の中間速度
double turn_speed = 1000; // 旋回時の速度
double end_speed = 200; // 停止直前の速度
double fruit_speed = 200; // 果物を取る時の速度
/*--------------------------------------*/
//-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
//DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
//DigitalOut canread_led(LED2); //canread -> 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
*/
int move_flag = 0;
while(1) {
id1_value[0] = 1;
switch(id1_value[0]) {
//-----auto mode----------------------------------------------------------------------------------------------------------------------//
case 1:
//set_condの引数詳細
//xy_type:（0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む）
//pm_typeX:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む）
//x_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標)
//pm_typeY:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む）
//y_base:超音波センサーで読む壁の座標(x軸平行の壁のy座標)
// set_cond(2,0,0,1,0);
// int f = 0;
// while(1) {
// if(f > 20) break;
// f++;
// printf("wait\n\r");
// }
//gogo_straight(0,0,0,0,100000,100000,200,800,5,0.1,10,0.1,250,0, 4095, 30);
// wait(100000);
set_cond(2,0,1950,1,0);
uwflag_change(1,0,0,1);
gogo_straight(1,1,457,457,700,463,300,300,5,0.1,10,0.1,250,0, 4095, 200);
gogo_straight(0,0,700,463,1300,500,300,150,5,0.1,10,0.1,250,0, 4095, 200);
mt_stop();
wait(0.2);
pos_correction(1450,500,0,0,0,100);
enc_correction(1,1);
wait(2);
gogo_straight(0,0,1500,500,700,500,300,300,5,0.1,10,0.1,250,0, 4095, 200);
gogo_straight(0,0,700,500,530,500,300,100,5,0.1,10,0.1,250,0, 4095, 200);
mt_stop();
uwflag_reset();
wait(1000);
/* spline_move(1, 1, 0, 0, 200,1000, 0,300,200,700,200, 800,5,0.1,10,0.1,500,0, 4095, 500, 10);
gogo_straight(1,1,200,1000,200,2750,800,800,5,0.1,10,0.1,250,0, 4095, 200);
purecurve(3,1,1,200,2750,-800,3500,9,800,10,0.1,10,0.1,250,-90,4095, 700);
purecurve(5,1,1,-800,3500,-1800,2750,9,800,10,0.1,10,0.1,250,-90,4095, 700);
gogo_straight(1,1,-1800,2750,-1800,1000,800,500,5,0.1,10,0.1,250,-90, 4095, 500);
set_cond(1,0,-465,1,-415);
gogo_straight(1,0,-1800,1000,-1800,300,500,100,5,0.1,10,0.1,150,-90, 4095, 50);
mt_stop();
wait(0.2);
enc_correction(0,1);
pos_correction(-1800,0,-90,1,1,50);
enc_correction2(-1800, 0);
uwflag_reset();
wait(2);
gogo_straight(1,1,-1800,0,-1800,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30);
gogo_straight(1,1,-1800,400,-1800,2750,500,500,5,0.1,10,0.1,250,-90, 4095, 200);
purecurve(2,1,1,-1800,2750,-800,3500,9,500,5,0.1,10,0.1,250,0,4095, 300);
purecurve(3,1,1,-800,3500,200,2750,9,500,5,0.1,10,0.1,250,0,4095, 300);
gogo_straight(1,1,200,2750,200,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200);
gogo_straight(1,1,200,1000,200,200,500,100,5,0.1,10,0.1,250,0, 4095, 30);
mt_stop();
wait(0.2);
pos_correction(0,0,0,1,1,50);*/
/* gogo_straight(1,1,0,0,0,400,200,800,5,0.1,10,0.1,250,0, 4095, 30);
gogo_straight(1,1,0,400,0,3000,800,800,5,0.1,10,0.1,250,0, 4095, 200);
purecurve(3,1,1,0,3000,-1000,3750,9,800,5,0.1,10,0.1,250,-90,4095, 500);
purecurve(5,1,1,-1000,3750,-2000,3000,9,800,5,0.1,10,0.1,250,-90,4095, 500);
gogo_straight(1,1,-2000,3000,-2000,1000,800,800,5,0.1,10,0.1,250,-90, 4095, 500);
//////set_cond(2,0,-600,1,-600);
gogo_straight(1,1,-2000,1000,-2000,200,800,100,5,0.1,10,0.1,100,-90, 4095, 50);
mt_stop();
wait(0.2);
pos_correction(-2000,0,-90,1,1,50);
enc_correction2(-2000, 0);
wait(2);
gogo_straight(1,1,-2000,0,-2000,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30);
gogo_straight(1,1,-2000,400,-2000,3000,500,500,5,0.1,10,0.1,250,-90, 4095, 200);
purecurve(2,1,1,-2000,3000,-1000,3750,9,500,5,0.1,10,0.1,250,0,4095, 300);
purecurve(3,1,1,-1000,3750,0,3000,9,500,5,0.1,10,0.1,250,0,4095, 300);
gogo_straight(1,1,0,3000,0,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200);
gogo_straight(1,1,0,1000,0,200,500,100,5,0.1,10,0.1,250,0, 4095, 30);
mt_stop();
wait(0.2);
pos_correction(0,0,0,1,1,50);*/
/* gogo_straight(1,1,0,0,0,400,50,200,5,0.1,10,0.1,250,0, 4095, 30);
gogo_straight(1,1,0,400,0,2000,500,500,5,0.1,10,0.1,250,0, 4095, 100);
gogo_straight(1,1,0,2000,0,2700,500,100,5,0.1,10,0.1,250,0, 4095, 50);
mt_stop();
wait(2);
pos_correction(0,3000,0,1,1,50);
gogo_straight(1,1,0,3000,0,2600,50,500,5,0.1,10,0.1,0,0, 4095,30);
gogo_straight(1,1,0,2600,0,1000,500,500,5,0.1,10,0.1,100,0, 4095, 100);
gogo_straight(1,1,0,1000,0,300,500,100,5,0.1,10,0.1,0,0, 4095, 50);
mt_stop();
wait(2);
pos_correction(0,0,0,1,1,50);*/
while(1) mt_stop();
move_flag = 1;
break;
//-----wait mode----------------------------------------------------------------------------------------------------------------------//
case 0:
calc_xy(0,1,1);
ashi_led();
//MaxonControl(0,0,0,0);
go_waitmode = 0;
break;
//-----manual mode--------------------------------------------------------------------------------------------------------------------//
case 2:
ManualOut(250,100,500,200);
go_waitmode = 0;
break;
}
//------------------------------------------------------------------------------------------------------------------------------------//
if(move_flag == 1)break;
}
}