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:
- 5:7c5e07260e1e
- Parent:
- 4:9f74525eb37f
- Child:
- 6:a102603c99fd
--- a/main.cpp Thu Aug 09 00:21:35 2018 +0000
+++ b/main.cpp Sun Aug 12 02:26:00 2018 +0000
@@ -16,26 +16,26 @@
//(X, Y, θ, speed, angle, injection, 補給昇降)
float position[19][6] = {{0.0, 0.0, 0.0, 0, 90, 0},//初期位置
{-3.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化
- {-3.825, 20.0, 0.0, 0, 82, 0},//y移動
- {-3.825, 20.0, 0.0, 35, 82, 0},//発射
- {-3.825, 0.0, 0.0, 0, 90, 0},//y移動,角度戻す, P上昇
+ {-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上昇
{-4.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化, 下降&バットマン駆動
- {-4.825, 20.0, 0.0, 0, 82, 0},//y移動
- {-4.825, 20.0, 0.0, 35, 82, 0},//発射
- {-4.825, 0.0, 0.0, 0, 90, 1},//y移動,角度戻す, P上昇
+ {-4.825, 18.0, 0.0, 0, 82, 0},//y移動
+ {-4.825, 18.0, 0.0, 35, 82, 0},//発射
+ {-4.825, 0.0, 0.0, 0, 88, 1},//y移動,角度戻す, P上昇
{-5.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化, 下降&バットマン駆動
- {-5.825, 20.0, 0.0, 0, 82, 0},//y移動
- {-5.825, 20.0, 0.0, 35, 82, 0},//発射
- {-5.825, 0.0, 0.0, 0, 90, 1},//y移動,角度戻す, P上昇
+ {-5.825, 18.0, 0.0, 0, 82, 0},//y移動
+ {-5.825, 18.0, 0.0, 35, 82, 0},//発射
+ {-5.825, 0.0, 0.0, 0, 88, 0},//y移動,角度戻す, P上昇
- {-1.825, 0.0, 0.0, 0, 82, 0.0},//x移動, 角度変化, 下降&バットマン駆動
- {-1.825, 20.0, 0.0, 0, 82, 0.0},//y移動
- {-1.825, 20.0, 0.0, 18, 82, 0.0},//下段に発射
- {-1.825, 20.0, 0.0, 0, 90, 0.0},//角度戻す, P上昇
- {-1.825, 20.0, 0.0, 0, 85, 0.0},//角度変化, 下降&バットマン駆動
- {-1.825, 20.0, 0.0, 18, 85, 0.0},//上段に発射
+ {-1.825, 0.0, 0.0, 0, 88, 1},//x移動, 角度変化, 下降&バットマン駆動
+ {-1.825, 18.0, 0.0, 0, 82, 0},//y移動
+ {-1.825, 18.0, 0.0, 18, 82, 0},//下段に発射
+ {-1.825, 18.0, 0.0, 0, 88, 1},//角度戻す, P上昇
+ {-1.825, 18.0, 0.0, 0, 85, 0},//角度変化, 下降&バットマン駆動
+ {-1.825, 18.0, 0.0, 18, 85, 0},//上段に発射
};
controller getPropoData();
@@ -54,7 +54,7 @@
float target_yow = 0;
//USS用
-PID pidUss(0.025, 0, 0, 0.1, 0.6, &timer);
+PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
float target_uss = 8.0;
int posi_num = 0;
@@ -67,6 +67,8 @@
wait(1);
imu.performCalibration();
imu.startAngleComputing();
+
+ uss.startTriger();
for(int i; i < 3; i++)
enc[i].changeDirection();
@@ -92,6 +94,14 @@
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();
+
//許容誤差
pidRobotX.allowable_error = 0.1;
pidRobotY.allowable_error = 0.1;
@@ -108,10 +118,11 @@
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, 3, int(position[posi_num][5]));
while(can.send() == 0);
can.read();
+ int supply_pid = can.get(2, 1);
if(pidRobotX.isConvergence(1) == 1
&& pidRobotYow.isConvergence(1) == 1)
@@ -122,7 +133,7 @@
&& isConvergetnceTops() == 1)
posi_num++;
}
- else if(pidRobotY.isConvergence(1) == 1)
+ else if(pidRobotY.isConvergence(1) == 1 && supply_pid == 1)
posi_num++;
}
@@ -143,7 +154,7 @@
//USS距離調整処理
if(position[posi_num][1] >= 10)
{
- *pidUss.target = position[posi_num][1] - 8;
+ *pidUss.target = position[posi_num][1] - 10;
robot_velocity[1] = -pidUss.output;
}
else
@@ -152,10 +163,10 @@
*pidRobotY.target = position[posi_num][1];
robot_velocity[1] = pidRobotY.output;
}
-
+ /*
if(sw1 == 1 && sw2 == 1)
imu.angle[2] = odm.y = 0;
-
+ */
//ホイール速度計算
mecanum.setVelG(robot_velocity);
mecanum.computeWheelVel();
@@ -169,9 +180,11 @@
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.01);
+ wait(0.02);
}
}
@@ -180,8 +193,7 @@
float dead_zone = 0.05;
controller propo;
sbus.isFailSafe();
-
- //propo直接コントロール
+
if(sbus.isFailSafe())
{
propo.LX = propo.LY = propo.RX = propo.RY = 0;