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@18:268ab2ab0b2a, 2018-09-27 (annotated)

Committer:
soyooo
Date:
Thu Sep 27 05:28:37 2018 +0000
Revision:
18:268ab2ab0b2a
Parent:
13:0479a4f3e997
Child:
19:bdb503dd1e8c
tops_lib;

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