勇紀 有岡 / Mbed 2 deprecated NHK2020-main

ver2

Dependencies:   uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P

main.cpp@2:820dcd23c8e3, 2019-12-11 (annotated)

Committer:
yuki0701
Date:
Wed Dec 11 04:59:36 2019 +0000
Revision:
2:820dcd23c8e3
Parent:
1:26fc1b2f1c42 
A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:b87fd8dd4322 1 #include "EC.h"
la00noix 0:b87fd8dd4322 2 #include "R1370P.h"
la00noix 0:b87fd8dd4322 3 #include "move4wheel.h"
la00noix 0:b87fd8dd4322 4 #include "mbed.h"
la00noix 0:b87fd8dd4322 5 #include "math.h"
la00noix 0:b87fd8dd4322 6 #include "PathFollowing.h"
la00noix 0:b87fd8dd4322 7 #include "movement.h"
la00noix 0:b87fd8dd4322 8 #include "manual.h"
la00noix 0:b87fd8dd4322 9 #include "can.h"
la00noix 0:b87fd8dd4322 10
la00noix 0:b87fd8dd4322 11 #define PI 3.141592
la00noix 0:b87fd8dd4322 12
la00noix 0:b87fd8dd4322 13 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
la00noix 0:b87fd8dd4322 14
la00noix 0:b87fd8dd4322 15 int go_waitmode = 0;
la00noix 0:b87fd8dd4322 16
yuki0701 1:26fc1b2f1c42 17 /*------------運動時の各速度の設定----------*/
yuki0701 1:26fc1b2f1c42 18 double st_speed = 500; // 移動開始時の速度
yuki0701 1:26fc1b2f1c42 19 double max_speed = 2500; // 直線移動時の最大速度
yuki0701 1:26fc1b2f1c42 20 double mid_speed = 2000; // 直線移動時の中間速度
yuki0701 1:26fc1b2f1c42 21 double turn_speed = 1000; // 旋回時の速度
yuki0701 1:26fc1b2f1c42 22 double end_speed = 200; // 停止直前の速度
yuki0701 1:26fc1b2f1c42 23 double fruit_speed = 200; // 果物を取る時の速度
yuki0701 1:26fc1b2f1c42 24 /*--------------------------------------*/
yuki0701 1:26fc1b2f1c42 25
la00noix 0:b87fd8dd4322 26 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
la00noix 0:b87fd8dd4322 27 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
la00noix 0:b87fd8dd4322 28 //DigitalOut canread_led(LED2); //canread -> on //can.cpp
la00noix 0:b87fd8dd4322 29 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
la00noix 0:b87fd8dd4322 30
la00noix 0:b87fd8dd4322 31 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
la00noix 0:b87fd8dd4322 32
la00noix 0:b87fd8dd4322 33 int main()
la00noix 0:b87fd8dd4322 34 {
yuki0701 2:820dcd23c8e3 35
yuki0701 2:820dcd23c8e3 36 //UserLoopSetting_maxon();
yuki0701 2:820dcd23c8e3 37 UserLoopSetting_sensor();
yuki0701 2:820dcd23c8e3 38 UserLoopSetting_can();
yuki0701 2:820dcd23c8e3 39 /*
yuki0701 2:820dcd23c8e3 40 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
yuki0701 2:820dcd23c8e3 41
yuki0701 2:820dcd23c8e3 42 printf("ソースファイル名 : %s¥n", __FILE__);
yuki0701 2:820dcd23c8e3 43 printf("作成日付 : %s¥n", __DATE__);
yuki0701 2:820dcd23c8e3 44 printf("作成時刻 : %s¥n", __TIME__);
yuki0701 2:820dcd23c8e3 45
yuki0701 2:820dcd23c8e3 46 #endif
yuki0701 2:820dcd23c8e3 47 */
yuki0701 2:820dcd23c8e3 48 int move_flag = 0;
yuki0701 2:820dcd23c8e3 49 while(1) {
yuki0701 2:820dcd23c8e3 50
yuki0701 2:820dcd23c8e3 51 id1_value[0] = 1;
yuki0701 2:820dcd23c8e3 52 switch(id1_value[0]) {
yuki0701 2:820dcd23c8e3 53 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
yuki0701 2:820dcd23c8e3 54 case 1:
yuki0701 2:820dcd23c8e3 55
yuki0701 2:820dcd23c8e3 56
yuki0701 2:820dcd23c8e3 57 //set_condの引数詳細
yuki0701 2:820dcd23c8e3 58 //xy_type:（0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む）
yuki0701 2:820dcd23c8e3 59 //pm_typeX:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む）
yuki0701 2:820dcd23c8e3 60 //x_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標)
yuki0701 2:820dcd23c8e3 61 //pm_typeY:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む）
yuki0701 2:820dcd23c8e3 62 //y_base:超音波センサーで読む壁の座標(x軸平行の壁のy座標)
yuki0701 2:820dcd23c8e3 63
yuki0701 2:820dcd23c8e3 64
la00noix 0:b87fd8dd4322 65
yuki0701 2:820dcd23c8e3 66 // set_cond(2,0,0,1,0);
yuki0701 2:820dcd23c8e3 67 // int f = 0;
yuki0701 2:820dcd23c8e3 68 // while(1) {
yuki0701 2:820dcd23c8e3 69 // if(f > 20) break;
yuki0701 2:820dcd23c8e3 70 // f++;
yuki0701 2:820dcd23c8e3 71 // printf("wait\n\r");
yuki0701 2:820dcd23c8e3 72 // }
yuki0701 2:820dcd23c8e3 73 //gogo_straight(0,0,0,0,100000,100000,200,800,5,0.1,10,0.1,250,0, 4095, 30);
yuki0701 2:820dcd23c8e3 74 // wait(100000);
yuki0701 2:820dcd23c8e3 75
yuki0701 2:820dcd23c8e3 76 set_cond(2,0,1950,1,0);
yuki0701 2:820dcd23c8e3 77 uwflag_change(1,0,0,1);
yuki0701 2:820dcd23c8e3 78 gogo_straight(1,1,457,457,700,463,300,300,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 79 gogo_straight(0,0,700,463,1300,500,300,150,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 80 mt_stop();
yuki0701 2:820dcd23c8e3 81 wait(0.2);
yuki0701 2:820dcd23c8e3 82 pos_correction(1450,500,0,0,0,100);
yuki0701 2:820dcd23c8e3 83 enc_correction(1,1);
yuki0701 2:820dcd23c8e3 84 wait(2);
yuki0701 2:820dcd23c8e3 85
yuki0701 2:820dcd23c8e3 86 gogo_straight(0,0,1500,500,700,500,300,300,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 87 gogo_straight(0,0,700,500,530,500,300,100,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 88 mt_stop();
yuki0701 2:820dcd23c8e3 89 uwflag_reset();
yuki0701 2:820dcd23c8e3 90 wait(1000);
yuki0701 2:820dcd23c8e3 91
yuki0701 2:820dcd23c8e3 92
yuki0701 2:820dcd23c8e3 93
yuki0701 2:820dcd23c8e3 94 /* 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);
yuki0701 2:820dcd23c8e3 95 gogo_straight(1,1,200,1000,200,2750,800,800,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 96 purecurve(3,1,1,200,2750,-800,3500,9,800,10,0.1,10,0.1,250,-90,4095, 700);
yuki0701 2:820dcd23c8e3 97 purecurve(5,1,1,-800,3500,-1800,2750,9,800,10,0.1,10,0.1,250,-90,4095, 700);
yuki0701 2:820dcd23c8e3 98 gogo_straight(1,1,-1800,2750,-1800,1000,800,500,5,0.1,10,0.1,250,-90, 4095, 500);
yuki0701 2:820dcd23c8e3 99 set_cond(1,0,-465,1,-415);
yuki0701 2:820dcd23c8e3 100 gogo_straight(1,0,-1800,1000,-1800,300,500,100,5,0.1,10,0.1,150,-90, 4095, 50);
yuki0701 2:820dcd23c8e3 101 mt_stop();
yuki0701 2:820dcd23c8e3 102 wait(0.2);
yuki0701 2:820dcd23c8e3 103 enc_correction(0,1);
yuki0701 2:820dcd23c8e3 104 pos_correction(-1800,0,-90,1,1,50);
yuki0701 2:820dcd23c8e3 105 enc_correction2(-1800, 0);
yuki0701 2:820dcd23c8e3 106 uwflag_reset();
la00noix 0:b87fd8dd4322 107
yuki0701 2:820dcd23c8e3 108 wait(2);
yuki0701 2:820dcd23c8e3 109 gogo_straight(1,1,-1800,0,-1800,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30);
yuki0701 2:820dcd23c8e3 110 gogo_straight(1,1,-1800,400,-1800,2750,500,500,5,0.1,10,0.1,250,-90, 4095, 200);
yuki0701 2:820dcd23c8e3 111 purecurve(2,1,1,-1800,2750,-800,3500,9,500,5,0.1,10,0.1,250,0,4095, 300);
yuki0701 2:820dcd23c8e3 112 purecurve(3,1,1,-800,3500,200,2750,9,500,5,0.1,10,0.1,250,0,4095, 300);
yuki0701 2:820dcd23c8e3 113 gogo_straight(1,1,200,2750,200,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 114 gogo_straight(1,1,200,1000,200,200,500,100,5,0.1,10,0.1,250,0, 4095, 30);
yuki0701 2:820dcd23c8e3 115 mt_stop();
yuki0701 2:820dcd23c8e3 116 wait(0.2);
yuki0701 2:820dcd23c8e3 117 pos_correction(0,0,0,1,1,50);*/
yuki0701 2:820dcd23c8e3 118
yuki0701 2:820dcd23c8e3 119
yuki0701 2:820dcd23c8e3 120
yuki0701 2:820dcd23c8e3 121 /* gogo_straight(1,1,0,0,0,400,200,800,5,0.1,10,0.1,250,0, 4095, 30);
yuki0701 2:820dcd23c8e3 122 gogo_straight(1,1,0,400,0,3000,800,800,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 123 purecurve(3,1,1,0,3000,-1000,3750,9,800,5,0.1,10,0.1,250,-90,4095, 500);
yuki0701 2:820dcd23c8e3 124 purecurve(5,1,1,-1000,3750,-2000,3000,9,800,5,0.1,10,0.1,250,-90,4095, 500);
yuki0701 2:820dcd23c8e3 125 gogo_straight(1,1,-2000,3000,-2000,1000,800,800,5,0.1,10,0.1,250,-90, 4095, 500);
yuki0701 2:820dcd23c8e3 126 //////set_cond(2,0,-600,1,-600);
yuki0701 2:820dcd23c8e3 127 gogo_straight(1,1,-2000,1000,-2000,200,800,100,5,0.1,10,0.1,100,-90, 4095, 50);
yuki0701 2:820dcd23c8e3 128 mt_stop();
yuki0701 2:820dcd23c8e3 129 wait(0.2);
yuki0701 2:820dcd23c8e3 130 pos_correction(-2000,0,-90,1,1,50);
yuki0701 2:820dcd23c8e3 131 enc_correction2(-2000, 0);
yuki0701 2:820dcd23c8e3 132
yuki0701 2:820dcd23c8e3 133 wait(2);
yuki0701 2:820dcd23c8e3 134 gogo_straight(1,1,-2000,0,-2000,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30);
yuki0701 2:820dcd23c8e3 135 gogo_straight(1,1,-2000,400,-2000,3000,500,500,5,0.1,10,0.1,250,-90, 4095, 200);
yuki0701 2:820dcd23c8e3 136 purecurve(2,1,1,-2000,3000,-1000,3750,9,500,5,0.1,10,0.1,250,0,4095, 300);
yuki0701 2:820dcd23c8e3 137 purecurve(3,1,1,-1000,3750,0,3000,9,500,5,0.1,10,0.1,250,0,4095, 300);
yuki0701 2:820dcd23c8e3 138 gogo_straight(1,1,0,3000,0,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200);
yuki0701 2:820dcd23c8e3 139 gogo_straight(1,1,0,1000,0,200,500,100,5,0.1,10,0.1,250,0, 4095, 30);
yuki0701 2:820dcd23c8e3 140 mt_stop();
yuki0701 2:820dcd23c8e3 141 wait(0.2);
yuki0701 2:820dcd23c8e3 142 pos_correction(0,0,0,1,1,50);*/
yuki0701 2:820dcd23c8e3 143
yuki0701 2:820dcd23c8e3 144
la00noix 0:b87fd8dd4322 145
yuki0701 2:820dcd23c8e3 146 /* gogo_straight(1,1,0,0,0,400,50,200,5,0.1,10,0.1,250,0, 4095, 30);
yuki0701 2:820dcd23c8e3 147 gogo_straight(1,1,0,400,0,2000,500,500,5,0.1,10,0.1,250,0, 4095, 100);
yuki0701 2:820dcd23c8e3 148 gogo_straight(1,1,0,2000,0,2700,500,100,5,0.1,10,0.1,250,0, 4095, 50);
yuki0701 2:820dcd23c8e3 149 mt_stop();
yuki0701 2:820dcd23c8e3 150 wait(2);
yuki0701 2:820dcd23c8e3 151 pos_correction(0,3000,0,1,1,50);
yuki0701 1:26fc1b2f1c42 152
yuki0701 2:820dcd23c8e3 153 gogo_straight(1,1,0,3000,0,2600,50,500,5,0.1,10,0.1,0,0, 4095,30);
yuki0701 2:820dcd23c8e3 154 gogo_straight(1,1,0,2600,0,1000,500,500,5,0.1,10,0.1,100,0, 4095, 100);
yuki0701 2:820dcd23c8e3 155 gogo_straight(1,1,0,1000,0,300,500,100,5,0.1,10,0.1,0,0, 4095, 50);
yuki0701 2:820dcd23c8e3 156 mt_stop();
yuki0701 2:820dcd23c8e3 157 wait(2);
yuki0701 2:820dcd23c8e3 158 pos_correction(0,0,0,1,1,50);*/
yuki0701 2:820dcd23c8e3 159
yuki0701 2:820dcd23c8e3 160 while(1) mt_stop();
yuki0701 2:820dcd23c8e3 161
yuki0701 2:820dcd23c8e3 162
yuki0701 2:820dcd23c8e3 163 move_flag = 1;
yuki0701 2:820dcd23c8e3 164 break;
la00noix 0:b87fd8dd4322 165
yuki0701 2:820dcd23c8e3 166 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
yuki0701 2:820dcd23c8e3 167 case 0:
la00noix 0:b87fd8dd4322 168
yuki0701 2:820dcd23c8e3 169 calc_xy(0,1,1);
yuki0701 2:820dcd23c8e3 170 ashi_led();
yuki0701 2:820dcd23c8e3 171 //MaxonControl(0,0,0,0);
yuki0701 2:820dcd23c8e3 172 go_waitmode = 0;
la00noix 0:b87fd8dd4322 173
yuki0701 2:820dcd23c8e3 174 break;
yuki0701 2:820dcd23c8e3 175 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
yuki0701 2:820dcd23c8e3 176 case 2:
yuki0701 2:820dcd23c8e3 177
yuki0701 2:820dcd23c8e3 178 ManualOut(250,100,500,200);
yuki0701 2:820dcd23c8e3 179 go_waitmode = 0;
yuki0701 2:820dcd23c8e3 180
yuki0701 2:820dcd23c8e3 181 break;
yuki0701 2:820dcd23c8e3 182 }
yuki0701 2:820dcd23c8e3 183 //------------------------------------------------------------------------------------------------------------------------------------//
yuki0701 1:26fc1b2f1c42 184 if(move_flag == 1)break;
yuki0701 2:820dcd23c8e3 185 }
yuki0701 2:820dcd23c8e3 186
la00noix 0:b87fd8dd4322 187 }