master
Dependencies: arrc_mbed
Revision 3:a35f7cab9de2, committed 2022-04-15
- Comitter:
- darkumatar
- Date:
- Fri Apr 15 08:11:58 2022 +0000
- Parent:
- 2:2feb9be14c08
- Commit message:
- Is this OK?
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 2feb9be14c08 -r a35f7cab9de2 main.cpp --- a/main.cpp Wed Apr 13 09:22:18 2022 +0000 +++ b/main.cpp Fri Apr 15 08:11:58 2022 +0000 @@ -1,92 +1,90 @@ -#include "mbed.h" -#include"scrp_slave.hpp" -#include "rotary_inc.hpp" -#include"cmath" -#include"gy521.hpp" -#include "neopixel.h" +#include "mbed.h" //mbed +#include"scrp_slave.hpp" //通信規格scrのファイル +#include "rotary_inc.hpp" //モーターのhファイル +#include"cmath" //sin cos計算などをするためのhファイル +#include"gy521.hpp" //ジャイロセンサのためのhファイル +#include "neopixel.h" //テープLEDのためのやつ -bool the_left=false; +bool the_left=true; //フィールドの左側でやるか 右側でやるかで変える -double goal_x = -17700.0; +double goal_x = -17700.0; //ゴールの位置の座標 今は仮に右側の値が代入されている double goal_y = 28800.0; -int goal_x2 = -8000000; +int goal_x2 = -8000000; //発射時のロボットの向きを決める変数 アークタンジェントで計算してる int goal_y2 = 10000000; -#define goal_x_right -17700.0 +#define goal_x_right -17700.0 //右側の時の設定集(ゴール位置 向き) #define goal_y_right 28800.0//23800.0 #define goal_x2_right -8000000 #define goal_y2_right 10000000 -#define goal_x_left -17700.0 -#define goal_y_left -23800.0 +#define goal_x_left -17700.0 //左側の時の設定集(ゴール位置 向き) +#define goal_y_left -25800.0//-23800.0 #define goal_x2_left -8000000 #define goal_y2_left -8000000 -#define period_r 8000.0 -NeoPixelOut npx(PB_5,30); +//#define period_r 8000.0 //今は もう 使ってない +NeoPixelOut npx(PB_5,36); //テープLEDの使用宣言 I2C i2d(PB_3,PB_10); GY521 gyro(i2d); -ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);//srcslave設定 +ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800); //通信規格宣言 RotaryInc v[4]{RotaryInc(PC_5,PA_12,1,256,4),RotaryInc(PA_9,PA_8,1,256,4),RotaryInc(PC_3,PC_2,1,256,4),RotaryInc(PC_11,PC_10,1,256,4)};//ロリコンピン RotaryInc rote[4]{RotaryInc(PA_15,PA_14,1,256,4),RotaryInc(PA_7,PA_6,1,256,4),RotaryInc(PC_1,PC_0,1,256,4),RotaryInc(PA_13,PC_4,1,256,4)};//読み取り用ピン -int target[4];//target変数 この変数の値を目指すように動く -bool auto_swich = false; -bool limit_switch = true; -int step=0; -int auto_mode=0; -int side_change=0; +int target[4]; //target変数 この変数の値を目指すように動く +bool auto_swich = false; //auto_swich true=自動モード false=手動モード +bool limit_switch = true; //limit_swich これがonの間 このプログラムが動く +int step=0; //自動操縦の時の移行 +int auto_mode=0; //通信状態のチェック 不安定時 limit_swichをoff +int side_change=0; //左か右かのときでの初期設定 たしか、1の時が左 0の時が右 -int ty=0; +int ty=0; //テープLEDの色変えに使う 絶対モードか相対モードか -int direct_xx=0; -int direct_yy=0; -int direct_turn=0; +int direct_xx=0; //絶対操作時の 上になんぼいけばいいかの変数(だいたい-64~64) +int direct_yy=0; //絶対操作時の 横になんぼいけばいいかの変数(だいたい-64~64) +int direct_turn=0; //絶対操作時の 回転 -DigitalOut Led1(PB_2); -DigitalOut Led2(PC_6); -DigitalOut Led3(PB_15); -DigitalOut Led4(PA_10); -int check_tepu=0; +DigitalOut Led1(PB_2); //LED 非常停止中かどうか +DigitalOut Led2(PC_6); //LED このマイコンが起動してるかどうか +DigitalOut Led3(PB_15); //LED 通信が途絶えて非常がかかったかどうか +DigitalOut Led4(PA_10); //LED 絶対モードか相対モードか +int check_tepu=0; // 通信状態のチェック 途絶えてたら切る -bool limi=true; -bool liset_abc=false; +bool limi=true; //用途不明 名残り説 +bool liset_abc=false; //位置リセット -DigitalIn limitB(PB_12,PullUp); +DigitalIn limitB(PB_12,PullUp); //今のところ 用途無し -bool add(int id,int ppp){//速度を変えるやつ +bool add(int id,int ppp){ //速度を変えるやつ Led4.write(0); - if(auto_swich==false&&limi==true){ + if(auto_swich==false&&limi==true){ target[id]=30*ppp; } return true; } -bool auto_on(int id,int ppp){//おまけで作ったやつ スピードを決めてその方向へ走る +bool auto_on(int id,int ppp){// if(ppp==1){ auto_swich=true; - step=0; } return true; } -bool auto_off(int id,int ppp){//おまけで作ったやつ スピードを決めてその方向へ走る +bool auto_off(int id,int ppp){// if(ppp==1){ auto_swich=false; - step=0; } return true; } -bool pro2(){ //アングルとスピードを同時に決める 下から3桁で角度を決める(0~360) それ以外でスピードを決める これなに?私もわからん +bool pro2(){ //アングルとスピードを同時に決める 下から3桁で角度を決める(0~360) それ以外でスピードを決める double anglert=-135.000000000+180.0*side_change; Led4.write(1); for(int i=0;i<4;i++){ @@ -260,6 +258,9 @@ double angle=0; double goal_angle1=0; Led1.write(1); + + + while(limit_switch==true) { angle=gyro.yaw-90.00000; if(auto_swich==true){ @@ -274,21 +275,8 @@ } - if(step==0&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))>1000)|| (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))<-1000))){ + if(((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))>1000)|| (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))<-1000))){ pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period)); - }else if(step>=0&&step<60){ - for(i=0;i<4;i++){ - target[i]=0; - motor_p[i]=0; - motor_m[i]=0; - } - - pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period)); - }else if(step==60&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)>1000)|| (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)<-1000))){ - - - }else { - auto_swich=false; } @@ -323,22 +311,22 @@ } //slave.port2.printf("%d\n",1); - while(name.read_ms()<30){} + //while(name.read_ms()<30){} //slave.send1(255,16,x_period*1); - while(name.read_ms()<60){} + //while(name.read_ms()<60){} //slave.send1(255,17,y_period*1); while(name.read_ms()<100){} //slave.send1(255,18,gyro.yaw*1); - for(i = 0; i < 10; i++){ + for(i = 0; i < 12; i++){ npx.global_scale = 0.05f; npx.normalize = false; if(ty==0){ - npx.setPixelColor(i,0xFF0000); + npx.setPixelColor(i,0x0000FF); }else { - npx.setPixelColor(i,0x0000FF); + npx.setPixelColor(i,0xFF0000); } } - for(i = 10; i < 20; i++){ + for(i = 12; i < 24; i++){ npx.global_scale = 0.05f; npx.normalize = false; if(auto_swich){ @@ -347,7 +335,7 @@ npx.setPixelColor(i,0x00FF00); } } - for(i = 20; i < 30; i++){ + for(i = 24; i < 36; i++){ npx.global_scale = 0.05f; npx.normalize = false; if(side_change==0){ @@ -373,6 +361,9 @@ gyro.update(); name.reset(); } + + + while(1){ Led3.write(1); for(i=0;i<4;i++){ @@ -383,7 +374,7 @@ npx.normalize = false; npx.setPixelColor(i,0x00FFFF); } - for(i=4;i<30;i++){ + for(i=4;i<36;i++){ npx.global_scale = 0.05f; npx.normalize = false; npx.setPixelColor(i,0x00FFFF);