MEC-B / Mbed 2 deprecated 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

main.cpp@7:1ee46b2e8dce, 2018-08-20 (annotated)

Committer:
TanakaTarou
Date:
Mon Aug 20 08:23:55 2018 +0000
Revision:
7:1ee46b2e8dce
Parent:
6:a102603c99fd
Child:
8:123cd1f07aea
8/20

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaTarou 0:6db16ad02a1b 1 #include "mbed.h"
TanakaTarou 0:6db16ad02a1b 2 #include "DriveController.h"
TanakaTarou 0:6db16ad02a1b 3 #include "IMU.h"
TanakaTarou 0:6db16ad02a1b 4 #include "MDD.h"
TanakaTarou 0:6db16ad02a1b 5 #include "MDD2.h"
TanakaTarou 0:6db16ad02a1b 6 #include "Mycan.h"
TanakaTarou 0:6db16ad02a1b 7 #include "Odometer.h"
TanakaTarou 0:6db16ad02a1b 8 #include "PID.h"
TanakaTarou 0:6db16ad02a1b 9 #include "RotaryEncoder.h"
TanakaTarou 0:6db16ad02a1b 10 #include "CSV.h"
TanakaTarou 0:6db16ad02a1b 11 #include "SBUS.h"
TanakaTarou 0:6db16ad02a1b 12 #include "USS.h"
TanakaTarou 0:6db16ad02a1b 13 #include "hardwareConfig.h"
TanakaTarou 0:6db16ad02a1b 14
TanakaTarou 7:1ee46b2e8dce 15 //(X, Y, θ, speed, angle)
TanakaTarou 7:1ee46b2e8dce 16 float position[19][6] = {{-1.825, 0.0, 0.0, 0.0, 0, 82},//0, x移動, 角度変化
TanakaTarou 7:1ee46b2e8dce 17 {-1.825, 0.0, 0.0, 18.0, 0, 82},//1,y移動
TanakaTarou 7:1ee46b2e8dce 18 {-1.825, 0.0, 0.0, 18.0, 35, 82},//2,発射
TanakaTarou 7:1ee46b2e8dce 19 {-1.825, 0.0, 0.0, 0.0, 0, 88},//3,y移動,角度戻す, P上昇
TanakaTarou 7:1ee46b2e8dce 20
TanakaTarou 7:1ee46b2e8dce 21 {-2.825, 0.0, 0.0, 0.0, 0, 88},//4, x移動, 角度変化, 下降&バットマン駆動
TanakaTarou 7:1ee46b2e8dce 22 {-2.825, 0.0, 0.0, 18.0, 0, 82},//5,y移動
TanakaTarou 7:1ee46b2e8dce 23 {-2.825, 0.0, 0.0, 18.0, 35, 82},//6,発射
TanakaTarou 7:1ee46b2e8dce 24 {-2.825, 0.0, 0.0, 0.0, 0, 88},//7,y移動,角度戻す, P上昇
TanakaTarou 7:1ee46b2e8dce 25
TanakaTarou 7:1ee46b2e8dce 26 {-3.825, 0.0, 0.0, 0.0, 0, 88},//8,x移動, 角度変化, 下降&バットマン駆動
TanakaTarou 7:1ee46b2e8dce 27 {-3.825, 0.0, 0.0, 18.0, 0, 82},//9,y移動
TanakaTarou 7:1ee46b2e8dce 28 {-3.825, 0.0, 0.0, 18.0, 35, 82},//10,発射
TanakaTarou 7:1ee46b2e8dce 29 {-3.825, 0.0, 0.0, 0.0, 0, 88},//11,y移動,角度戻す, P上昇
TanakaTarou 6:a102603c99fd 30
TanakaTarou 7:1ee46b2e8dce 31 {-0.825, 0.0, 0.0, 0.0, 0, 88},//12,x移動, 角度変化, 下降&バットマン駆動
TanakaTarou 7:1ee46b2e8dce 32 {-0.825, 0.0, 0.0, 18.0, 0, 82},//13,y移動
TanakaTarou 7:1ee46b2e8dce 33 {-0.825, 0.0, 0.0, 18.0, 18, 82},//14,下段に発射
TanakaTarou 7:1ee46b2e8dce 34 {-0.825,0.0, 0.0, 18.0, 0, 88},//15,角度戻す, P上昇
TanakaTarou 7:1ee46b2e8dce 35 {-0.825, 0.0, 0.0, 18.0, 0, 85},//16,角度変化, 下降&バットマン駆動
TanakaTarou 7:1ee46b2e8dce 36 {-0.825, 0.0, 0.0, 18.0, 18, 85},//17,上段に発射
TanakaTarou 7:1ee46b2e8dce 37 /*
TanakaTarou 7:1ee46b2e8dce 38 {-0.825, 0.0, 90.0, 0.0, 0, 88},//18
TanakaTarou 6:a102603c99fd 39
TanakaTarou 7:1ee46b2e8dce 40 {-0.5, 0.0, 90.0, 0.0, 0, 88},//19
TanakaTarou 7:1ee46b2e8dce 41 {-0.5, 1.125, 90.0, 0.0, 0, 88},//20
TanakaTarou 7:1ee46b2e8dce 42 {-0.5, 1.125, 90.0, 18.0, 0, 82},//21
TanakaTarou 7:1ee46b2e8dce 43 {-0.5, 1.125, 90.0, 18.0, 30, 82},//22
TanakaTarou 7:1ee46b2e8dce 44 {-0.5, 1.125, 90.0, 0.0, 0, 88},//23
TanakaTarou 0:6db16ad02a1b 45
TanakaTarou 7:1ee46b2e8dce 46 {-0.5, 1.625, 90.0, 0.0, 0, 88},//24
TanakaTarou 7:1ee46b2e8dce 47 {-0.5, 1.625, 90.0, 18.0, 0, 82},//25
TanakaTarou 7:1ee46b2e8dce 48 {-0.5, 1.625, 90.0, 18.0, 30, 82},//26
TanakaTarou 7:1ee46b2e8dce 49 {-0.5, 1.625, 90.0, 0.0, 0, 88},//27
TanakaTarou 7:1ee46b2e8dce 50
TanakaTarou 7:1ee46b2e8dce 51 {-0.5, 2.125, 90.0, 0.0, 0, 88},//28
TanakaTarou 7:1ee46b2e8dce 52 {-0.5, 2.125, 90.0, 18.0, 0, 82},//29
TanakaTarou 7:1ee46b2e8dce 53 {-0.5, 2.125, 90.0, 18.0, 30, 82},//30
TanakaTarou 7:1ee46b2e8dce 54 {-0.5, 2.125, 90.0, 0.0, 0, 88},//31
TanakaTarou 7:1ee46b2e8dce 55 */
TanakaTarou 7:1ee46b2e8dce 56 {-0.0, 0.0, 0.0, 0, 0.0, 90}//18
TanakaTarou 0:6db16ad02a1b 57 };
TanakaTarou 0:6db16ad02a1b 58
TanakaTarou 3:6b4adb4d7101 59 bool isConvergetnceTops();
TanakaTarou 0:6db16ad02a1b 60
TanakaTarou 0:6db16ad02a1b 61 //x軸補正用 PID
soyooo 1:d7ceb38da3d8 62 PID pidRobotX(2, 0, 0, 0.01, 0.3, &timer);
TanakaTarou 0:6db16ad02a1b 63 float target_x = 0;
TanakaTarou 0:6db16ad02a1b 64
TanakaTarou 0:6db16ad02a1b 65 //y軸補正用 PID
soyooo 1:d7ceb38da3d8 66 PID pidRobotY(2, 0, 0, 0.01, 0.3, &timer);
TanakaTarou 0:6db16ad02a1b 67 float target_y = 0;
TanakaTarou 0:6db16ad02a1b 68
soyooo 1:d7ceb38da3d8 69 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
soyooo 1:d7ceb38da3d8 70 PID pidRobotYow(0.05, 0, 0, 0.01, 0.95, &timer);
soyooo 1:d7ceb38da3d8 71 float target_yow = 0;
soyooo 1:d7ceb38da3d8 72
TanakaTarou 0:6db16ad02a1b 73 //USS用
TanakaTarou 5:7c5e07260e1e 74 PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
TanakaTarou 4:9f74525eb37f 75 float target_uss = 8.0;
TanakaTarou 3:6b4adb4d7101 76 int posi_num = 0;
TanakaTarou 3:6b4adb4d7101 77
TanakaTarou 0:6db16ad02a1b 78 int main()
TanakaTarou 0:6db16ad02a1b 79 {
TanakaTarou 0:6db16ad02a1b 80 //タイマー３の優先度を最低にする
TanakaTarou 0:6db16ad02a1b 81 NVIC_SetPriority(TIMER3_IRQn, 100);
TanakaTarou 0:6db16ad02a1b 82
TanakaTarou 0:6db16ad02a1b 83 //IMUのキャリブレーション
TanakaTarou 4:9f74525eb37f 84 wait(1);
TanakaTarou 0:6db16ad02a1b 85 imu.performCalibration();
TanakaTarou 0:6db16ad02a1b 86 imu.startAngleComputing();
TanakaTarou 5:7c5e07260e1e 87
TanakaTarou 5:7c5e07260e1e 88 uss.startTriger();
TanakaTarou 0:6db16ad02a1b 89
TanakaTarou 0:6db16ad02a1b 90 for(int i; i < 3; i++)
TanakaTarou 0:6db16ad02a1b 91 enc[i].changeDirection();
TanakaTarou 0:6db16ad02a1b 92
TanakaTarou 0:6db16ad02a1b 93 //オドメーターの定義
TanakaTarou 0:6db16ad02a1b 94 float matrix[3][3] = {{1, 0, 0},
TanakaTarou 0:6db16ad02a1b 95 {0, 1, 0},
TanakaTarou 0:6db16ad02a1b 96 {0, 0, 0}
TanakaTarou 0:6db16ad02a1b 97 };
TanakaTarou 0:6db16ad02a1b 98 Odometer odm(matrix, 0.048);
TanakaTarou 0:6db16ad02a1b 99 float tmp = 0;
TanakaTarou 0:6db16ad02a1b 100 float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
TanakaTarou 0:6db16ad02a1b 101 odm.setupOdometerSensors(encoders, &imu.angle[2]);
TanakaTarou 0:6db16ad02a1b 102 odm.startComputingOdometry(0.005, 0, 0, 0);
TanakaTarou 0:6db16ad02a1b 103
TanakaTarou 7:1ee46b2e8dce 104 //PID設定
TanakaTarou 0:6db16ad02a1b 105 pidRobotX.sensor = &odm.x;
TanakaTarou 0:6db16ad02a1b 106 pidRobotX.target = &target_x;
TanakaTarou 0:6db16ad02a1b 107 pidRobotX.start();
TanakaTarou 0:6db16ad02a1b 108 pidRobotY.sensor = &odm.y;
TanakaTarou 0:6db16ad02a1b 109 pidRobotY.target = &target_y;
TanakaTarou 0:6db16ad02a1b 110 pidRobotY.start();
TanakaTarou 5:7c5e07260e1e 111 pidRobotYow.sensor = &imu.angle[2];
TanakaTarou 5:7c5e07260e1e 112 pidRobotYow.target = &target_yow;
TanakaTarou 5:7c5e07260e1e 113 pidRobotYow.start();
TanakaTarou 5:7c5e07260e1e 114 pidUss.sensor = &uss.distance;
TanakaTarou 5:7c5e07260e1e 115 pidUss.target = &target_uss;
TanakaTarou 5:7c5e07260e1e 116 pidUss.start();
TanakaTarou 5:7c5e07260e1e 117
TanakaTarou 4:9f74525eb37f 118 //許容誤差
soyooo 1:d7ceb38da3d8 119 pidRobotX.allowable_error = 0.1;
soyooo 1:d7ceb38da3d8 120 pidRobotY.allowable_error = 0.1;
soyooo 1:d7ceb38da3d8 121 pidRobotYow.allowable_error = 2;
soyooo 1:d7ceb38da3d8 122 pidUss.allowable_error = 3;
soyooo 1:d7ceb38da3d8 123
TanakaTarou 4:9f74525eb37f 124 //マイクロスイッチ
TanakaTarou 3:6b4adb4d7101 125 sw1.mode(PullUp);
TanakaTarou 3:6b4adb4d7101 126 sw2.mode(PullUp);
TanakaTarou 3:6b4adb4d7101 127
TanakaTarou 7:1ee46b2e8dce 128 while(sw1 == 0);
TanakaTarou 6:a102603c99fd 129 while(1)
TanakaTarou 0:6db16ad02a1b 130 {
TanakaTarou 7:1ee46b2e8dce 131 can.read();
TanakaTarou 7:1ee46b2e8dce 132 int supply_pid = can.get(2, 1);
TanakaTarou 7:1ee46b2e8dce 133 int supply_wait = can.get(2, 2);
TanakaTarou 7:1ee46b2e8dce 134 int angle_wait = can.get(3, 5);
TanakaTarou 7:1ee46b2e8dce 135 int sw[8] = {can.get(4, 1),
TanakaTarou 7:1ee46b2e8dce 136 can.get(4, 2),
TanakaTarou 7:1ee46b2e8dce 137 can.get(4, 3),
TanakaTarou 7:1ee46b2e8dce 138 can.get(4, 4),
TanakaTarou 7:1ee46b2e8dce 139 can.get(4, 5),
TanakaTarou 7:1ee46b2e8dce 140 can.get(4, 6),
TanakaTarou 7:1ee46b2e8dce 141 can.get(4, 7),
TanakaTarou 7:1ee46b2e8dce 142 can.get(4, 8)
TanakaTarou 7:1ee46b2e8dce 143 };
TanakaTarou 0:6db16ad02a1b 144
TanakaTarou 7:1ee46b2e8dce 145 can.set(1, 1, int(position[posi_num][4]));
TanakaTarou 7:1ee46b2e8dce 146 can.set(1, 2, int(position[posi_num][5]));
TanakaTarou 7:1ee46b2e8dce 147 if(supply_wait == 1 && angle_wait == 1)
TanakaTarou 7:1ee46b2e8dce 148 can.set(1, 3, 1);
TanakaTarou 7:1ee46b2e8dce 149 else can.set(1, 3, 0);
TanakaTarou 7:1ee46b2e8dce 150
TanakaTarou 3:6b4adb4d7101 151 while(can.send() == 0);
TanakaTarou 3:6b4adb4d7101 152
TanakaTarou 7:1ee46b2e8dce 153 //ポジション収束判定
TanakaTarou 3:6b4adb4d7101 154 if(pidRobotX.isConvergence(1) == 1
TanakaTarou 3:6b4adb4d7101 155 && pidRobotYow.isConvergence(1) == 1)
soyooo 1:d7ceb38da3d8 156 {
TanakaTarou 6:a102603c99fd 157 //超音波で近づくとき
TanakaTarou 3:6b4adb4d7101 158 if(position[posi_num][1] >= 10)
TanakaTarou 3:6b4adb4d7101 159 {
TanakaTarou 3:6b4adb4d7101 160 if(pidUss.isConvergence(1) == 1
TanakaTarou 3:6b4adb4d7101 161 && isConvergetnceTops() == 1)
TanakaTarou 3:6b4adb4d7101 162 posi_num++;
TanakaTarou 3:6b4adb4d7101 163 }
TanakaTarou 6:a102603c99fd 164 //超音波使わないとき
TanakaTarou 6:a102603c99fd 165 else if(pidRobotY.isConvergence(1) == 1)
TanakaTarou 7:1ee46b2e8dce 166 {
TanakaTarou 7:1ee46b2e8dce 167 if(posi_num == 2 || posi_num == 6 || posi_num == 10 || posi_num == 14 || posi_num == 17)
TanakaTarou 7:1ee46b2e8dce 168 {
TanakaTarou 7:1ee46b2e8dce 169 if(supply_pid == 1)
TanakaTarou 7:1ee46b2e8dce 170 posi_num++;
TanakaTarou 7:1ee46b2e8dce 171 }
TanakaTarou 7:1ee46b2e8dce 172 else posi_num++;
TanakaTarou 7:1ee46b2e8dce 173 }
soyooo 1:d7ceb38da3d8 174 }
TanakaTarou 7:1ee46b2e8dce 175 if(sw[0] == 1 && posi_num == 0)
TanakaTarou 7:1ee46b2e8dce 176 posi_num = 4;
TanakaTarou 7:1ee46b2e8dce 177 if(sw[1] == 1 && posi_num == 4)
TanakaTarou 7:1ee46b2e8dce 178 posi_num = 8;
TanakaTarou 7:1ee46b2e8dce 179 if(sw[2] == 1 && posi_num == 8)
TanakaTarou 7:1ee46b2e8dce 180 posi_num = 12;
TanakaTarou 7:1ee46b2e8dce 181 if(sw[3] == 1 && sw[4] == 1 && posi_num == 12)
TanakaTarou 7:1ee46b2e8dce 182 posi_num = 14;
TanakaTarou 7:1ee46b2e8dce 183 if(sw[3] == 1 && posi_num == 14)
TanakaTarou 7:1ee46b2e8dce 184 posi_num = 17;
TanakaTarou 7:1ee46b2e8dce 185 if(sw[4] == 1 && posi_num == 17)
TanakaTarou 7:1ee46b2e8dce 186 posi_num = 18;
soyooo 1:d7ceb38da3d8 187
TanakaTarou 0:6db16ad02a1b 188 //ロボットの移動速度(LX, LY, RX)
TanakaTarou 0:6db16ad02a1b 189 float robot_velocity[3];
TanakaTarou 0:6db16ad02a1b 190
TanakaTarou 0:6db16ad02a1b 191 //yow角調整処理
TanakaTarou 0:6db16ad02a1b 192 *pidRobotYow.target = position[posi_num][2];
TanakaTarou 0:6db16ad02a1b 193 robot_velocity[2] = pidRobotYow.output;
TanakaTarou 0:6db16ad02a1b 194
TanakaTarou 0:6db16ad02a1b 195 //x軸調整処理
TanakaTarou 0:6db16ad02a1b 196 *pidRobotX.target = position[posi_num][0];
TanakaTarou 0:6db16ad02a1b 197 robot_velocity[0] = pidRobotX.output;
TanakaTarou 0:6db16ad02a1b 198
TanakaTarou 0:6db16ad02a1b 199 //USS距離調整処理
TanakaTarou 7:1ee46b2e8dce 200 if(position[posi_num][3] >= 10)
TanakaTarou 0:6db16ad02a1b 201 {
TanakaTarou 7:1ee46b2e8dce 202 *pidUss.target = position[posi_num][3] - 10;
TanakaTarou 0:6db16ad02a1b 203 robot_velocity[1] = -pidUss.output;
TanakaTarou 0:6db16ad02a1b 204 }
TanakaTarou 0:6db16ad02a1b 205 else
TanakaTarou 0:6db16ad02a1b 206 {
TanakaTarou 0:6db16ad02a1b 207 //y軸調整処理
TanakaTarou 0:6db16ad02a1b 208 *pidRobotY.target = position[posi_num][1];
TanakaTarou 0:6db16ad02a1b 209 robot_velocity[1] = pidRobotY.output;
TanakaTarou 0:6db16ad02a1b 210 }
TanakaTarou 5:7c5e07260e1e 211 /*
TanakaTarou 3:6b4adb4d7101 212 if(sw1 == 1 && sw2 == 1)
TanakaTarou 4:9f74525eb37f 213 imu.angle[2] = odm.y = 0;
TanakaTarou 5:7c5e07260e1e 214 */
TanakaTarou 0:6db16ad02a1b 215 //ホイール速度計算
TanakaTarou 4:9f74525eb37f 216 mecanum.setVelG(robot_velocity);
TanakaTarou 0:6db16ad02a1b 217 mecanum.computeWheelVel();
TanakaTarou 0:6db16ad02a1b 218 mecanum.rescaleWheelVel();
TanakaTarou 0:6db16ad02a1b 219
TanakaTarou 0:6db16ad02a1b 220 //モーターの駆動
TanakaTarou 0:6db16ad02a1b 221 for(int i = 0; i < 4; i++)
TanakaTarou 3:6b4adb4d7101 222 Motor[i].drive(mecanum.wheel_vel[i]);
TanakaTarou 0:6db16ad02a1b 223
TanakaTarou 3:6b4adb4d7101 224 pc.printf("%.2f\t", odm.x);
TanakaTarou 3:6b4adb4d7101 225 pc.printf("%.2f\t", odm.y);
TanakaTarou 3:6b4adb4d7101 226 pc.printf("%.2f\t", imu.angle[2]);
TanakaTarou 3:6b4adb4d7101 227 pc.printf("%.2f\t", uss.distance);
TanakaTarou 0:6db16ad02a1b 228 pc.printf("\n");
TanakaTarou 2:7af15d4ee55a 229
TanakaTarou 5:7c5e07260e1e 230 wait(0.02);
TanakaTarou 0:6db16ad02a1b 231 }
TanakaTarou 0:6db16ad02a1b 232 }
TanakaTarou 0:6db16ad02a1b 233
TanakaTarou 3:6b4adb4d7101 234 bool isConvergetnceTops()
TanakaTarou 0:6db16ad02a1b 235 {
TanakaTarou 4:9f74525eb37f 236 int velocity_pid = can.get(3, 1);
TanakaTarou 4:9f74525eb37f 237 int angle_pid = can.get(3, 2);
TanakaTarou 4:9f74525eb37f 238 int velocity_val = can.get(3, 3);
TanakaTarou 4:9f74525eb37f 239 int angle_val = can.get(3, 4);
TanakaTarou 3:6b4adb4d7101 240
TanakaTarou 4:9f74525eb37f 241 if(angle_pid == 1 && velocity_pid == 1 && velocity_val == position[posi_num][3] && angle_val == position[posi_num][4])
TanakaTarou 4:9f74525eb37f 242 return 1;
TanakaTarou 3:6b4adb4d7101 243 else return 0;
TanakaTarou 4:9f74525eb37f 244 }