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@13:0479a4f3e997, 2018-09-15 (annotated)

Committer:
TanakaTarou
Date:
Sat Sep 15 06:48:13 2018 +0000
Revision:
13:0479a4f3e997
Parent:
12:91218718ae75
Child:
14:8334c241bb0a
Child:
18:268ab2ab0b2a
?????????????????

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