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:
- 3:6b4adb4d7101
- Parent:
- 2:7af15d4ee55a
- Child:
- 4:9f74525eb37f
diff -r 7af15d4ee55a -r 6b4adb4d7101 main.cpp
--- a/main.cpp Mon Aug 06 10:12:44 2018 +0000
+++ b/main.cpp Tue Aug 07 08:17:52 2018 +0000
@@ -13,33 +13,33 @@
#include "hardwareConfig.h"
//(X, Y, θ, speed, angle, injection, 補給昇降)
-float position[13][7] = {{0.0, 0.0, 0.0, 0, 90, 0, 0.0},//初期位置
- {-0.3, 0.0, 0.0, 0, 86, 0, 0.0},//x移動, 角度変化
- {-0.3, 20.0, 0.0, 0, 86, 0, 0.0},//y移動
- {-0.3, 20.0, 0.0, 10, 86, 1, 0.0},//発射
- {-0.3, 0.0, 0.0, 0, 82, 0, 0.0},//y移動,角度戻す, P上昇
+float position[13][6] = {{0.0, 0.0, 0.0, 0, 90, 0},//初期位置
+ {-1.0, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化
+ {-1.0, 20.0, 0.0, 0, 82, 0},//y移動
+ {-1.0, 20.0, 0.0, 35, 82, 0},//発射
+ {-1.0, 0.0, 0.0, 0, 90, 0},//y移動,角度戻す, P上昇
- {-0.6, 0.0, 0.0, 0, 82, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
- {-0.6, 20.0, 0.0, 0, 82, 0, 0.0},//y移動
- {-0.6, 20.0, 0.0, 10, 80, 1, 0.0},//発射
- {-0.6, 0.0, 0.0, 0, 78, 0, 6.8},//y移動,角度戻す, P上昇
+ {-2.0, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化, 下降&バットマン駆動
+ {-2.0, 20.0, 0.0, 0, 82, 0},//y移動
+ {-2.0, 20.0, 0.0, 35, 82, 0},//発射
+ {-2.0, 0.0, 0.0, 0, 90, 1},//y移動,角度戻す, P上昇
- {-0.9, 0.0, 0.0, 0, 78, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
- {-0.9, 20.0, 0.0, 0, 78, 0, 0.0},//y移動
- {-0.9, 20.0, 0.0, 10, 78, 1, 0.0},//発射
- {-0.9, 0.0, 0.0, 0, 90, 0, 6.8},//y移動,角度戻す, P上昇
+ {-3.0, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化, 下降&バットマン駆動
+ {-3.0, 20.0, 0.0, 0, 82, 0},//y移動
+ {-3.0, 20.0, 0.0, 35, 82, 0},//発射
+ {-3.0, 0.0, 0.0, 0, 90, 1},//y移動,角度戻す, P上昇
/*
- {3.0, 0.0, 0.0, 20, 80, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
- {3.0, 2.5, 0.0, 20, 80, 0, 0.0},//y移動
- {3.0, 2.5, 0.0, 20, 80, 1, 0.0},//下段に発射
- {3.0, 2.5, 0.0, 20, 90, 0, 6.8},//角度戻す, P上昇
- {3.0, 2.5, 0.0, 20, 80, 0, 0.0},//角度変化, 下降&バットマン駆動
- {3.0, 2.5, 0.0, 20, 80, 1, 0.0},//上段に発射
+ {3.0, 0.0, 0.0, 20, 80, 0.0},//x移動, 角度変化, 下降&バットマン駆動
+ {3.0, 2.5, 0.0, 20, 80, 0.0},//y移動
+ {3.0, 2.5, 0.0, 20, 80, 0.0},//下段に発射
+ {3.0, 2.5, 0.0, 20, 90, 6.8},//角度戻す, P上昇
+ {3.0, 2.5, 0.0, 20, 80, 0.0},//角度変化, 下降&バットマン駆動
+ {3.0, 2.5, 0.0, 20, 80, 0.0},//上段に発射
*/
};
controller getPropoData();
-//bool isConvergence(int num);
+bool isConvergetnceTops();
//x軸補正用 PID
PID pidRobotX(2, 0, 0, 0.01, 0.3, &timer);
@@ -54,9 +54,11 @@
float target_yow = 0;
//USS用
-PID pidUss(0.02, 0, 0, 0.01, 0.3, &timer);
+PID pidUss(0.04, 0, 0, 0.01, 0.3, &timer);
float target_uss = 25.0;
+int posi_num = 0;
+
int main()
{
//タイマー３の優先度を最低にする
@@ -90,13 +92,13 @@
pidRobotY.target = &target_y;
pidRobotY.start();
- //IMUyow
+ //IMUジャイロ
mecanum.imu_yow = &imu.angle[2];
pidRobotYow.sensor = &imu.angle[2];
pidRobotYow.target = &target_yow;
pidRobotYow.start();
- //USS pid設定
+ //超音波 PID設定
uss.startTriger();
pidUss.sensor = &uss.distance;
pidUss.target = &target_uss;
@@ -107,39 +109,36 @@
pidRobotYow.allowable_error = 2;
pidUss.allowable_error = 3;
+ sw1.mode(PullUp);
+ sw2.mode(PullUp);
+
while(1)
{
controller cmd = getPropoData(); //getPropoData & getCanData
- static int posi_num = 0;
- int angle_pid;
- int velocity_pid;
+ 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]));
+ while(can.send() == 0);
+
can.read();
- angle_pid = can.get(3, 1);
- velocity_pid = can.get(3, 2);
- if(position[posi_num][1] >= 10)
+ if(pidRobotX.isConvergence(1) == 1
+ && pidRobotYow.isConvergence(1) == 1)
{
- if(pidRobotX.isConvergence(1) == 1
- && pidRobotYow.isConvergence(1) == 1
- && pidUss.isConvergence(1) == 1
- && velocity_pid == 1)
+ if(position[posi_num][1] >= 10)
+ {
+ if(pidUss.isConvergence(1) == 1
+ && isConvergetnceTops() == 1)
+ posi_num++;
+ }
+ else if(pidRobotY.isConvergence(1) == 1)
posi_num++;
}
- else if(pidRobotX.isConvergence(1) == 1
- && pidRobotYow.isConvergence(1) == 1
- && pidRobotY.isConvergence(1) == 1
- && velocity_pid == 1)
- posi_num++;
- if(posi_num >= 12)
+ if(posi_num >= 13)
posi_num = 0;
- can.set(1, 1, position[posi_num][3]);
- can.set(1, 2, int(position[posi_num][4]));
- //can.set(2, 1, (position[posi_num][6]));
- while(can.send() == 0);
-
//ロボットの移動速度(LX, LY, RX)
float robot_velocity[3];
@@ -163,16 +162,11 @@
*pidRobotY.target = position[posi_num][1];
robot_velocity[1] = pidRobotY.output;
}
-
- //角度・XYリセット
- if(cmd.F == 2)
+
+ if(sw1 == 1 && sw2 == 1)
{
imu.angle[2] = 0;
- *pidRobotYow.target = 0;
- odm.x = 0;
- *pidRobotX.target = 0;
odm.y = 0;
- *pidRobotY.target = 0;
}
//ホイール速度計算
@@ -182,15 +176,15 @@
//モーターの駆動
for(int i = 0; i < 4; i++)
- ;//Motor[i].drive(mecanum.wheel_vel[i]);
+ Motor[i].drive(mecanum.wheel_vel[i]);
- pc.printf("%d\t", angle_pid);
- pc.printf("%d\t", velocity_pid);
- pc.printf("%d\t", posi_num);
-
+ pc.printf("%.2f\t", odm.x);
+ pc.printf("%.2f\t", odm.y);
+ pc.printf("%.2f\t", imu.angle[2]);
+ pc.printf("%.2f\t", uss.distance);
pc.printf("\n");
- wait(0.02);
+ wait(0.01);
}
}
@@ -227,56 +221,17 @@
return propo;
}
-/*
-bool isConvergence(int num)
+bool isConvergetnceTops()
{
- bool tf;
- if(num > 3 && num < 8)
- num -= 4;
- else if(num > 7 && num < 12)
- num -= 8;
- else if(num > 11 && num < 15)
- num -= 8;
-
- if(num == 0)
- {
- if(pidRobotX.output == 0)
- tf = 1;
- else
- tf = 0;
- }
- else if(num == 1)
- {
- if(pidUss.output == 0)
- tf = 1;
- else
- tf = 0;
- }
- else if(num == 2)
- {
- wait(1.0);
- tf = 1;
- }
- else if(num == 3)
- {
- if(pidRobotY.output == 0)
- tf = 1;
- else
- tf = 0;
- }
- else if(num == 15)
- {
-
- }
- else if(num == 16)
- {
-
- }
- else if(num == 17)
- {
-
- }
-
- return tf;
+ int velocity_pid;
+ int angle_pid;
+ int velocity_val;
+
+ velocity_pid = can.get(3, 1);
+ angle_pid = can.get(3, 2);
+ velocity_val = can.get(3, 3);
+
+ if(angle_pid == 1 && velocity_pid == 1 && velocity_val == position[posi_num][3])
+ return 1;
+ else return 0;
}
-*/
\ No newline at end of file