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@17:1409cdfb6043, 2018-09-25 (annotated)

Committer:
TanakaTarou
Date:
Tue Sep 25 08:53:56 2018 +0000
Revision:
17:1409cdfb6043
Parent:
16:621f04b15f86 
9/25  changeZone???????

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();
TanakaTarou 15:ae2043fde152 17 int updateStateNum(int tops_num);
TanakaTarou 15:ae2043fde152 18 int updateTopsNum(int num);
TanakaTarou 15:ae2043fde152 19 int changeNextTable(int num);
TanakaTarou 15:ae2043fde152 20 state updateTopsState(float val, int tops_num);
soyooo 10:ebb59c1d369e 21 bool isConvergenceTops(int);
soyooo 10:ebb59c1d369e 22 bool isConvergenceSupply(int);
soyooo 9:ce5a1315fe0d 23 controller getPropoData();
TanakaTarou 12:91218718ae75 24 void riseUssTriger();
TanakaTarou 0:6db16ad02a1b 25
TanakaTarou 0:6db16ad02a1b 26 //x軸補正用 PID
TanakaTarou 15:ae2043fde152 27 PID pidRobotX(4, 0, 0, 0.01, 0.5, &timer);
TanakaTarou 0:6db16ad02a1b 28 float target_x = 0;
TanakaTarou 0:6db16ad02a1b 29
TanakaTarou 0:6db16ad02a1b 30 //y軸補正用 PID
TanakaTarou 15:ae2043fde152 31 PID pidRobotY(2, 0, 0, 0.01, 0.5, &timer);
TanakaTarou 0:6db16ad02a1b 32 float target_y = 0;
TanakaTarou 0:6db16ad02a1b 33
soyooo 1:d7ceb38da3d8 34 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
TanakaTarou 12:91218718ae75 35 PID pidRobotYow(0.05, 0, 0, 0.01, 0.3, &timer);
soyooo 1:d7ceb38da3d8 36 float target_yow = 0;
soyooo 9:ce5a1315fe0d 37
TanakaTarou 0:6db16ad02a1b 38 //USS用
TanakaTarou 5:7c5e07260e1e 39 PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
soyooo 9:ce5a1315fe0d 40 float target_uss = 0;
TanakaTarou 3:6b4adb4d7101 41
TanakaTarou 12:91218718ae75 42 //オドメーターの定義
TanakaTarou 12:91218718ae75 43 float matrix[3][3] = {{1, 0, 0},
TanakaTarou 12:91218718ae75 44 {0, 1, 0},
TanakaTarou 12:91218718ae75 45 {0, 0, 0}
TanakaTarou 12:91218718ae75 46 };
TanakaTarou 12:91218718ae75 47 Odometer odm(matrix, 0.050);
TanakaTarou 15:ae2043fde152 48
TanakaTarou 15:ae2043fde152 49 int table_sw[9];
TanakaTarou 12:91218718ae75 50
TanakaTarou 0:6db16ad02a1b 51 int main()
TanakaTarou 0:6db16ad02a1b 52 {
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
TanakaTarou 15:ae2043fde152 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);
TanakaTarou 15:ae2043fde152 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())
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 14:8334c241bb0a 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
TanakaTarou 17:1409cdfb6043 136 /*
soyooo 11:b89289eabaa2 137 pc.printf("%.2f\t", odm.x);
soyooo 11:b89289eabaa2 138 pc.printf("%.2f\t", odm.y);
TanakaTarou 3:6b4adb4d7101 139 pc.printf("%.2f\t", imu.angle[2]);
TanakaTarou 16:621f04b15f86 140
TanakaTarou 15:ae2043fde152 141 int angle_pid = can.get(3, 2);
TanakaTarou 15:ae2043fde152 142 float angle_val = can.get(3, 4);
TanakaTarou 15:ae2043fde152 143 angle_val = 90 - (angle_val / 10.0);
TanakaTarou 16:621f04b15f86 144
TanakaTarou 15:ae2043fde152 145 pc.printf("%d\t", angle_pid);
TanakaTarou 15:ae2043fde152 146 pc.printf("%.2f\t", angle_val);
TanakaTarou 15:ae2043fde152 147 pc.printf("%d\t", can_angle);
TanakaTarou 15:ae2043fde152 148 pc.printf("%.2f\t", tar_state.angle);
TanakaTarou 17:1409cdfb6043 149
soyooo 10:ebb59c1d369e 150 pc.printf("%.2f\t", uss[0].distance);
soyooo 10:ebb59c1d369e 151 pc.printf("%.2f\t", uss[1].distance);
TanakaTarou 2:7af15d4ee55a 152
TanakaTarou 14:8334c241bb0a 153 pc.printf("%.2f\t", enc[0].rotations);
TanakaTarou 14:8334c241bb0a 154 pc.printf("%.2f\t", enc[1].rotations);
TanakaTarou 14:8334c241bb0a 155 pc.printf("%.2f\t", enc[2].rotations);
TanakaTarou 17:1409cdfb6043 156 */
TanakaTarou 17:1409cdfb6043 157
TanakaTarou 17:1409cdfb6043 158 for(int i = 0; i < 6; i++)
TanakaTarou 17:1409cdfb6043 159 table_sw[i] = can.get(6, i + 1);
TanakaTarou 17:1409cdfb6043 160 for(int i = 1; i < 4; i++)
TanakaTarou 17:1409cdfb6043 161 table_sw[i + 5] = can.get(7, i);
TanakaTarou 17:1409cdfb6043 162 for(int i = 0; i < 9; i++)
TanakaTarou 17:1409cdfb6043 163 pc.printf("%d\t", table_sw[i]);
TanakaTarou 14:8334c241bb0a 164
soyooo 9:ce5a1315fe0d 165 pc.printf("\n");
TanakaTarou 0:6db16ad02a1b 166 }
TanakaTarou 0:6db16ad02a1b 167 }
soyooo 9:ce5a1315fe0d 168 state getTargetState()
soyooo 9:ce5a1315fe0d 169 {
soyooo 9:ce5a1315fe0d 170 static bool start_flag = 0;
TanakaTarou 15:ae2043fde152 171
TanakaTarou 15:ae2043fde152 172 int num[2];
soyooo 9:ce5a1315fe0d 173
soyooo 9:ce5a1315fe0d 174 /*
soyooo 9:ce5a1315fe0d 175 スタートボタン待機
soyooo 9:ce5a1315fe0d 176 whileにするとほかの処理がすべて止めるためこれを回避
soyooo 9:ce5a1315fe0d 177 */
soyooo 9:ce5a1315fe0d 178 if(sw1 == 1)
soyooo 9:ce5a1315fe0d 179 start_flag = 1;
soyooo 9:ce5a1315fe0d 180
soyooo 11:b89289eabaa2 181 //sw1が1度も押されていないとき
soyooo 9:ce5a1315fe0d 182 if(!start_flag)
TanakaTarou 15:ae2043fde152 183 num[0] = num[1] = odm.x = odm.y = imu.angle[2] = 0;
TanakaTarou 15:ae2043fde152 184 else
TanakaTarou 15:ae2043fde152 185 {
TanakaTarou 15:ae2043fde152 186 num[0] = updateStateNum(num[1]);
TanakaTarou 15:ae2043fde152 187 num[1] = updateTopsNum(num[0]);
TanakaTarou 15:ae2043fde152 188 }
soyooo 9:ce5a1315fe0d 189
soyooo 11:b89289eabaa2 190 //再びスタート待機
TanakaTarou 15:ae2043fde152 191 if(num[0] == 0)
TanakaTarou 15:ae2043fde152 192 start_flag = 0;
TanakaTarou 15:ae2043fde152 193 if(num[1] == 0)
TanakaTarou 15:ae2043fde152 194 num[0] = changeNextTable(num[0]);
soyooo 9:ce5a1315fe0d 195
TanakaTarou 17:1409cdfb6043 196 //states辞典から値を引っ張る
soyooo 9:ce5a1315fe0d 197 state a;
TanakaTarou 17:1409cdfb6043 198
TanakaTarou 17:1409cdfb6043 199 table_sw[8] = can.get(7, 3);
TanakaTarou 17:1409cdfb6043 200 if(table_sw[8] == 0)
TanakaTarou 17:1409cdfb6043 201 {
TanakaTarou 17:1409cdfb6043 202 a.x = -1 * state_lib[num[0]][0];
TanakaTarou 17:1409cdfb6043 203 a.theta = -1 * state_lib[num[0]][2];
TanakaTarou 17:1409cdfb6043 204 }
TanakaTarou 17:1409cdfb6043 205 else
TanakaTarou 17:1409cdfb6043 206 {
TanakaTarou 17:1409cdfb6043 207 a.x = state_lib[num[0]][0];
TanakaTarou 17:1409cdfb6043 208 a.theta = state_lib[num[0]][2];
TanakaTarou 17:1409cdfb6043 209 }
TanakaTarou 17:1409cdfb6043 210
TanakaTarou 15:ae2043fde152 211 a.y = state_lib[num[0]][1];
TanakaTarou 15:ae2043fde152 212 updateTopsState(state_lib[num[0]][3], num[1]);
TanakaTarou 15:ae2043fde152 213
soyooo 9:ce5a1315fe0d 214 return a;
soyooo 9:ce5a1315fe0d 215 }
TanakaTarou 15:ae2043fde152 216 int updateStateNum(int tops_num)
soyooo 9:ce5a1315fe0d 217 {
soyooo 9:ce5a1315fe0d 218 /*
soyooo 9:ce5a1315fe0d 219 状態収束を判定して次のステップに進む
soyooo 9:ce5a1315fe0d 220 */
soyooo 9:ce5a1315fe0d 221 float t = 0.1;
soyooo 9:ce5a1315fe0d 222 static int num = 0;
TanakaTarou 15:ae2043fde152 223 //x方向、y方向、yow方向、topsの判定をはっきりしておく
soyooo 9:ce5a1315fe0d 224 bool flag_x = 0, flag_y = 0, flag_yow = 0;
soyooo 9:ce5a1315fe0d 225
soyooo 9:ce5a1315fe0d 226 if(pidRobotYow.isConvergence(t))
soyooo 9:ce5a1315fe0d 227 flag_yow = 1;
soyooo 9:ce5a1315fe0d 228
TanakaTarou 12:91218718ae75 229 if(state_lib[num][0] >= 10 || state_lib[num][0] <= -10)
soyooo 9:ce5a1315fe0d 230 {
TanakaTarou 15:ae2043fde152 231 if(pidUss.isConvergence(t+1) && isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
TanakaTarou 12:91218718ae75 232 flag_x = flag_y = 1;
TanakaTarou 12:91218718ae75 233 }
TanakaTarou 12:91218718ae75 234 else if(pidRobotX.isConvergence(t) == 1)
TanakaTarou 12:91218718ae75 235 flag_x = 1;
TanakaTarou 12:91218718ae75 236
TanakaTarou 12:91218718ae75 237 //超音波で近づくとき
TanakaTarou 12:91218718ae75 238 if(state_lib[num][1] >= 10 || state_lib[num][1] <= -10)
TanakaTarou 12:91218718ae75 239 {
TanakaTarou 15:ae2043fde152 240 if(pidUss.isConvergence(t+1) && isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
TanakaTarou 12:91218718ae75 241 flag_y = flag_x = 1;
soyooo 9:ce5a1315fe0d 242 }
soyooo 9:ce5a1315fe0d 243 else if(pidRobotY.isConvergence(t) == 1)
TanakaTarou 12:91218718ae75 244 flag_y = 1;
soyooo 9:ce5a1315fe0d 245
soyooo 9:ce5a1315fe0d 246 //全部が収束してるか
soyooo 9:ce5a1315fe0d 247 if(flag_x && flag_y && flag_yow)
soyooo 9:ce5a1315fe0d 248 num++;
soyooo 9:ce5a1315fe0d 249
soyooo 9:ce5a1315fe0d 250 //振り出しに戻る
soyooo 9:ce5a1315fe0d 251 if(num >= LIBNUM)
soyooo 9:ce5a1315fe0d 252 num = 0;
soyooo 9:ce5a1315fe0d 253
soyooo 9:ce5a1315fe0d 254 return num;
soyooo 9:ce5a1315fe0d 255 }
TanakaTarou 15:ae2043fde152 256 int updateTopsNum(int num)
TanakaTarou 15:ae2043fde152 257 {
TanakaTarou 15:ae2043fde152 258 static int tops_num = 0;
TanakaTarou 15:ae2043fde152 259 bool flag_tops = 0;
TanakaTarou 15:ae2043fde152 260
TanakaTarou 15:ae2043fde152 261 if(isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
TanakaTarou 15:ae2043fde152 262 {
TanakaTarou 15:ae2043fde152 263 if(state_lib[num][4] == 1)
TanakaTarou 15:ae2043fde152 264 flag_tops = 1;
TanakaTarou 15:ae2043fde152 265 else
TanakaTarou 15:ae2043fde152 266 {
TanakaTarou 15:ae2043fde152 267 if(tops_num == 1)
TanakaTarou 15:ae2043fde152 268 flag_tops = 0;
TanakaTarou 15:ae2043fde152 269 else flag_tops = 1;
TanakaTarou 15:ae2043fde152 270 }
TanakaTarou 15:ae2043fde152 271 }
TanakaTarou 15:ae2043fde152 272 if(flag_tops)
TanakaTarou 15:ae2043fde152 273 tops_num++;
TanakaTarou 15:ae2043fde152 274 if(tops_num >= 4)
TanakaTarou 15:ae2043fde152 275 tops_num = 0;
TanakaTarou 15:ae2043fde152 276
TanakaTarou 15:ae2043fde152 277 return tops_num;
TanakaTarou 15:ae2043fde152 278 }
TanakaTarou 15:ae2043fde152 279 int changeNextTable(int num)
TanakaTarou 15:ae2043fde152 280 {
TanakaTarou 15:ae2043fde152 281 for(int i = 0; i < 6; i++)
TanakaTarou 17:1409cdfb6043 282 table_sw[i] = can.get(6, i + 1);
TanakaTarou 17:1409cdfb6043 283 for(int i = 1; i < 3; i++)
TanakaTarou 17:1409cdfb6043 284 table_sw[i + 5] = can.get(7, i);
TanakaTarou 15:ae2043fde152 285
TanakaTarou 15:ae2043fde152 286 if(table_sw[3] == 1 && table_sw[4] == 0)
TanakaTarou 15:ae2043fde152 287 {
TanakaTarou 15:ae2043fde152 288 if(num == 5)
TanakaTarou 15:ae2043fde152 289 num = 17;
TanakaTarou 15:ae2043fde152 290 }
TanakaTarou 15:ae2043fde152 291 else if(table_sw[3] == 0 && table_sw[4] == 1)
TanakaTarou 15:ae2043fde152 292 {
TanakaTarou 15:ae2043fde152 293 if(num == 1)
TanakaTarou 15:ae2043fde152 294 num = 5;
TanakaTarou 15:ae2043fde152 295 if(num == 9)
TanakaTarou 15:ae2043fde152 296 num = 17;
TanakaTarou 15:ae2043fde152 297 }
TanakaTarou 15:ae2043fde152 298 else if(table_sw[3] == 1 && table_sw[4] == 1)
TanakaTarou 15:ae2043fde152 299 {
TanakaTarou 15:ae2043fde152 300 if(num == 1)
TanakaTarou 15:ae2043fde152 301 num = 9;
TanakaTarou 15:ae2043fde152 302 }
TanakaTarou 16:621f04b15f86 303 else
TanakaTarou 16:621f04b15f86 304 {
TanakaTarou 16:621f04b15f86 305 if(num == 1)
TanakaTarou 17:1409cdfb6043 306 num = 18;
TanakaTarou 16:621f04b15f86 307 }
TanakaTarou 15:ae2043fde152 308 if(table_sw[5] == 0 && table_sw[6] == 0 && table_sw[7] == 0)
TanakaTarou 15:ae2043fde152 309 {
TanakaTarou 15:ae2043fde152 310 if(num == 17)
TanakaTarou 15:ae2043fde152 311 num = 43;
TanakaTarou 15:ae2043fde152 312 }
TanakaTarou 15:ae2043fde152 313 if(table_sw[5] == 0)
TanakaTarou 15:ae2043fde152 314 {
TanakaTarou 15:ae2043fde152 315 if(num == 18)
TanakaTarou 15:ae2043fde152 316 num = 22;
TanakaTarou 15:ae2043fde152 317 }
TanakaTarou 15:ae2043fde152 318 if(table_sw[6] == 0)
TanakaTarou 15:ae2043fde152 319 {
TanakaTarou 15:ae2043fde152 320 if(num == 22)
TanakaTarou 15:ae2043fde152 321 num = 26;
TanakaTarou 15:ae2043fde152 322 }
TanakaTarou 15:ae2043fde152 323 if(table_sw[7] == 0)
TanakaTarou 15:ae2043fde152 324 {
TanakaTarou 15:ae2043fde152 325 if(num == 26)
TanakaTarou 15:ae2043fde152 326 num = 30;
TanakaTarou 15:ae2043fde152 327 }
TanakaTarou 16:621f04b15f86 328 if(table_sw[3] == 0 || table_sw[4] == 0 || table_sw[5] == 0 || table_sw[6] == 0 || table_sw[7] == 0)
TanakaTarou 15:ae2043fde152 329 {
TanakaTarou 16:621f04b15f86 330 if(table_sw[0] == 0)
TanakaTarou 16:621f04b15f86 331 {
TanakaTarou 16:621f04b15f86 332 if(num == 30)
TanakaTarou 16:621f04b15f86 333 num = 34;
TanakaTarou 16:621f04b15f86 334 }
TanakaTarou 16:621f04b15f86 335 if(table_sw[1] == 0)
TanakaTarou 16:621f04b15f86 336 {
TanakaTarou 16:621f04b15f86 337 if(num == 34)
TanakaTarou 16:621f04b15f86 338 num = 38;
TanakaTarou 16:621f04b15f86 339 }
TanakaTarou 16:621f04b15f86 340 if(table_sw[2] == 0)
TanakaTarou 16:621f04b15f86 341 {
TanakaTarou 16:621f04b15f86 342 if(num == 38)
TanakaTarou 16:621f04b15f86 343 num = 42;
TanakaTarou 16:621f04b15f86 344 }
TanakaTarou 16:621f04b15f86 345 if(table_sw[1] == 0 || table_sw[2] == 0)
TanakaTarou 16:621f04b15f86 346 {
TanakaTarou 16:621f04b15f86 347 if(num == 42)
TanakaTarou 16:621f04b15f86 348 num = 43;
TanakaTarou 16:621f04b15f86 349 }
TanakaTarou 15:ae2043fde152 350 }
TanakaTarou 15:ae2043fde152 351
TanakaTarou 15:ae2043fde152 352 return num;
TanakaTarou 15:ae2043fde152 353 }
TanakaTarou 15:ae2043fde152 354 state updateTopsState(float val, int tops_num)
TanakaTarou 15:ae2043fde152 355 {
TanakaTarou 15:ae2043fde152 356 state a;
TanakaTarou 15:ae2043fde152 357 if(val == 1)
TanakaTarou 15:ae2043fde152 358 {
TanakaTarou 15:ae2043fde152 359 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 360 state_tops[tops_num][i] = A_1[tops_num][i];
TanakaTarou 15:ae2043fde152 361 }
TanakaTarou 15:ae2043fde152 362 else if(val == 2)
TanakaTarou 15:ae2043fde152 363 {
TanakaTarou 15:ae2043fde152 364 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 365 state_tops[tops_num][i] = A_2[tops_num][i];
TanakaTarou 15:ae2043fde152 366 }
TanakaTarou 15:ae2043fde152 367 else if(val == 3)
TanakaTarou 15:ae2043fde152 368 {
TanakaTarou 15:ae2043fde152 369 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 370 state_tops[tops_num][i] = A_3[tops_num][i];
TanakaTarou 15:ae2043fde152 371 }
TanakaTarou 15:ae2043fde152 372 else if(val == 4)
TanakaTarou 15:ae2043fde152 373 {
TanakaTarou 15:ae2043fde152 374 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 375 state_tops[tops_num][i] = B_1[tops_num][i];
TanakaTarou 15:ae2043fde152 376 }
TanakaTarou 15:ae2043fde152 377 else if(val == 5)
TanakaTarou 15:ae2043fde152 378 {
TanakaTarou 15:ae2043fde152 379 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 380 state_tops[tops_num][i] = B_2[tops_num][i];
TanakaTarou 15:ae2043fde152 381 }
TanakaTarou 15:ae2043fde152 382 else if(val == 6)
TanakaTarou 15:ae2043fde152 383 {
TanakaTarou 15:ae2043fde152 384 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 385 state_tops[tops_num][i] = C_1[tops_num][i];
TanakaTarou 15:ae2043fde152 386 }
TanakaTarou 15:ae2043fde152 387 else if(val == 7)
TanakaTarou 15:ae2043fde152 388 {
TanakaTarou 15:ae2043fde152 389 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 390 state_tops[tops_num][i] = C_2[tops_num][i];
TanakaTarou 15:ae2043fde152 391 }
TanakaTarou 15:ae2043fde152 392 else if(val == 8)
TanakaTarou 15:ae2043fde152 393 {
TanakaTarou 15:ae2043fde152 394 for(int i = 0; i < 3; i++)
TanakaTarou 15:ae2043fde152 395 state_tops[tops_num][i] = C_3[tops_num][i];
TanakaTarou 15:ae2043fde152 396 }
TanakaTarou 15:ae2043fde152 397 else if(val == 9)
TanakaTarou 15:ae2043fde152 398 {
TanakaTarou 15:ae2043fde152 399 state_tops[0][0] = 0;
TanakaTarou 15:ae2043fde152 400 state_tops[0][1] = 90;
TanakaTarou 15:ae2043fde152 401 state_tops[0][2] = 1;
TanakaTarou 15:ae2043fde152 402 }
TanakaTarou 15:ae2043fde152 403 else
TanakaTarou 15:ae2043fde152 404 {
TanakaTarou 15:ae2043fde152 405 state_tops[0][0] = 0;
TanakaTarou 15:ae2043fde152 406 state_tops[0][1] = 90;
TanakaTarou 15:ae2043fde152 407 state_tops[0][2] = 0;
TanakaTarou 15:ae2043fde152 408 }
TanakaTarou 15:ae2043fde152 409 a.shoot = state_tops[tops_num][0];
TanakaTarou 15:ae2043fde152 410 a.angle = state_tops[tops_num][1];
TanakaTarou 15:ae2043fde152 411 a.supply = state_tops[tops_num][2];
TanakaTarou 15:ae2043fde152 412
TanakaTarou 15:ae2043fde152 413 return a;
TanakaTarou 15:ae2043fde152 414 }
soyooo 9:ce5a1315fe0d 415 elements getRobotVelocity(state a)
soyooo 9:ce5a1315fe0d 416 {
soyooo 9:ce5a1315fe0d 417 elements vel;
soyooo 9:ce5a1315fe0d 418
soyooo 9:ce5a1315fe0d 419 //yow角調整処理
soyooo 9:ce5a1315fe0d 420 *pidRobotYow.target = a.theta;
soyooo 9:ce5a1315fe0d 421 vel.theta = pidRobotYow.output;
soyooo 9:ce5a1315fe0d 422
TanakaTarou 12:91218718ae75 423 float right_dis = uss[0].distance;
TanakaTarou 12:91218718ae75 424 float left_dis = uss[1].distance;
TanakaTarou 12:91218718ae75 425 float error = right_dis - left_dis;
TanakaTarou 12:91218718ae75 426 if(right_dis < left_dis)
TanakaTarou 12:91218718ae75 427 *pidUss.sensor = right_dis;
TanakaTarou 12:91218718ae75 428 else *pidUss.sensor = left_dis;
soyooo 9:ce5a1315fe0d 429
TanakaTarou 12:91218718ae75 430 if(a.x >= 10 || a.x <= -10 || a.y >= 10 || a.y <= -10)
soyooo 9:ce5a1315fe0d 431 {
TanakaTarou 12:91218718ae75 432 if(a.x >= 10)
TanakaTarou 12:91218718ae75 433 {
TanakaTarou 12:91218718ae75 434 if(error > 100)
TanakaTarou 12:91218718ae75 435 vel.y = 0.2;
TanakaTarou 12:91218718ae75 436 else if(error < -100)
TanakaTarou 12:91218718ae75 437 vel.y = -0.2;
TanakaTarou 12:91218718ae75 438 else vel.y = 0;
TanakaTarou 12:91218718ae75 439
TanakaTarou 12:91218718ae75 440 *pidUss.target = a.x - 10;
TanakaTarou 12:91218718ae75 441 vel.x = -pidUss.output;
TanakaTarou 12:91218718ae75 442 }
TanakaTarou 12:91218718ae75 443 else if(a.x <= -10)
TanakaTarou 12:91218718ae75 444 {
TanakaTarou 12:91218718ae75 445 if(error > 100)
TanakaTarou 12:91218718ae75 446 vel.y = -0.2;
TanakaTarou 12:91218718ae75 447 else if(error < -100)
TanakaTarou 12:91218718ae75 448 vel.y = 0.2;
TanakaTarou 12:91218718ae75 449 else vel.y = 0;
TanakaTarou 12:91218718ae75 450
TanakaTarou 12:91218718ae75 451 *pidUss.target = -a.x - 10;
TanakaTarou 12:91218718ae75 452 vel.x = pidUss.output;
TanakaTarou 12:91218718ae75 453 }
TanakaTarou 12:91218718ae75 454 else if(a.y >= 10)
TanakaTarou 12:91218718ae75 455 {
TanakaTarou 12:91218718ae75 456 if(error > 100)
TanakaTarou 12:91218718ae75 457 vel.x = -0.2;
TanakaTarou 12:91218718ae75 458 else if(error < -100)
TanakaTarou 12:91218718ae75 459 vel.x = 0.2;
TanakaTarou 12:91218718ae75 460 else vel.x = 0;
TanakaTarou 12:91218718ae75 461
TanakaTarou 12:91218718ae75 462 *pidUss.target = a.y - 10;
TanakaTarou 12:91218718ae75 463 vel.y = -pidUss.output;
TanakaTarou 12:91218718ae75 464 }
TanakaTarou 12:91218718ae75 465 else if(a.y <= -10)
TanakaTarou 12:91218718ae75 466 {
TanakaTarou 12:91218718ae75 467 if(error > 100)
TanakaTarou 12:91218718ae75 468 vel.x = 0.2;
TanakaTarou 12:91218718ae75 469 else if(error < -100)
TanakaTarou 12:91218718ae75 470 vel.x = -0.2;
TanakaTarou 12:91218718ae75 471 else vel.x = 0;
TanakaTarou 12:91218718ae75 472
TanakaTarou 12:91218718ae75 473 *pidUss.target = -a.y - 10;
TanakaTarou 12:91218718ae75 474 vel.y = pidUss.output;
TanakaTarou 12:91218718ae75 475 }
soyooo 9:ce5a1315fe0d 476 }
soyooo 9:ce5a1315fe0d 477 else
soyooo 9:ce5a1315fe0d 478 {
TanakaTarou 12:91218718ae75 479 *pidUss.target = 0;
soyooo 9:ce5a1315fe0d 480 *pidRobotX.target = a.x;
soyooo 9:ce5a1315fe0d 481 *pidRobotY.target = a.y;
TanakaTarou 12:91218718ae75 482 vel.x = pidRobotX.output;
soyooo 9:ce5a1315fe0d 483 vel.y = pidRobotY.output;
soyooo 9:ce5a1315fe0d 484 }
soyooo 9:ce5a1315fe0d 485 return vel;
soyooo 9:ce5a1315fe0d 486 }
TanakaTarou 15:ae2043fde152 487 bool isConvergenceTops(int tops_num)
TanakaTarou 0:6db16ad02a1b 488 {
TanakaTarou 4:9f74525eb37f 489 int velocity_pid = can.get(3, 1);
TanakaTarou 4:9f74525eb37f 490 int angle_pid = can.get(3, 2);
TanakaTarou 4:9f74525eb37f 491 int velocity_val = can.get(3, 3);
soyooo 9:ce5a1315fe0d 492 float angle_val = can.get(3, 4);
TanakaTarou 15:ae2043fde152 493 angle_val = 90 - (angle_val / 10.0);
TanakaTarou 17:1409cdfb6043 494
TanakaTarou 15:ae2043fde152 495 if(angle_pid == 1 && velocity_pid == 1 && velocity_val == state_tops[tops_num][0] && angle_val == state_tops[tops_num][1])
TanakaTarou 4:9f74525eb37f 496 return 1;
TanakaTarou 3:6b4adb4d7101 497 else return 0;
soyooo 9:ce5a1315fe0d 498 }
TanakaTarou 15:ae2043fde152 499 bool isConvergenceSupply(int num)
soyooo 10:ebb59c1d369e 500 {
soyooo 10:ebb59c1d369e 501 int is_supply_done = can.get(3, 5);
soyooo 10:ebb59c1d369e 502
TanakaTarou 15:ae2043fde152 503 if(state_tops[num][2] == 1)
soyooo 10:ebb59c1d369e 504 {
soyooo 10:ebb59c1d369e 505 if(is_supply_done == 1)
soyooo 10:ebb59c1d369e 506 return 1;
soyooo 10:ebb59c1d369e 507 else return 0;
soyooo 10:ebb59c1d369e 508 }else return 1;
soyooo 10:ebb59c1d369e 509 }
soyooo 9:ce5a1315fe0d 510 controller getPropoData()
soyooo 9:ce5a1315fe0d 511 {
soyooo 9:ce5a1315fe0d 512 float dead_zone = 0.05;
soyooo 9:ce5a1315fe0d 513 controller propo;
soyooo 9:ce5a1315fe0d 514 sbus.isFailSafe();
soyooo 9:ce5a1315fe0d 515
soyooo 9:ce5a1315fe0d 516 //propo直接コントロール
soyooo 9:ce5a1315fe0d 517 if(sbus.isFailSafe())
soyooo 9:ce5a1315fe0d 518 {
soyooo 9:ce5a1315fe0d 519 propo.LX = propo.LY = propo.RX = propo.RY = 0;
soyooo 9:ce5a1315fe0d 520 propo.H = propo.A = propo.D = propo.F = propo.G = 0;
soyooo 9:ce5a1315fe0d 521 }
soyooo 9:ce5a1315fe0d 522 else
soyooo 9:ce5a1315fe0d 523 {
soyooo 9:ce5a1315fe0d 524 propo.LX = sbus.getStickVal(0) / 255.0;
soyooo 9:ce5a1315fe0d 525 propo.LY = sbus.getStickVal(1) / 255.0;
soyooo 9:ce5a1315fe0d 526 propo.RX = -sbus.getStickVal(2) / 255.0;
soyooo 9:ce5a1315fe0d 527 propo.RY = sbus.getStickVal(3) / 255.0;
soyooo 9:ce5a1315fe0d 528 propo.H = sbus.getSwitchVal(0);
soyooo 10:ebb59c1d369e 529 propo.C = sbus.getSwitchVal(1);
soyooo 10:ebb59c1d369e 530 propo.E = sbus.getSwitchVal(2);
soyooo 9:ce5a1315fe0d 531 propo.F = sbus.getSwitchVal(3);
soyooo 9:ce5a1315fe0d 532 propo.G = sbus.getSwitchVal(4);
soyooo 9:ce5a1315fe0d 533 }
soyooo 9:ce5a1315fe0d 534
soyooo 9:ce5a1315fe0d 535 if(propo.RX < dead_zone && propo.RX > -dead_zone) propo.RX = 0;
soyooo 9:ce5a1315fe0d 536 if(propo.RY < dead_zone && propo.RY > -dead_zone) propo.RY = 0;
soyooo 9:ce5a1315fe0d 537 if(propo.LX < dead_zone && propo.LX > -dead_zone) propo.LX = 0;
soyooo 9:ce5a1315fe0d 538 if(propo.LY < dead_zone && propo.LY > -dead_zone) propo.LY = 0;
soyooo 9:ce5a1315fe0d 539 return propo;
TanakaTarou 12:91218718ae75 540 }
TanakaTarou 12:91218718ae75 541 void riseUssTriger()
TanakaTarou 12:91218718ae75 542 {
TanakaTarou 12:91218718ae75 543 static double start_time = timer.read();
TanakaTarou 12:91218718ae75 544 static bool flag = 1;
TanakaTarou 12:91218718ae75 545 double now_time = timer.read();
TanakaTarou 15:ae2043fde152 546 if(now_time - start_time > 0.025)
TanakaTarou 12:91218718ae75 547 {
TanakaTarou 12:91218718ae75 548 if(flag)
TanakaTarou 12:91218718ae75 549 {
TanakaTarou 12:91218718ae75 550 uss[0].riseTrigerEvent();
TanakaTarou 12:91218718ae75 551 flag = 0;
TanakaTarou 12:91218718ae75 552 }
TanakaTarou 12:91218718ae75 553 else
TanakaTarou 12:91218718ae75 554 {
TanakaTarou 12:91218718ae75 555 uss[1].riseTrigerEvent();
TanakaTarou 12:91218718ae75 556 flag = 1;
TanakaTarou 12:91218718ae75 557 }
TanakaTarou 12:91218718ae75 558 start_time = now_time;
TanakaTarou 12:91218718ae75 559 }
soyooo 10:ebb59c1d369e 560 }