MEC-B
/
AR_MastarNode_copy
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: DriveConroller IMU MDD Mycan Odometer PID RotaryEncoder UART USS mbed
Fork of
AR_MastarNode
by 
MEC-B
Diff: main.cpp
- Revision:
- 7:1ee46b2e8dce
- Parent:
- 6:a102603c99fd
- Child:
- 8:123cd1f07aea
--- a/main.cpp Fri Aug 17 08:00:09 2018 +0000
+++ b/main.cpp Mon Aug 20 08:23:55 2018 +0000
@@ -12,33 +12,50 @@
#include "USS.h"
#include "hardwareConfig.h"
-
-//(X, Y, θ, speed, angle, injection, 補給昇降)
-float position[19][6] = {{0.0, 0.0, 0.0, 0, 90, 0},//初期位置
- {-1.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化
- {-1.825, 18.0, 0.0, 0, 82, 0},//y移動
- {-1.825, 18.0, 0.0, 35, 82, 0},//発射
- {-1.825, 0.0, 0.0, 0, 88, 1},//y移動,角度戻す, P上昇
+//(X, Y, θ, speed, angle)
+float position[19][6] = {{-1.825, 0.0, 0.0, 0.0, 0, 82},//0, x移動, 角度変化
+ {-1.825, 0.0, 0.0, 18.0, 0, 82},//1,y移動
+ {-1.825, 0.0, 0.0, 18.0, 35, 82},//2,発射
+ {-1.825, 0.0, 0.0, 0.0, 0, 88},//3,y移動,角度戻す, P上昇
+
+ {-2.825, 0.0, 0.0, 0.0, 0, 88},//4, x移動, 角度変化, 下降&バットマン駆動
+ {-2.825, 0.0, 0.0, 18.0, 0, 82},//5,y移動
+ {-2.825, 0.0, 0.0, 18.0, 35, 82},//6,発射
+ {-2.825, 0.0, 0.0, 0.0, 0, 88},//7,y移動,角度戻す, P上昇
+
+ {-3.825, 0.0, 0.0, 0.0, 0, 88},//8,x移動, 角度変化, 下降&バットマン駆動
+ {-3.825, 0.0, 0.0, 18.0, 0, 82},//9,y移動
+ {-3.825, 0.0, 0.0, 18.0, 35, 82},//10,発射
+ {-3.825, 0.0, 0.0, 0.0, 0, 88},//11,y移動,角度戻す, P上昇
- {-2.825, 0.0, 0.0, 0, 88, 1},//x移動, 角度変化, 下降&バットマン駆動
- {-2.825, 18.0, 0.0, 0, 82, 0},//y移動
- {-2.825, 18.0, 0.0, 35, 82, 0},//発射
- {-2.825, 0.0, 0.0, 0, 88, 1},//y移動,角度戻す, P上昇
+ {-0.825, 0.0, 0.0, 0.0, 0, 88},//12,x移動, 角度変化, 下降&バットマン駆動
+ {-0.825, 0.0, 0.0, 18.0, 0, 82},//13,y移動
+ {-0.825, 0.0, 0.0, 18.0, 18, 82},//14,下段に発射
+ {-0.825,0.0, 0.0, 18.0, 0, 88},//15,角度戻す, P上昇
+ {-0.825, 0.0, 0.0, 18.0, 0, 85},//16,角度変化, 下降&バットマン駆動
+ {-0.825, 0.0, 0.0, 18.0, 18, 85},//17,上段に発射
+ /*
+ {-0.825, 0.0, 90.0, 0.0, 0, 88},//18
- {-3.825, 0.0, 0.0, 0, 88, 1},//x移動, 角度変化, 下降&バットマン駆動
- {-3.825, 18.0, 0.0, 0, 82, 0},//y移動
- {-3.825, 18.0, 0.0, 35, 82, 0},//発射
- {-3.825, 0.0, 0.0, 0, 88, 1},//y移動,角度戻す, P上昇
+ {-0.5, 0.0, 90.0, 0.0, 0, 88},//19
+ {-0.5, 1.125, 90.0, 0.0, 0, 88},//20
+ {-0.5, 1.125, 90.0, 18.0, 0, 82},//21
+ {-0.5, 1.125, 90.0, 18.0, 30, 82},//22
+ {-0.5, 1.125, 90.0, 0.0, 0, 88},//23
- {-0.825, 0.0, 0.0, 0, 88, 1},//x移動, 角度変化, 下降&バットマン駆動
- {-0.825, 18.0, 0.0, 0, 82, 0},//y移動
- {-0.825, 18.0, 0.0, 18, 82, 0},//下段に発射
- {-0.825, 18.0, 0.0, 0, 88, 1},//角度戻す, P上昇
- {-0.825, 18.0, 0.0, 0, 85, 0},//角度変化, 下降&バットマン駆動
- {-0.825, 18.0, 0.0, 18, 85, 0},//上段に発射
+ {-0.5, 1.625, 90.0, 0.0, 0, 88},//24
+ {-0.5, 1.625, 90.0, 18.0, 0, 82},//25
+ {-0.5, 1.625, 90.0, 18.0, 30, 82},//26
+ {-0.5, 1.625, 90.0, 0.0, 0, 88},//27
+
+ {-0.5, 2.125, 90.0, 0.0, 0, 88},//28
+ {-0.5, 2.125, 90.0, 18.0, 0, 82},//29
+ {-0.5, 2.125, 90.0, 18.0, 30, 82},//30
+ {-0.5, 2.125, 90.0, 0.0, 0, 88},//31
+ */
+ {-0.0, 0.0, 0.0, 0, 0.0, 90}//18
};
-controller getPropoData();
bool isConvergetnceTops();
//x軸補正用 PID
@@ -84,20 +101,16 @@
odm.setupOdometerSensors(encoders, &imu.angle[2]);
odm.startComputingOdometry(0.005, 0, 0, 0);
- //オドメーターX
+ //PID設定
pidRobotX.sensor = &odm.x;
pidRobotX.target = &target_x;
pidRobotX.start();
-
- //オドメーターY
pidRobotY.sensor = &odm.y;
pidRobotY.target = &target_y;
pidRobotY.start();
-
pidRobotYow.sensor = &imu.angle[2];
pidRobotYow.target = &target_yow;
pidRobotYow.start();
-
pidUss.sensor = &uss.distance;
pidUss.target = &target_uss;
pidUss.start();
@@ -112,18 +125,32 @@
sw1.mode(PullUp);
sw2.mode(PullUp);
+ while(sw1 == 0);
while(1)
{
- controller cmd = getPropoData();
+ can.read();
+ int supply_pid = can.get(2, 1);
+ int supply_wait = can.get(2, 2);
+ int angle_wait = can.get(3, 5);
+ int sw[8] = {can.get(4, 1),
+ can.get(4, 2),
+ can.get(4, 3),
+ can.get(4, 4),
+ can.get(4, 5),
+ can.get(4, 6),
+ can.get(4, 7),
+ can.get(4, 8)
+ };
- can.set(1, 1, int(position[posi_num][3]));
- can.set(1, 2, int(position[posi_num][4]));
- can.set(1, 3, int(position[posi_num][5]));
+ can.set(1, 1, int(position[posi_num][4]));
+ can.set(1, 2, int(position[posi_num][5]));
+ if(supply_wait == 1 && angle_wait == 1)
+ can.set(1, 3, 1);
+ else can.set(1, 3, 0);
+
while(can.send() == 0);
- can.read();
- //int supply_pid = can.get(2, 1);
-
+ //ポジション収束判定
if(pidRobotX.isConvergence(1) == 1
&& pidRobotYow.isConvergence(1) == 1)
{
@@ -136,11 +163,27 @@
}
//超音波使わないとき
else if(pidRobotY.isConvergence(1) == 1)
- posi_num++;
+ {
+ if(posi_num == 2 || posi_num == 6 || posi_num == 10 || posi_num == 14 || posi_num == 17)
+ {
+ if(supply_pid == 1)
+ posi_num++;
+ }
+ else posi_num++;
+ }
}
-
- if(posi_num >= 19)
- posi_num = 0;
+ if(sw[0] == 1 && posi_num == 0)
+ posi_num = 4;
+ if(sw[1] == 1 && posi_num == 4)
+ posi_num = 8;
+ if(sw[2] == 1 && posi_num == 8)
+ posi_num = 12;
+ if(sw[3] == 1 && sw[4] == 1 && posi_num == 12)
+ posi_num = 14;
+ if(sw[3] == 1 && posi_num == 14)
+ posi_num = 17;
+ if(sw[4] == 1 && posi_num == 17)
+ posi_num = 18;
//ロボットの移動速度(LX, LY, RX)
float robot_velocity[3];
@@ -154,9 +197,9 @@
robot_velocity[0] = pidRobotX.output;
//USS距離調整処理
- if(position[posi_num][1] >= 10)
+ if(position[posi_num][3] >= 10)
{
- *pidUss.target = position[posi_num][1] - 10;
+ *pidUss.target = position[posi_num][3] - 10;
robot_velocity[1] = -pidUss.output;
}
else
@@ -182,45 +225,12 @@
pc.printf("%.2f\t", odm.y);
pc.printf("%.2f\t", imu.angle[2]);
pc.printf("%.2f\t", uss.distance);
- pc.printf("%.2f\t", *odm.rotations[0]);
- pc.printf("%.2f\t", *odm.rotations[1]);
pc.printf("\n");
wait(0.02);
}
}
-controller getPropoData()
-{
- float dead_zone = 0.05;
- controller propo;
- sbus.isFailSafe();
-
- if(sbus.isFailSafe())
- {
- propo.LX = propo.LY = propo.RX = propo.RY = 0;
- propo.H = propo.A = propo.D = propo.F = propo.G = propo.fail_safe = 0;
- }
- else
- {
- propo.LX = sbus.getStickVal(0) / 255.0;
- propo.LY = sbus.getStickVal(1) / 255.0;
- propo.RX = -sbus.getStickVal(2) / 255.0;
- propo.RY = sbus.getStickVal(3) / 255.0;
- propo.H = sbus.getSwitchVal(0);
- propo.C = sbus.getSwitchVal(1);
- propo.E = sbus.getSwitchVal(2);
- propo.F = sbus.getSwitchVal(3);
- propo.G = sbus.getSwitchVal(4);
- propo.fail_safe = 1;
- }
- if(propo.RX < dead_zone && propo.RX > -dead_zone) propo.RX = 0;
- if(propo.RY < dead_zone && propo.RY > -dead_zone) propo.RY = 0;
- if(propo.LX < dead_zone && propo.LX > -dead_zone) propo.LX = 0;
- if(propo.LY < dead_zone && propo.LY > -dead_zone) propo.LY = 0;
- return propo;
-}
-
bool isConvergetnceTops()
{
int velocity_pid = can.get(3, 1);