勇紀 有岡
ver2
Dependencies: uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P
Diff: main.cpp
- Revision:
- 1:26fc1b2f1c42
- Parent:
- 0:b87fd8dd4322
- Child:
- 2:820dcd23c8e3
diff -r b87fd8dd4322 -r 26fc1b2f1c42 main.cpp
--- a/main.cpp Tue Mar 19 13:20:23 2019 +0000
+++ b/main.cpp Sat Nov 16 06:26:57 2019 +0000
@@ -5,7 +5,6 @@
#include "math.h"
#include "PathFollowing.h"
#include "movement.h"
-#include "maxonsetting.h"
#include "manual.h"
#include "can.h"
@@ -15,6 +14,15 @@
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
@@ -24,110 +32,63 @@
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) {
-
- switch(id1_value[0]) {
-//-----auto mode----------------------------------------------------------------------------------------------------------------------//
- case 1:
+
+ //UserLoopSetting_maxon();
+ UserLoopSetting_sensor();
+ UserLoopSetting_can();
+/*
+ #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
- switch(id1_value[6]) {
- case 0:
- //-----right mode-------------------------------------------------------------------------------------------------------------//
- flag = 0;
- if(T1 == 0) { //スタート位置から少し移動
-
- wait(0.5);
- T1++;
- }
- if(T1 == 1) { //みかんの木まで移動して掴む
-
- wait(0.5);
- T1++;
- }
-
- if(T1 == 2) { //みかんを持ち上げる
- while(1) {
- wait(0.1);
- if(T1 == 3) {
- break;
- }
- }
- }
- if(T1 == 3) { //みかんの木から三宝置き場まで移動
+ printf("ソースファイル名 : %s¥n", __FILE__);
+ printf("作成日付 : %s¥n", __DATE__);
+ printf("作成時刻 : %s¥n", __TIME__);
- wait(0.5);
- T1++;
- }
- if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる
- while(1) {
- wait(0.1);
- if(T1 == 5) {
- break;
- }
- }
- }
- if(T1 == 5) { //三宝置き場からりんごの木まで移動
-
- wait(0.5);
- T1++;
- }
- if(T1 == 6) { //りんごを持ち上げる
- while(1) {
- wait(0.1);
- if(T1 == 7) {
- break;
- }
- }
- }
- if(T1 == 7) { //りんごの木からお供え台まで移動
+ #endif
+ */
+ int move_flag = 0;
+ while(1) {
+
+ id1_value[0] = 1;
+ switch(id1_value[0]) {
+ //-----auto mode----------------------------------------------------------------------------------------------------------------------//
+ case 1:
+
+ // gogo_straight(1,1,0,0,200,0,50,500,5,0.1,10,0.1,50,0);
+// gogo_straight(1,1,200,0,800,0,500,500,5,0.1,10,0.1,50,0);
+// gogo_straight(1,1,800,0,1000,0,500,200,5,0.1,10,0.1,50,0);
+// mt_stop();
+// pos_correction(1000,0,0,1,1);
+// mt_stop();
+// wait(1.5);
+// gogo_straight(1,1,1000,0,800,0,200,500,5,0.1,10,0.1,50,0);
+// gogo_straight(1,1,800,0,200,0,500,500,5,0.1,10,0.1,50,0);
+// gogo_straight(1,1,200,0,0,0,500,200,5,0.1,10,0.1,50,0);
+// mt_stop();
+// pos_correction(0,0,0,1,1);
+ pos_correction(50,0,0,1,1);
+ mt_stop();
+ move_flag = 1;
+ break;
- wait(0.5);
- T1++;
- }
- if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く
- while(1) {
- printf("ashi finished\n\r");
- MaxonControl(0,0,0,0);
- if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
- if(id1_value[6] != flag)break;
- }
- }
- break;
- case 1:
- //-----left mode--------------------------------------------------------------------------------------------------------------//
- flag = 1;
+ //-----wait mode----------------------------------------------------------------------------------------------------------------------//
+ case 0:
+
+ calc_xy(0,1,1);
+ ashi_led();
+ //MaxonControl(0,0,0,0);
+ go_waitmode = 0;
- break;
- }
-//-----wait mode----------------------------------------------------------------------------------------------------------------------//
- case 0:
+ break;
+ //-----manual mode--------------------------------------------------------------------------------------------------------------------//
+ case 2:
- calc_xy(0,1,1);
- ashi_led();
- MaxonControl(0,0,0,0);
- go_waitmode = 0;
+ ManualOut(250,100,500,200);
+ go_waitmode = 0;
- break;
-//-----manual mode--------------------------------------------------------------------------------------------------------------------//
- case 2:
-
- ManualOut(250,100,500,200);
- go_waitmode = 0;
-
- break;
- }
-//------------------------------------------------------------------------------------------------------------------------------------//
- }
+ break;
+ }
+ //------------------------------------------------------------------------------------------------------------------------------------//
+ if(move_flag == 1)break;
+ }
+
}
\ No newline at end of file