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:
- 4:9f74525eb37f
- Parent:
- 3:6b4adb4d7101
- Child:
- 5:7c5e07260e1e
diff -r 6b4adb4d7101 -r 9f74525eb37f main.cpp
--- a/main.cpp Tue Aug 07 08:17:52 2018 +0000
+++ b/main.cpp Thu Aug 09 00:21:35 2018 +0000
@@ -12,30 +12,30 @@
#include "USS.h"
#include "hardwareConfig.h"
+
//(X, Y, θ, speed, angle, injection, 補給昇降)
-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上昇
-
- {-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上昇
+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.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},//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},//上段に発射
- */
+ {-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上昇
+
+ {-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上昇
+
+ {-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},//上段に発射
};
controller getPropoData();
@@ -54,9 +54,8 @@
float target_yow = 0;
//USS用
-PID pidUss(0.04, 0, 0, 0.01, 0.3, &timer);
-float target_uss = 25.0;
-
+PID pidUss(0.025, 0, 0, 0.1, 0.6, &timer);
+float target_uss = 8.0;
int posi_num = 0;
int main()
@@ -65,6 +64,7 @@
NVIC_SetPriority(TIMER3_IRQn, 100);
//IMUのキャリブレーション
+ wait(1);
imu.performCalibration();
imu.startAngleComputing();
@@ -92,29 +92,19 @@
pidRobotY.target = &target_y;
pidRobotY.start();
- //IMUジャイロ
- mecanum.imu_yow = &imu.angle[2];
- pidRobotYow.sensor = &imu.angle[2];
- pidRobotYow.target = &target_yow;
- pidRobotYow.start();
-
- //超音波 PID設定
- uss.startTriger();
- pidUss.sensor = &uss.distance;
- pidUss.target = &target_uss;
- pidUss.start();
-
+ //許容誤差
pidRobotX.allowable_error = 0.1;
pidRobotY.allowable_error = 0.1;
pidRobotYow.allowable_error = 2;
pidUss.allowable_error = 3;
+ //マイクロスイッチ
sw1.mode(PullUp);
sw2.mode(PullUp);
while(1)
{
- controller cmd = getPropoData(); //getPropoData & getCanData
+ controller cmd = getPropoData();
can.set(1, 1, int(position[posi_num][3]));
can.set(1, 2, int(position[posi_num][4]));
@@ -136,7 +126,7 @@
posi_num++;
}
- if(posi_num >= 13)
+ if(posi_num >= 19)
posi_num = 0;
//ロボットの移動速度(LX, LY, RX)
@@ -153,7 +143,7 @@
//USS距離調整処理
if(position[posi_num][1] >= 10)
{
- *pidUss.target = position[posi_num][1] - 10;
+ *pidUss.target = position[posi_num][1] - 8;
robot_velocity[1] = -pidUss.output;
}
else
@@ -164,13 +154,10 @@
}
if(sw1 == 1 && sw2 == 1)
- {
- imu.angle[2] = 0;
- odm.y = 0;
- }
+ imu.angle[2] = odm.y = 0;
//ホイール速度計算
- mecanum.setVelL(robot_velocity);
+ mecanum.setVelG(robot_velocity);
mecanum.computeWheelVel();
mecanum.rescaleWheelVel();
@@ -213,7 +200,6 @@
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;
@@ -223,15 +209,12 @@
bool isConvergetnceTops()
{
- int velocity_pid;
- int angle_pid;
- int velocity_val;
+ int velocity_pid = can.get(3, 1);
+ int angle_pid = can.get(3, 2);
+ int velocity_val = can.get(3, 3);
+ int angle_val = can.get(3, 4);
- 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;
+ if(angle_pid == 1 && velocity_pid == 1 && velocity_val == position[posi_num][3] && angle_val == position[posi_num][4])
+ return 1;
else return 0;
-}
+}
\ No newline at end of file