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
main.cpp
- Committer:
- TanakaTarou
- Date:
- 2018-08-20
- Revision:
- 8:123cd1f07aea
- Parent:
- 7:1ee46b2e8dce
- Child:
- 9:ce5a1315fe0d
File content as of revision 8:123cd1f07aea:
#include "mbed.h" #include "DriveController.h" #include "IMU.h" #include "MDD.h" #include "MDD2.h" #include "Mycan.h" #include "Odometer.h" #include "PID.h" #include "RotaryEncoder.h" #include "CSV.h" #include "SBUS.h" #include "USS.h" #include "hardwareConfig.h" //(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上昇 {-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 {-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.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 }; bool isConvergetnceTops(); //x軸補正用 PID PID pidRobotX(2, 0, 0, 0.01, 0.3, &timer); float target_x = 0; //y軸補正用 PID PID pidRobotY(2, 0, 0, 0.01, 0.3, &timer); float target_y = 0; //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義) PID pidRobotYow(0.05, 0, 0, 0.01, 0.95, &timer); float target_yow = 0; //USS用 PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer); float target_uss = 8.0; int posi_num = 0; int main() { //タイマー3の優先度を最低にする NVIC_SetPriority(TIMER3_IRQn, 100); //IMUのキャリブレーション wait(1); imu.performCalibration(); imu.startAngleComputing(); uss.startTriger(); for(int i; i < 3; i++) enc[i].changeDirection(); //オドメーターの定義 float matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 0} }; Odometer odm(matrix, 0.048); float tmp = 0; float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp}; odm.setupOdometerSensors(encoders, &imu.angle[2]); odm.startComputingOdometry(0.005, 0, 0, 0); //PID設定 pidRobotX.sensor = &odm.x; pidRobotX.target = &target_x; pidRobotX.start(); 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(); //許容誤差 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(sw1 == 0); while(1) { 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][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); //ポジション収束判定 if(pidRobotX.isConvergence(1) == 1 && pidRobotYow.isConvergence(1) == 1) { //超音波で近づくとき if(position[posi_num][1] >= 10) { if(pidUss.isConvergence(1) == 1 && isConvergetnceTops() == 1) posi_num++; } //超音波使わないとき else if(pidRobotY.isConvergence(1) == 1) { /* 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(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; if(posi_num < 19) { posi_num = 0; while(sw1 == 0); } //ロボットの移動速度(LX, LY, RX) float robot_velocity[3]; //yow角調整処理 *pidRobotYow.target = position[posi_num][2]; robot_velocity[2] = pidRobotYow.output; //x軸調整処理 *pidRobotX.target = position[posi_num][0]; robot_velocity[0] = pidRobotX.output; //USS距離調整処理 if(position[posi_num][3] >= 10) { *pidUss.target = position[posi_num][3] - 10; robot_velocity[1] = -pidUss.output; } else { //y軸調整処理 *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(); mecanum.rescaleWheelVel(); //モーターの駆動 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", imu.angle[2]); pc.printf("%.2f\t", uss.distance); pc.printf("\n"); wait(0.02); } } bool isConvergetnceTops() { 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); if(angle_pid == 1 && velocity_pid == 1 && velocity_val == position[posi_num][3] && angle_val == position[posi_num][4]) return 1; else return 0; }