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:
- 2:7af15d4ee55a
- Parent:
- 1:d7ceb38da3d8
- Child:
- 3:6b4adb4d7101
--- a/main.cpp Mon Aug 06 03:04:47 2018 +0000
+++ b/main.cpp Mon Aug 06 10:12:44 2018 +0000
@@ -13,20 +13,21 @@
#include "hardwareConfig.h"
//(X, Y, θ, speed, angle, injection, 補給昇降)
-float position[10][7] = {{0.0, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化
- {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動
- {-0.3, 15.0, 0.0, 0, 90, 0, 0.0},//発射
- {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動,角度戻す, P上昇
+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上昇
- {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
- {-0.6, 15.0, 0.0, 0, 90, 0, 0.0},//y移動
- {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//発射
- //{6.0, 0.0, 0.0, 20, 90, 0, 6.8},//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上昇
- {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
- {-0.9, 15.0, 0.0, 20, 90, 0, 0.0},//y移動
- {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//発射
- //{7.0, 0.0, 0.0, 20, 90, 0, 6.8},//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, 20, 80, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
{3.0, 2.5, 0.0, 20, 80, 0, 0.0},//y移動
@@ -34,7 +35,6 @@
{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},//上段に発射
- {0.0, 0.0, 0.0, 20, 90, 0, 6.8} //x・y移動,スタートゾーンに戻る
*/
};
@@ -112,41 +112,33 @@
controller cmd = getPropoData(); //getPropoData & getCanData
static int posi_num = 0;
+ int angle_pid;
+ int velocity_pid;
+ 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 && pidUss.isConvergence(1) == 1)
+ if(pidRobotX.isConvergence(1) == 1
+ && pidRobotYow.isConvergence(1) == 1
+ && pidUss.isConvergence(1) == 1
+ && velocity_pid == 1)
posi_num++;
}
- else if(pidRobotX.isConvergence(1) + pidRobotYow.isConvergence(1) + pidRobotY.isConvergence(1) == 3)
+ else if(pidRobotX.isConvergence(1) == 1
+ && pidRobotYow.isConvergence(1) == 1
+ && pidRobotY.isConvergence(1) == 1
+ && velocity_pid == 1)
posi_num++;
-
- if(posi_num >= 9)
+ if(posi_num >= 12)
posi_num = 0;
-
- /*
- static int reset_swich = 0;
- if(cmd.H == 2 && reset_swich == 0)
- {
- posi_num += 1;
- if(posi_num > 9)
- posi_num = 0;
- reset_swich = 1;
- }
- else if(cmd.H == 0 && reset_swich == 1)
- reset_swich = 0;
- */
- /*
- if(position[posi_num][5] == 1)
- can.set(1, 1, position[posi_num][3]);
- else
- can.set(1, 1, 0);
- can.set(1, 2, position[posi_num][4]);
- can.set(2, 1, position[posi_num][6]);
- can.send();
- */
+ 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];
@@ -190,24 +182,14 @@
//モーターの駆動
for(int i = 0; i < 4; i++)
- Motor[i].drive(mecanum.wheel_vel[i]);
-
- pc.printf("%.2f\t", odm.x);
- pc.printf("%.2f\t", odm.y);
- pc.printf("%.2f\t", *pidRobotYow.sensor);
+ ;//Motor[i].drive(mecanum.wheel_vel[i]);
- pc.printf("%.2f\t", *pidRobotX.target);
- pc.printf("%.2f\t", *pidRobotY.target);
- pc.printf("%.2f\t", *pidRobotYow.target);
-
- pc.printf("%d\t", pidRobotX.isConvergence(1));
- pc.printf("%d\t", pidRobotY.isConvergence(1));
- pc.printf("%d\t", pidRobotYow.isConvergence(1));
- pc.printf("%d\t", pidUss.isConvergence(1));
-
+ pc.printf("%d\t", angle_pid);
+ pc.printf("%d\t", velocity_pid);
pc.printf("%d\t", posi_num);
pc.printf("\n");
+
wait(0.02);
}
}