go-roboB
master
Dependencies: arrc_mbed
Diff: main.cpp
- Revision:
- 3:a35f7cab9de2
- Parent:
- 2:2feb9be14c08
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);