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@19:bdb503dd1e8c, 2018-10-03 (annotated)

Committer:
TanakaTarou
Date:
Wed Oct 03 08:10:17 2018 +0000
Revision:
19:bdb503dd1e8c
Parent:
18:268ab2ab0b2a
Child:
20:1957c67ab740
10/3  ??????? ????, Lib????????+?mini?

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 "SBUS.h"
TanakaTarou 0:6db16ad02a1b 11 #include "USS.h"
TanakaTarou 0:6db16ad02a1b 12 #include "hardwareConfig.h"
soyooo 9:ce5a1315fe0d 13 #include "stateLib.h"
TanakaTarou 0:6db16ad02a1b 14
soyooo 9:ce5a1315fe0d 15 elements getRobotVelocity(state);
soyooo 9:ce5a1315fe0d 16 state getTargetState();
soyooo 9:ce5a1315fe0d 17 int updateStateNum();
TanakaTarou 19:bdb503dd1e8c 18 int changeNextTable(int);
soyooo 18:268ab2ab0b2a 19 int updateTopsNum(bool);
soyooo 18:268ab2ab0b2a 20 bool isConvergenceCatapult(int);
soyooo 10:ebb59c1d369e 21 bool isConvergenceSupply(int);
soyooo 9:ce5a1315fe0d 22 controller getPropoData();
TanakaTarou 12:91218718ae75 23 void riseUssTriger();
TanakaTarou 0:6db16ad02a1b 24
TanakaTarou 0:6db16ad02a1b 25 //x軸補正用 PID
TanakaTarou 12:91218718ae75 26 PID pidRobotX(4, 0, 0, 0.01, 0.3, &timer);
TanakaTarou 0:6db16ad02a1b 27 float target_x = 0;
TanakaTarou 0:6db16ad02a1b 28
TanakaTarou 0:6db16ad02a1b 29 //y軸補正用 PID
soyooo 1:d7ceb38da3d8 30 PID pidRobotY(2, 0, 0, 0.01, 0.3, &timer);
TanakaTarou 0:6db16ad02a1b 31 float target_y = 0;
TanakaTarou 0:6db16ad02a1b 32
soyooo 1:d7ceb38da3d8 33 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
TanakaTarou 12:91218718ae75 34 PID pidRobotYow(0.05, 0, 0, 0.01, 0.3, &timer);
soyooo 1:d7ceb38da3d8 35 float target_yow = 0;
soyooo 9:ce5a1315fe0d 36
TanakaTarou 0:6db16ad02a1b 37 //USS用
TanakaTarou 5:7c5e07260e1e 38 PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
soyooo 9:ce5a1315fe0d 39 float target_uss = 0;
TanakaTarou 3:6b4adb4d7101 40
TanakaTarou 12:91218718ae75 41 //オドメーターの定義
TanakaTarou 12:91218718ae75 42 float matrix[3][3] = {{1, 0, 0},
TanakaTarou 12:91218718ae75 43 {0, 1, 0},
TanakaTarou 12:91218718ae75 44 {0, 0, 0}
TanakaTarou 12:91218718ae75 45 };
TanakaTarou 12:91218718ae75 46 Odometer odm(matrix, 0.050);
TanakaTarou 19:bdb503dd1e8c 47
TanakaTarou 19:bdb503dd1e8c 48 int table_sw[9];
TanakaTarou 19:bdb503dd1e8c 49
TanakaTarou 0:6db16ad02a1b 50 int main()
TanakaTarou 0:6db16ad02a1b 51 {
TanakaTarou 19:bdb503dd1e8c 52 pc.baud(115200);
TanakaTarou 0:6db16ad02a1b 53 //タイマー３の優先度を最低にする
soyooo 9:ce5a1315fe0d 54 NVIC_SetPriority(TIMER3_IRQn, 3);
TanakaTarou 0:6db16ad02a1b 55
TanakaTarou 0:6db16ad02a1b 56 //IMUのキャリブレーション
TanakaTarou 4:9f74525eb37f 57 wait(1);
TanakaTarou 0:6db16ad02a1b 58 imu.performCalibration();
TanakaTarou 0:6db16ad02a1b 59 imu.startAngleComputing();
TanakaTarou 5:7c5e07260e1e 60
soyooo 9:ce5a1315fe0d 61 enc[0].changeDirection();
soyooo 10:ebb59c1d369e 62 //enc[1].changeDirection();
soyooo 9:ce5a1315fe0d 63 enc[2].changeDirection();
TanakaTarou 0:6db16ad02a1b 64
TanakaTarou 0:6db16ad02a1b 65 float tmp = 0;
TanakaTarou 0:6db16ad02a1b 66 float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
TanakaTarou 0:6db16ad02a1b 67 odm.setupOdometerSensors(encoders, &imu.angle[2]);
soyooo 9:ce5a1315fe0d 68 odm.startComputingOdometry(0.01, 0, 0, 0);
soyooo 9:ce5a1315fe0d 69 mecanum.imu_yow = &imu.angle[2];
soyooo 9:ce5a1315fe0d 70
soyooo 9:ce5a1315fe0d 71 //許容誤差
TanakaTarou 12:91218718ae75 72 pidRobotX.allowable_error = 0.03;
soyooo 9:ce5a1315fe0d 73 pidRobotY.allowable_error = 0.1;
soyooo 9:ce5a1315fe0d 74 pidRobotYow.allowable_error = 2;
soyooo 9:ce5a1315fe0d 75 pidUss.allowable_error = 3;
soyooo 9:ce5a1315fe0d 76
TanakaTarou 7:1ee46b2e8dce 77 //PID設定
TanakaTarou 12:91218718ae75 78 float tmp1 = 0;
TanakaTarou 0:6db16ad02a1b 79 pidRobotX.sensor = &odm.x;
TanakaTarou 0:6db16ad02a1b 80 pidRobotX.target = &target_x;
TanakaTarou 0:6db16ad02a1b 81 pidRobotX.start();
TanakaTarou 0:6db16ad02a1b 82 pidRobotY.sensor = &odm.y;
TanakaTarou 0:6db16ad02a1b 83 pidRobotY.target = &target_y;
TanakaTarou 0:6db16ad02a1b 84 pidRobotY.start();
TanakaTarou 5:7c5e07260e1e 85 pidRobotYow.sensor = &imu.angle[2];
TanakaTarou 5:7c5e07260e1e 86 pidRobotYow.target = &target_yow;
TanakaTarou 5:7c5e07260e1e 87 pidRobotYow.start();
TanakaTarou 12:91218718ae75 88 pidUss.sensor = &tmp1;
TanakaTarou 5:7c5e07260e1e 89 pidUss.target = &target_uss;
TanakaTarou 5:7c5e07260e1e 90 pidUss.start();
TanakaTarou 5:7c5e07260e1e 91
TanakaTarou 4:9f74525eb37f 92 //マイクロスイッチ
TanakaTarou 3:6b4adb4d7101 93 sw1.mode(PullUp);
TanakaTarou 3:6b4adb4d7101 94 sw2.mode(PullUp);
TanakaTarou 3:6b4adb4d7101 95
soyooo 9:ce5a1315fe0d 96 odm.x = 0;
soyooo 9:ce5a1315fe0d 97 timer.start();
soyooo 9:ce5a1315fe0d 98
TanakaTarou 6:a102603c99fd 99 while(1)
TanakaTarou 12:91218718ae75 100 {
TanakaTarou 12:91218718ae75 101 riseUssTriger();
TanakaTarou 7:1ee46b2e8dce 102 can.read();
soyooo 9:ce5a1315fe0d 103
soyooo 9:ce5a1315fe0d 104 //目指すべきロボットの状態を取得
soyooo 9:ce5a1315fe0d 105 state tar_state = getTargetState();
TanakaTarou 0:6db16ad02a1b 106
soyooo 18:268ab2ab0b2a 107 int can_angle = 900 - int(tar_state.angle * 10);
soyooo 9:ce5a1315fe0d 108 //canに置く
soyooo 11:b89289eabaa2 109 //角度は2倍して送って、2で割って受け取る 角度可変が0.5度刻みにできる
soyooo 9:ce5a1315fe0d 110 can.set(1, 1, tar_state.shoot);
soyooo 18:268ab2ab0b2a 111 can.set(1, 2, int(can_angle));
soyooo 9:ce5a1315fe0d 112 can.set(1, 3, tar_state.supply);
TanakaTarou 3:6b4adb4d7101 113
soyooo 9:ce5a1315fe0d 114 //送信周期調整
soyooo 9:ce5a1315fe0d 115 static double pre_time = 0;
soyooo 9:ce5a1315fe0d 116 double now_time = timer.read();
soyooo 9:ce5a1315fe0d 117 if(now_time - pre_time >= 0.01)
soyooo 1:d7ceb38da3d8 118 {
soyooo 10:ebb59c1d369e 119 if(can.send())
TanakaTarou 19:bdb503dd1e8c 120 {
soyooo 10:ebb59c1d369e 121 pre_time = now_time;
TanakaTarou 19:bdb503dd1e8c 122 LED[0] = !LED[0];
TanakaTarou 19:bdb503dd1e8c 123 }
TanakaTarou 8:123cd1f07aea 124 }
soyooo 11:b89289eabaa2 125
soyooo 9:ce5a1315fe0d 126 //ロボット速度を取得
soyooo 9:ce5a1315fe0d 127 elements robot_vel = getRobotVelocity(tar_state);
soyooo 11:b89289eabaa2 128 float robot_velocity[3] = {robot_vel.x, robot_vel.y, robot_vel.theta};
soyooo 9:ce5a1315fe0d 129
TanakaTarou 0:6db16ad02a1b 130 //ホイール速度計算
TanakaTarou 4:9f74525eb37f 131 mecanum.setVelG(robot_velocity);
TanakaTarou 0:6db16ad02a1b 132 mecanum.computeWheelVel();
TanakaTarou 0:6db16ad02a1b 133 mecanum.rescaleWheelVel();
soyooo 18:268ab2ab0b2a 134
TanakaTarou 0:6db16ad02a1b 135 //モーターの駆動
TanakaTarou 0:6db16ad02a1b 136 for(int i = 0; i < 4; i++)
TanakaTarou 3:6b4adb4d7101 137 Motor[i].drive(mecanum.wheel_vel[i]);
TanakaTarou 19:bdb503dd1e8c 138 /*
soyooo 11:b89289eabaa2 139 pc.printf("%.2f\t", odm.x);
soyooo 11:b89289eabaa2 140 pc.printf("%.2f\t", odm.y);
TanakaTarou 3:6b4adb4d7101 141 pc.printf("%.2f\t", imu.angle[2]);
soyooo 11:b89289eabaa2 142
TanakaTarou 19:bdb503dd1e8c 143 pc.printf("%.2f\t", uss[0].distance);
TanakaTarou 19:bdb503dd1e8c 144 pc.printf("%.2f\t", uss[1].distance);
TanakaTarou 19:bdb503dd1e8c 145 pc.printf("%.2f\t", enc[0].rotations);
TanakaTarou 19:bdb503dd1e8c 146 pc.printf("%.2f\t", enc[1].rotations);
TanakaTarou 19:bdb503dd1e8c 147
TanakaTarou 19:bdb503dd1e8c 148 for(int i = 0; i < 9; i++)
TanakaTarou 19:bdb503dd1e8c 149 pc.printf("%d\t", table_sw[i]);
TanakaTarou 19:bdb503dd1e8c 150 */
soyooo 9:ce5a1315fe0d 151 pc.printf("\n");
TanakaTarou 0:6db16ad02a1b 152 }
TanakaTarou 0:6db16ad02a1b 153 }
TanakaTarou 0:6db16ad02a1b 154
soyooo 9:ce5a1315fe0d 155 state getTargetState()
soyooo 9:ce5a1315fe0d 156 {
soyooo 18:268ab2ab0b2a 157 static bool have_ever_shot = 0;
soyooo 9:ce5a1315fe0d 158 static bool start_flag = 0;
soyooo 18:268ab2ab0b2a 159
soyooo 18:268ab2ab0b2a 160 int state_num;
soyooo 18:268ab2ab0b2a 161 int tops_num;
soyooo 9:ce5a1315fe0d 162
soyooo 9:ce5a1315fe0d 163 /*
soyooo 9:ce5a1315fe0d 164 スタートボタン待機
TanakaTarou 19:bdb503dd1e8c 165 whileにするとほかの処理がすべて止まるためこれを回避
soyooo 9:ce5a1315fe0d 166 */
soyooo 9:ce5a1315fe0d 167 if(sw1 == 1)
soyooo 9:ce5a1315fe0d 168 start_flag = 1;
soyooo 9:ce5a1315fe0d 169
soyooo 11:b89289eabaa2 170 //sw1が1度も押されていないとき
soyooo 9:ce5a1315fe0d 171 if(!start_flag)
TanakaTarou 19:bdb503dd1e8c 172 state_num = tops_num = odm.x = odm.y = imu.angle[2] = have_ever_shot = 0;
soyooo 18:268ab2ab0b2a 173 else state_num = updateStateNum();
soyooo 9:ce5a1315fe0d 174
soyooo 11:b89289eabaa2 175 //再びスタート待機
soyooo 18:268ab2ab0b2a 176 if(state_num == 0)
TanakaTarou 19:bdb503dd1e8c 177 start_flag = 0;
TanakaTarou 19:bdb503dd1e8c 178
TanakaTarou 19:bdb503dd1e8c 179 for(int i = 0; i < 6; i++)
TanakaTarou 19:bdb503dd1e8c 180 table_sw[i] = can.get(6, i + 1);
TanakaTarou 19:bdb503dd1e8c 181 for(int i = 1; i < 4; i++)
TanakaTarou 19:bdb503dd1e8c 182 table_sw[i + 5] = can.get(7, i);
soyooo 18:268ab2ab0b2a 183
soyooo 18:268ab2ab0b2a 184 int target_table = state_lib[state_num][3];
soyooo 18:268ab2ab0b2a 185 bool is_shoot = state_lib[state_num][4];
soyooo 18:268ab2ab0b2a 186
soyooo 18:268ab2ab0b2a 187 have_ever_shot |= is_shoot;
soyooo 18:268ab2ab0b2a 188
soyooo 18:268ab2ab0b2a 189 if(have_ever_shot)
soyooo 18:268ab2ab0b2a 190 tops_lib[1][2] = 1;
soyooo 18:268ab2ab0b2a 191 else tops_lib[1][2] = 0;
soyooo 18:268ab2ab0b2a 192
soyooo 18:268ab2ab0b2a 193 tops_lib[3][0] = para_lib[target_table].vel;
soyooo 18:268ab2ab0b2a 194 tops_lib[2][1] = tops_lib[3][1] = para_lib[target_table].angle;
TanakaTarou 19:bdb503dd1e8c 195
soyooo 18:268ab2ab0b2a 196 tops_num = updateTopsNum(is_shoot);
soyooo 9:ce5a1315fe0d 197
TanakaTarou 19:bdb503dd1e8c 198 pc.printf("%d\t", state_num);
TanakaTarou 19:bdb503dd1e8c 199 pc.printf("%d\t", tops_num);
TanakaTarou 19:bdb503dd1e8c 200
soyooo 9:ce5a1315fe0d 201 state a;
TanakaTarou 19:bdb503dd1e8c 202
TanakaTarou 19:bdb503dd1e8c 203 //changeZoneの処理
TanakaTarou 19:bdb503dd1e8c 204 if(table_sw[8] == 1)
TanakaTarou 19:bdb503dd1e8c 205 {
TanakaTarou 19:bdb503dd1e8c 206 a.x = -1 * state_lib[state_num][0];
TanakaTarou 19:bdb503dd1e8c 207 a.theta = -1 * state_lib[state_num][2];
TanakaTarou 19:bdb503dd1e8c 208 } else {
TanakaTarou 19:bdb503dd1e8c 209 a.x = state_lib[state_num][0];
TanakaTarou 19:bdb503dd1e8c 210 a.theta = state_lib[state_num][2];
TanakaTarou 19:bdb503dd1e8c 211 }
soyooo 18:268ab2ab0b2a 212 a.y = state_lib[state_num][1];
soyooo 18:268ab2ab0b2a 213 a.shoot = tops_lib[tops_num][0];
soyooo 18:268ab2ab0b2a 214 a.angle = tops_lib[tops_num][1];
soyooo 18:268ab2ab0b2a 215 a.supply = tops_lib[tops_num][2];
soyooo 9:ce5a1315fe0d 216 return a;
soyooo 9:ce5a1315fe0d 217 }
soyooo 9:ce5a1315fe0d 218
soyooo 18:268ab2ab0b2a 219 bool is_shot = 0;
soyooo 18:268ab2ab0b2a 220
soyooo 9:ce5a1315fe0d 221 int updateStateNum()
soyooo 9:ce5a1315fe0d 222 {
soyooo 9:ce5a1315fe0d 223 /*
soyooo 9:ce5a1315fe0d 224 状態収束を判定して次のステップに進む
soyooo 9:ce5a1315fe0d 225 */
soyooo 9:ce5a1315fe0d 226 float t = 0.1;
soyooo 9:ce5a1315fe0d 227 static int num = 0;
soyooo 9:ce5a1315fe0d 228 //x方向、y方向、yow方向の判定をはっきりしておく
soyooo 18:268ab2ab0b2a 229 bool flag_x = 0, flag_y = 0, flag_yow = 0, flag_tops = 0;
soyooo 9:ce5a1315fe0d 230
soyooo 9:ce5a1315fe0d 231 if(pidRobotYow.isConvergence(t))
soyooo 9:ce5a1315fe0d 232 flag_yow = 1;
soyooo 9:ce5a1315fe0d 233
TanakaTarou 12:91218718ae75 234 if(state_lib[num][0] >= 10 || state_lib[num][0] <= -10)
soyooo 9:ce5a1315fe0d 235 {
soyooo 18:268ab2ab0b2a 236 if(pidUss.isConvergence(t+1))
TanakaTarou 12:91218718ae75 237 flag_x = flag_y = 1;
TanakaTarou 12:91218718ae75 238 }
TanakaTarou 12:91218718ae75 239 else if(pidRobotX.isConvergence(t) == 1)
TanakaTarou 12:91218718ae75 240 flag_x = 1;
TanakaTarou 19:bdb503dd1e8c 241
TanakaTarou 12:91218718ae75 242 //超音波で近づくとき
TanakaTarou 12:91218718ae75 243 if(state_lib[num][1] >= 10 || state_lib[num][1] <= -10)
TanakaTarou 12:91218718ae75 244 {
soyooo 18:268ab2ab0b2a 245 if(pidUss.isConvergence(t+1))
TanakaTarou 12:91218718ae75 246 flag_y = flag_x = 1;
soyooo 9:ce5a1315fe0d 247 }
soyooo 9:ce5a1315fe0d 248 else if(pidRobotY.isConvergence(t) == 1)
TanakaTarou 12:91218718ae75 249 flag_y = 1;
soyooo 9:ce5a1315fe0d 250
soyooo 18:268ab2ab0b2a 251 if(state_lib[num][4] == 1)
soyooo 18:268ab2ab0b2a 252 {
soyooo 18:268ab2ab0b2a 253 if(is_shot)
soyooo 18:268ab2ab0b2a 254 flag_tops = 1;
soyooo 18:268ab2ab0b2a 255 }
soyooo 18:268ab2ab0b2a 256 else flag_tops = 1;
soyooo 9:ce5a1315fe0d 257
soyooo 9:ce5a1315fe0d 258 //全部が収束してるか
soyooo 18:268ab2ab0b2a 259 if(flag_x && flag_y && flag_yow && flag_tops)
TanakaTarou 19:bdb503dd1e8c 260 {
soyooo 9:ce5a1315fe0d 261 num++;
TanakaTarou 19:bdb503dd1e8c 262 is_shot = 0;
TanakaTarou 19:bdb503dd1e8c 263 }
soyooo 9:ce5a1315fe0d 264 //振り出しに戻る
soyooo 9:ce5a1315fe0d 265 if(num >= LIBNUM)
soyooo 9:ce5a1315fe0d 266 num = 0;
soyooo 9:ce5a1315fe0d 267
TanakaTarou 19:bdb503dd1e8c 268 num = changeNextTable(num);
TanakaTarou 19:bdb503dd1e8c 269
soyooo 18:268ab2ab0b2a 270 pc.printf("%d\t", flag_x);
soyooo 18:268ab2ab0b2a 271 pc.printf("%d\t", flag_y);
soyooo 18:268ab2ab0b2a 272 pc.printf("%d\t", flag_yow);
soyooo 18:268ab2ab0b2a 273 pc.printf("%d\t", flag_tops);
TanakaTarou 19:bdb503dd1e8c 274 pc.printf("%d\t", pidRobotX.isConvergence(0.1));
TanakaTarou 19:bdb503dd1e8c 275 pc.printf("%d\t", pidRobotY.isConvergence(0.1));
TanakaTarou 19:bdb503dd1e8c 276 pc.printf("%d\t", pidRobotYow.isConvergence(0.1));
TanakaTarou 19:bdb503dd1e8c 277
soyooo 9:ce5a1315fe0d 278 return num;
soyooo 9:ce5a1315fe0d 279 }
soyooo 9:ce5a1315fe0d 280
soyooo 18:268ab2ab0b2a 281 int updateTopsNum(bool is_shoot)
soyooo 18:268ab2ab0b2a 282 {
TanakaTarou 19:bdb503dd1e8c 283 static bool pre_is_shoot = 0;
soyooo 18:268ab2ab0b2a 284 static int num = 0;
soyooo 18:268ab2ab0b2a 285 if(isConvergenceCatapult(num) && isConvergenceSupply(num))
soyooo 18:268ab2ab0b2a 286 {
soyooo 18:268ab2ab0b2a 287 if(num != 2)
soyooo 18:268ab2ab0b2a 288 num++;
TanakaTarou 19:bdb503dd1e8c 289 else if(is_shoot && !pre_is_shoot)
TanakaTarou 19:bdb503dd1e8c 290 {
soyooo 18:268ab2ab0b2a 291 num++;
TanakaTarou 19:bdb503dd1e8c 292 pre_is_shoot = 1;
TanakaTarou 19:bdb503dd1e8c 293 }
soyooo 18:268ab2ab0b2a 294 }
soyooo 18:268ab2ab0b2a 295 if(num >= 4)
soyooo 18:268ab2ab0b2a 296 {
soyooo 18:268ab2ab0b2a 297 num = 0;
soyooo 18:268ab2ab0b2a 298 is_shot = 1;
soyooo 18:268ab2ab0b2a 299 }
TanakaTarou 19:bdb503dd1e8c 300 if(!is_shoot)
TanakaTarou 19:bdb503dd1e8c 301 pre_is_shoot = 0;
TanakaTarou 19:bdb503dd1e8c 302
TanakaTarou 19:bdb503dd1e8c 303 return num;
TanakaTarou 19:bdb503dd1e8c 304 }
TanakaTarou 19:bdb503dd1e8c 305 int changeNextTable(int num)
TanakaTarou 19:bdb503dd1e8c 306 {
TanakaTarou 19:bdb503dd1e8c 307 if(table_sw[3] == 1 && table_sw[4] == 0)
TanakaTarou 19:bdb503dd1e8c 308 {
TanakaTarou 19:bdb503dd1e8c 309 if(num > 4 && num < 17)
TanakaTarou 19:bdb503dd1e8c 310 num = 17;
TanakaTarou 19:bdb503dd1e8c 311 }
TanakaTarou 19:bdb503dd1e8c 312 else if(table_sw[3] == 0 && table_sw[4] == 1)
TanakaTarou 19:bdb503dd1e8c 313 {
TanakaTarou 19:bdb503dd1e8c 314 if(num > 0 && num < 5)
TanakaTarou 19:bdb503dd1e8c 315 num = 5;
TanakaTarou 19:bdb503dd1e8c 316 if(num > 8 && num < 17)
TanakaTarou 19:bdb503dd1e8c 317 num = 17;
TanakaTarou 19:bdb503dd1e8c 318 }
TanakaTarou 19:bdb503dd1e8c 319 else if(table_sw[3] == 1 && table_sw[4] == 1)
TanakaTarou 19:bdb503dd1e8c 320 {
TanakaTarou 19:bdb503dd1e8c 321 if(num > 0 && num < 9)
TanakaTarou 19:bdb503dd1e8c 322 num = 9;
TanakaTarou 19:bdb503dd1e8c 323 }
TanakaTarou 19:bdb503dd1e8c 324 else if(num > 0 && num < 18)
TanakaTarou 19:bdb503dd1e8c 325 num = 18;
TanakaTarou 19:bdb503dd1e8c 326 if(table_sw[5] == 0 && table_sw[6] == 0 && table_sw[7] == 0)
TanakaTarou 19:bdb503dd1e8c 327 {
TanakaTarou 19:bdb503dd1e8c 328 if(num > 16 && num < 30)
TanakaTarou 19:bdb503dd1e8c 329 num = 30;
TanakaTarou 19:bdb503dd1e8c 330 }
TanakaTarou 19:bdb503dd1e8c 331 else
TanakaTarou 19:bdb503dd1e8c 332 {
TanakaTarou 19:bdb503dd1e8c 333 if(table_sw[5] == 0 && num > 17 && num < 22)
TanakaTarou 19:bdb503dd1e8c 334 num = 22;
TanakaTarou 19:bdb503dd1e8c 335 if(table_sw[6] == 0 && num > 21 && num < 26)
TanakaTarou 19:bdb503dd1e8c 336 num = 26;
TanakaTarou 19:bdb503dd1e8c 337 if(table_sw[7] == 0 && num > 25 && num < 30)
TanakaTarou 19:bdb503dd1e8c 338 num = 30;
TanakaTarou 19:bdb503dd1e8c 339 }
TanakaTarou 19:bdb503dd1e8c 340 if(table_sw[0] == 1 || table_sw[1] == 1 || table_sw[2] == 1)
TanakaTarou 19:bdb503dd1e8c 341 {
TanakaTarou 19:bdb503dd1e8c 342 if(table_sw[0] == 0 && num > 30 && num < 35)
TanakaTarou 19:bdb503dd1e8c 343 num = 35;
TanakaTarou 19:bdb503dd1e8c 344 if(table_sw[1] == 0 && num < 34 && num > 39)
TanakaTarou 19:bdb503dd1e8c 345 num = 39;
TanakaTarou 19:bdb503dd1e8c 346 if(table_sw[2] == 0 && num > 38 && num < 43)
TanakaTarou 19:bdb503dd1e8c 347 num = 42;
TanakaTarou 19:bdb503dd1e8c 348 if(table_sw[1] == 0 && table_sw[2] == 0 && num == 43)
TanakaTarou 19:bdb503dd1e8c 349 num = 44;
TanakaTarou 19:bdb503dd1e8c 350 }
TanakaTarou 19:bdb503dd1e8c 351 else if(num > 29 && num < 44)
TanakaTarou 19:bdb503dd1e8c 352 num = 44;
TanakaTarou 19:bdb503dd1e8c 353
soyooo 18:268ab2ab0b2a 354 return num;
soyooo 18:268ab2ab0b2a 355 }
soyooo 18:268ab2ab0b2a 356
soyooo 18:268ab2ab0b2a 357 bool isConvergenceCatapult(int num)
soyooo 18:268ab2ab0b2a 358 {
soyooo 18:268ab2ab0b2a 359 int velocity_pid = can.get(3, 1);
soyooo 18:268ab2ab0b2a 360 int angle_pid = can.get(3, 2);
soyooo 18:268ab2ab0b2a 361 int velocity_val = can.get(3, 3);
soyooo 18:268ab2ab0b2a 362 float angle_val = can.get(3, 4);
soyooo 18:268ab2ab0b2a 363 angle_val = 90 - (angle_val / 10.0);
soyooo 18:268ab2ab0b2a 364
soyooo 18:268ab2ab0b2a 365 if(angle_pid == 1 && velocity_pid == 1 && velocity_val == tops_lib[num][0] && angle_val == tops_lib[num][1])
soyooo 18:268ab2ab0b2a 366 return 1;
soyooo 18:268ab2ab0b2a 367 else return 0;
soyooo 18:268ab2ab0b2a 368 }
soyooo 18:268ab2ab0b2a 369
soyooo 18:268ab2ab0b2a 370 bool isConvergenceSupply(int num)
soyooo 18:268ab2ab0b2a 371 {
soyooo 18:268ab2ab0b2a 372 int is_supply_done = can.get(3, 5);
soyooo 18:268ab2ab0b2a 373
soyooo 18:268ab2ab0b2a 374 if(tops_lib[num][2] == 1)
soyooo 18:268ab2ab0b2a 375 {
soyooo 18:268ab2ab0b2a 376 if(is_supply_done == 1)
soyooo 18:268ab2ab0b2a 377 return 1;
soyooo 18:268ab2ab0b2a 378 else return 0;
soyooo 18:268ab2ab0b2a 379 }else return 1;
soyooo 18:268ab2ab0b2a 380 }
soyooo 18:268ab2ab0b2a 381
soyooo 9:ce5a1315fe0d 382 elements getRobotVelocity(state a)
soyooo 9:ce5a1315fe0d 383 {
soyooo 9:ce5a1315fe0d 384 elements vel;
soyooo 9:ce5a1315fe0d 385
soyooo 9:ce5a1315fe0d 386 //yow角調整処理
soyooo 9:ce5a1315fe0d 387 *pidRobotYow.target = a.theta;
soyooo 9:ce5a1315fe0d 388 vel.theta = pidRobotYow.output;
soyooo 9:ce5a1315fe0d 389
TanakaTarou 12:91218718ae75 390 float right_dis = uss[0].distance;
TanakaTarou 12:91218718ae75 391 float left_dis = uss[1].distance;
TanakaTarou 12:91218718ae75 392 float error = right_dis - left_dis;
TanakaTarou 12:91218718ae75 393 if(right_dis < left_dis)
TanakaTarou 12:91218718ae75 394 *pidUss.sensor = right_dis;
TanakaTarou 12:91218718ae75 395 else *pidUss.sensor = left_dis;
soyooo 9:ce5a1315fe0d 396
TanakaTarou 12:91218718ae75 397 if(a.x >= 10 || a.x <= -10 || a.y >= 10 || a.y <= -10)
soyooo 9:ce5a1315fe0d 398 {
TanakaTarou 12:91218718ae75 399 if(a.x >= 10)
TanakaTarou 12:91218718ae75 400 {
TanakaTarou 12:91218718ae75 401 if(error > 100)
TanakaTarou 12:91218718ae75 402 vel.y = 0.2;
TanakaTarou 12:91218718ae75 403 else if(error < -100)
TanakaTarou 12:91218718ae75 404 vel.y = -0.2;
TanakaTarou 12:91218718ae75 405 else vel.y = 0;
TanakaTarou 12:91218718ae75 406
TanakaTarou 12:91218718ae75 407 *pidUss.target = a.x - 10;
TanakaTarou 12:91218718ae75 408 vel.x = -pidUss.output;
TanakaTarou 12:91218718ae75 409 }
TanakaTarou 12:91218718ae75 410 else if(a.x <= -10)
TanakaTarou 12:91218718ae75 411 {
TanakaTarou 12:91218718ae75 412 if(error > 100)
TanakaTarou 12:91218718ae75 413 vel.y = -0.2;
TanakaTarou 12:91218718ae75 414 else if(error < -100)
TanakaTarou 12:91218718ae75 415 vel.y = 0.2;
TanakaTarou 12:91218718ae75 416 else vel.y = 0;
TanakaTarou 12:91218718ae75 417
TanakaTarou 12:91218718ae75 418 *pidUss.target = -a.x - 10;
TanakaTarou 12:91218718ae75 419 vel.x = pidUss.output;
TanakaTarou 12:91218718ae75 420 }
TanakaTarou 12:91218718ae75 421 else if(a.y >= 10)
TanakaTarou 12:91218718ae75 422 {
TanakaTarou 12:91218718ae75 423 if(error > 100)
TanakaTarou 12:91218718ae75 424 vel.x = -0.2;
TanakaTarou 12:91218718ae75 425 else if(error < -100)
TanakaTarou 12:91218718ae75 426 vel.x = 0.2;
TanakaTarou 12:91218718ae75 427 else vel.x = 0;
TanakaTarou 12:91218718ae75 428
TanakaTarou 12:91218718ae75 429 *pidUss.target = a.y - 10;
TanakaTarou 12:91218718ae75 430 vel.y = -pidUss.output;
TanakaTarou 12:91218718ae75 431 }
TanakaTarou 12:91218718ae75 432 else if(a.y <= -10)
TanakaTarou 12:91218718ae75 433 {
TanakaTarou 12:91218718ae75 434 if(error > 100)
TanakaTarou 12:91218718ae75 435 vel.x = 0.2;
TanakaTarou 12:91218718ae75 436 else if(error < -100)
TanakaTarou 12:91218718ae75 437 vel.x = -0.2;
TanakaTarou 12:91218718ae75 438 else vel.x = 0;
TanakaTarou 12:91218718ae75 439
TanakaTarou 12:91218718ae75 440 *pidUss.target = -a.y - 10;
TanakaTarou 12:91218718ae75 441 vel.y = pidUss.output;
TanakaTarou 12:91218718ae75 442 }
soyooo 9:ce5a1315fe0d 443 }
soyooo 9:ce5a1315fe0d 444 else
soyooo 9:ce5a1315fe0d 445 {
TanakaTarou 12:91218718ae75 446 *pidUss.target = 0;
soyooo 9:ce5a1315fe0d 447 *pidRobotX.target = a.x;
soyooo 9:ce5a1315fe0d 448 *pidRobotY.target = a.y;
soyooo 18:268ab2ab0b2a 449 vel.x = pidRobotX.output;
soyooo 9:ce5a1315fe0d 450 vel.y = pidRobotY.output;
soyooo 9:ce5a1315fe0d 451 }
soyooo 9:ce5a1315fe0d 452 return vel;
soyooo 9:ce5a1315fe0d 453 }
soyooo 18:268ab2ab0b2a 454
soyooo 9:ce5a1315fe0d 455 controller getPropoData()
soyooo 9:ce5a1315fe0d 456 {
soyooo 9:ce5a1315fe0d 457 float dead_zone = 0.05;
soyooo 9:ce5a1315fe0d 458 controller propo;
soyooo 9:ce5a1315fe0d 459 sbus.isFailSafe();
soyooo 9:ce5a1315fe0d 460
soyooo 9:ce5a1315fe0d 461 //propo直接コントロール
soyooo 9:ce5a1315fe0d 462 if(sbus.isFailSafe())
soyooo 9:ce5a1315fe0d 463 {
soyooo 9:ce5a1315fe0d 464 propo.LX = propo.LY = propo.RX = propo.RY = 0;
soyooo 9:ce5a1315fe0d 465 propo.H = propo.A = propo.D = propo.F = propo.G = 0;
soyooo 9:ce5a1315fe0d 466 }
soyooo 9:ce5a1315fe0d 467 else
soyooo 9:ce5a1315fe0d 468 {
soyooo 9:ce5a1315fe0d 469 propo.LX = sbus.getStickVal(0) / 255.0;
soyooo 9:ce5a1315fe0d 470 propo.LY = sbus.getStickVal(1) / 255.0;
soyooo 9:ce5a1315fe0d 471 propo.RX = -sbus.getStickVal(2) / 255.0;
soyooo 9:ce5a1315fe0d 472 propo.RY = sbus.getStickVal(3) / 255.0;
soyooo 9:ce5a1315fe0d 473 propo.H = sbus.getSwitchVal(0);
soyooo 10:ebb59c1d369e 474 propo.C = sbus.getSwitchVal(1);
soyooo 10:ebb59c1d369e 475 propo.E = sbus.getSwitchVal(2);
soyooo 9:ce5a1315fe0d 476 propo.F = sbus.getSwitchVal(3);
soyooo 9:ce5a1315fe0d 477 propo.G = sbus.getSwitchVal(4);
soyooo 9:ce5a1315fe0d 478 }
soyooo 9:ce5a1315fe0d 479
soyooo 9:ce5a1315fe0d 480 if(propo.RX < dead_zone && propo.RX > -dead_zone) propo.RX = 0;
soyooo 9:ce5a1315fe0d 481 if(propo.RY < dead_zone && propo.RY > -dead_zone) propo.RY = 0;
soyooo 9:ce5a1315fe0d 482 if(propo.LX < dead_zone && propo.LX > -dead_zone) propo.LX = 0;
soyooo 9:ce5a1315fe0d 483 if(propo.LY < dead_zone && propo.LY > -dead_zone) propo.LY = 0;
soyooo 9:ce5a1315fe0d 484 return propo;
TanakaTarou 12:91218718ae75 485 }
TanakaTarou 12:91218718ae75 486
TanakaTarou 12:91218718ae75 487 void riseUssTriger()
TanakaTarou 12:91218718ae75 488 {
TanakaTarou 12:91218718ae75 489 static double start_time = timer.read();
TanakaTarou 12:91218718ae75 490 static bool flag = 1;
TanakaTarou 12:91218718ae75 491 double now_time = timer.read();
TanakaTarou 12:91218718ae75 492 if(now_time - start_time > 0.05)
TanakaTarou 12:91218718ae75 493 {
TanakaTarou 12:91218718ae75 494 if(flag)
TanakaTarou 12:91218718ae75 495 {
TanakaTarou 12:91218718ae75 496 uss[0].riseTrigerEvent();
TanakaTarou 12:91218718ae75 497 flag = 0;
TanakaTarou 12:91218718ae75 498 }
TanakaTarou 12:91218718ae75 499 else
TanakaTarou 12:91218718ae75 500 {
TanakaTarou 12:91218718ae75 501 uss[1].riseTrigerEvent();
TanakaTarou 12:91218718ae75 502 flag = 1;
TanakaTarou 12:91218718ae75 503 }
TanakaTarou 12:91218718ae75 504 start_time = now_time;
TanakaTarou 12:91218718ae75 505 }
soyooo 10:ebb59c1d369e 506 }