ichinoseki_Bteam_2019
/
2019_AR_Itsuki
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: IMU mbed Odometer PID MDD RotaryEncoder UART Mycan DriveConroller
main.cpp@0:56a2c0ed99c5, 2019-10-21 (annotated)
- Committer:
- TanakaTarou
- Date:
- Mon Oct 21 14:25:38 2019 +0000
- Revision:
- 0:56a2c0ed99c5
- Child:
- 1:0f33a68d1390
2019_Itsuki
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| TanakaTarou | 0:56a2c0ed99c5 | 1 | #include "mbed.h" |
| TanakaTarou | 0:56a2c0ed99c5 | 2 | #include "stateLib.h" |
| TanakaTarou | 0:56a2c0ed99c5 | 3 | #include "hardwareConfig.h" |
| TanakaTarou | 0:56a2c0ed99c5 | 4 | |
| TanakaTarou | 0:56a2c0ed99c5 | 5 | void mainLoop(); |
| TanakaTarou | 0:56a2c0ed99c5 | 6 | controller getPropoData(); |
| TanakaTarou | 0:56a2c0ed99c5 | 7 | void canFnc(); |
| TanakaTarou | 0:56a2c0ed99c5 | 8 | void readLineData(); |
| TanakaTarou | 0:56a2c0ed99c5 | 9 | void readSwitchData(); |
| TanakaTarou | 0:56a2c0ed99c5 | 10 | void encCorrection(); |
| TanakaTarou | 0:56a2c0ed99c5 | 11 | state getTargetState(); |
| TanakaTarou | 0:56a2c0ed99c5 | 12 | int updateStateNum(int, int); |
| TanakaTarou | 0:56a2c0ed99c5 | 13 | int updateTopsNum(int, int); |
| TanakaTarou | 0:56a2c0ed99c5 | 14 | void updateSupplyTarget(int, int); |
| TanakaTarou | 0:56a2c0ed99c5 | 15 | int updateRouteNum(int); |
| TanakaTarou | 0:56a2c0ed99c5 | 16 | void imuCalibration(); |
| TanakaTarou | 0:56a2c0ed99c5 | 17 | void reset_System(); |
| TanakaTarou | 0:56a2c0ed99c5 | 18 | int changeNextNumber(int); |
| TanakaTarou | 0:56a2c0ed99c5 | 19 | elements getRobotVelocity(state); |
| TanakaTarou | 0:56a2c0ed99c5 | 20 | int safety, now_state, now_tops, now_spline_num, now_route_num, read_sw_data[2]; |
| TanakaTarou | 0:56a2c0ed99c5 | 21 | float robot_velocity[3], lift_val, wide_val, supply_val, read_line_data[2], line_error; |
| TanakaTarou | 0:56a2c0ed99c5 | 22 | bool S_sw, B_sw[4], T_sw[4], zone, match, reset_sw = false, do_roop = false; |
| TanakaTarou | 0:56a2c0ed99c5 | 23 | |
| TanakaTarou | 0:56a2c0ed99c5 | 24 | int main() |
| TanakaTarou | 0:56a2c0ed99c5 | 25 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 26 | for(int i=0; i<4; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 27 | sw[i].mode(PullUp); |
| TanakaTarou | 0:56a2c0ed99c5 | 28 | |
| TanakaTarou | 0:56a2c0ed99c5 | 29 | //IMUのキャリブレーション |
| TanakaTarou | 0:56a2c0ed99c5 | 30 | imuCalibration(); |
| TanakaTarou | 0:56a2c0ed99c5 | 31 | |
| TanakaTarou | 0:56a2c0ed99c5 | 32 | float tmp = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 33 | float *encoders[3] = {&odm_enc[0].rotations, &odm_enc[1].rotations, &tmp}; |
| TanakaTarou | 0:56a2c0ed99c5 | 34 | odm.setupOdometerSensors(encoders, &imu.angle[2]); |
| TanakaTarou | 0:56a2c0ed99c5 | 35 | odm.startComputingOdometry(0.01, 0, 0, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 36 | omni.imu_yaw = &imu.angle[2]; |
| TanakaTarou | 0:56a2c0ed99c5 | 37 | |
| TanakaTarou | 0:56a2c0ed99c5 | 38 | for(int i = 0; i < SPLINE_NUM; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 39 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 40 | for(int j = 0; j < ROUTE_NUM; j++) { |
| TanakaTarou | 0:56a2c0ed99c5 | 41 | spline[i][j].odm[0] = &odm.x; |
| TanakaTarou | 0:56a2c0ed99c5 | 42 | spline[i][j].odm[1] = &odm.y; |
| TanakaTarou | 0:56a2c0ed99c5 | 43 | spline[i][j].start(); |
| TanakaTarou | 0:56a2c0ed99c5 | 44 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 45 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 46 | |
| TanakaTarou | 0:56a2c0ed99c5 | 47 | //許容誤差 |
| TanakaTarou | 0:56a2c0ed99c5 | 48 | pidRobotX.allowable_error = 0.08; //[m] |
| TanakaTarou | 0:56a2c0ed99c5 | 49 | pidRobotY.allowable_error = 0.08; //[m] |
| TanakaTarou | 0:56a2c0ed99c5 | 50 | pidRobotYaw.allowable_error = 4; //[°] |
| TanakaTarou | 0:56a2c0ed99c5 | 51 | pidLift.allowable_error = 0.05; //×0.3180[m] = 15.9[mm] |
| TanakaTarou | 0:56a2c0ed99c5 | 52 | pidWide.allowable_error = 0.075; //×0.1000[m] = 7.5[mm] |
| TanakaTarou | 0:56a2c0ed99c5 | 53 | pidSupply.allowable_error = 0.075; //×0.0625[m] = 4.7[mm] |
| TanakaTarou | 0:56a2c0ed99c5 | 54 | |
| TanakaTarou | 0:56a2c0ed99c5 | 55 | //PID設定 |
| TanakaTarou | 0:56a2c0ed99c5 | 56 | pidRobotX.sensor = &odm.x; |
| TanakaTarou | 0:56a2c0ed99c5 | 57 | pidRobotX.target = &target_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 58 | pidRobotX.start(); |
| TanakaTarou | 0:56a2c0ed99c5 | 59 | pidRobotY.sensor = &odm.y; |
| TanakaTarou | 0:56a2c0ed99c5 | 60 | pidRobotY.target = &target_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 61 | pidRobotY.start(); |
| TanakaTarou | 0:56a2c0ed99c5 | 62 | pidRobotYaw.sensor = &imu.angle[2]; |
| TanakaTarou | 0:56a2c0ed99c5 | 63 | pidRobotYaw.target = &target_yaw; |
| TanakaTarou | 0:56a2c0ed99c5 | 64 | pidRobotYaw.start(); |
| TanakaTarou | 0:56a2c0ed99c5 | 65 | pidLift.sensor = &enc[4]; |
| TanakaTarou | 0:56a2c0ed99c5 | 66 | pidLift.target = &target_lift; |
| TanakaTarou | 0:56a2c0ed99c5 | 67 | pidLift.start(); |
| TanakaTarou | 0:56a2c0ed99c5 | 68 | pidWide.sensor = &enc[5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 69 | pidWide.target = &target_wide; |
| TanakaTarou | 0:56a2c0ed99c5 | 70 | pidWide.start(); |
| TanakaTarou | 0:56a2c0ed99c5 | 71 | pidSupply.sensor = &enc[6]; |
| TanakaTarou | 0:56a2c0ed99c5 | 72 | pidSupply.target = &target_supply; |
| TanakaTarou | 0:56a2c0ed99c5 | 73 | pidSupply.start(); |
| TanakaTarou | 0:56a2c0ed99c5 | 74 | |
| TanakaTarou | 0:56a2c0ed99c5 | 75 | ticker.attach(&mainLoop, 0.00725); |
| TanakaTarou | 0:56a2c0ed99c5 | 76 | |
| TanakaTarou | 0:56a2c0ed99c5 | 77 | while(1) |
| TanakaTarou | 0:56a2c0ed99c5 | 78 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 79 | pc.printf("%.3f\t", odm.x); |
| TanakaTarou | 0:56a2c0ed99c5 | 80 | pc.printf("%.3f\t", odm.y); |
| TanakaTarou | 0:56a2c0ed99c5 | 81 | pc.printf("%.3f\t", imu.angle[2]); |
| TanakaTarou | 0:56a2c0ed99c5 | 82 | |
| TanakaTarou | 0:56a2c0ed99c5 | 83 | pc.printf("%.3f\t", lift_val); |
| TanakaTarou | 0:56a2c0ed99c5 | 84 | pc.printf("%.3f\t", enc[4]); |
| TanakaTarou | 0:56a2c0ed99c5 | 85 | pc.printf("%.3f\t", wide_val); |
| TanakaTarou | 0:56a2c0ed99c5 | 86 | pc.printf("%.3f\t", enc[5]); |
| TanakaTarou | 0:56a2c0ed99c5 | 87 | pc.printf("%.3f\t", supply_val); |
| TanakaTarou | 0:56a2c0ed99c5 | 88 | pc.printf("%.3f\t", enc[6]); |
| TanakaTarou | 0:56a2c0ed99c5 | 89 | |
| TanakaTarou | 0:56a2c0ed99c5 | 90 | bool _sw[4]; |
| TanakaTarou | 0:56a2c0ed99c5 | 91 | for(int i = 0; i < 4; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 92 | _sw[i] = sw[i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 93 | pc.printf("%d\t", (int)(_sw[0] + _sw[1] + _sw[2] + _sw[3])); |
| TanakaTarou | 0:56a2c0ed99c5 | 94 | |
| TanakaTarou | 0:56a2c0ed99c5 | 95 | pc.printf("%d\t", read_sw_data[0]); |
| TanakaTarou | 0:56a2c0ed99c5 | 96 | pc.printf("%d\t", read_sw_data[1]); |
| TanakaTarou | 0:56a2c0ed99c5 | 97 | pc.printf("%.3f\t", read_line_data[0]); |
| TanakaTarou | 0:56a2c0ed99c5 | 98 | pc.printf("%.3f\t", read_line_data[1]); |
| TanakaTarou | 0:56a2c0ed99c5 | 99 | pc.printf("%.3f\t", robot_velocity[0]); |
| TanakaTarou | 0:56a2c0ed99c5 | 100 | pc.printf("%.3f\t", robot_velocity[1]); |
| TanakaTarou | 0:56a2c0ed99c5 | 101 | //pc.printf("%.3f\t", robot_velocity[2]); |
| TanakaTarou | 0:56a2c0ed99c5 | 102 | pc.printf("%.3f\t", *pidRobotX.target); |
| TanakaTarou | 0:56a2c0ed99c5 | 103 | pc.printf("%.3f\t", *pidRobotY.target); |
| TanakaTarou | 0:56a2c0ed99c5 | 104 | //pc.printf("%.3f\t", *pidLift.target); |
| TanakaTarou | 0:56a2c0ed99c5 | 105 | //pc.printf("%.3f\t", pidLift.output); |
| TanakaTarou | 0:56a2c0ed99c5 | 106 | pc.printf("%d\t", now_state); |
| TanakaTarou | 0:56a2c0ed99c5 | 107 | pc.printf("%d\t", now_tops); |
| TanakaTarou | 0:56a2c0ed99c5 | 108 | |
| TanakaTarou | 0:56a2c0ed99c5 | 109 | pc.printf("\n"); |
| TanakaTarou | 0:56a2c0ed99c5 | 110 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 111 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 112 | |
| TanakaTarou | 0:56a2c0ed99c5 | 113 | void mainLoop() |
| TanakaTarou | 0:56a2c0ed99c5 | 114 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 115 | reset_System(); |
| TanakaTarou | 0:56a2c0ed99c5 | 116 | canFnc(); |
| TanakaTarou | 0:56a2c0ed99c5 | 117 | controller cmd = getPropoData(); |
| TanakaTarou | 0:56a2c0ed99c5 | 118 | |
| TanakaTarou | 0:56a2c0ed99c5 | 119 | if(cmd.fail_safe == false) |
| TanakaTarou | 0:56a2c0ed99c5 | 120 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 121 | safety = 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 122 | state tar_state = getTargetState(); |
| TanakaTarou | 0:56a2c0ed99c5 | 123 | |
| TanakaTarou | 0:56a2c0ed99c5 | 124 | elements robot_vel = getRobotVelocity(tar_state); |
| TanakaTarou | 0:56a2c0ed99c5 | 125 | robot_velocity[0] = robot_vel.x; |
| TanakaTarou | 0:56a2c0ed99c5 | 126 | robot_velocity[1] = robot_vel.y; |
| TanakaTarou | 0:56a2c0ed99c5 | 127 | robot_velocity[2] = robot_vel.yaw; |
| TanakaTarou | 0:56a2c0ed99c5 | 128 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 129 | else |
| TanakaTarou | 0:56a2c0ed99c5 | 130 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 131 | robot_velocity[0] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 132 | robot_velocity[1] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 133 | robot_velocity[2] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 134 | if(cmd.A == 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 135 | *pidLift.target = 5.13; |
| TanakaTarou | 0:56a2c0ed99c5 | 136 | else *pidLift.target = 0.095; |
| TanakaTarou | 0:56a2c0ed99c5 | 137 | *pidWide.target = 0.00; |
| TanakaTarou | 0:56a2c0ed99c5 | 138 | *pidSupply.target = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 139 | //lift_val = cmd.RY; |
| TanakaTarou | 0:56a2c0ed99c5 | 140 | //wide_val = cmd.RY; |
| TanakaTarou | 0:56a2c0ed99c5 | 141 | //supply_val = cmd.RY; |
| TanakaTarou | 0:56a2c0ed99c5 | 142 | safety = cmd.F; |
| TanakaTarou | 0:56a2c0ed99c5 | 143 | if(cmd.H == 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 144 | servo_mode = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 145 | else servo_mode = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 146 | |
| TanakaTarou | 0:56a2c0ed99c5 | 147 | if(cmd.D == 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 148 | servo_right = servo_left = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 149 | else servo_right = servo_left = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 150 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 151 | |
| TanakaTarou | 0:56a2c0ed99c5 | 152 | //ホイール速度計算 |
| TanakaTarou | 0:56a2c0ed99c5 | 153 | omni.setVelG(robot_velocity); |
| TanakaTarou | 0:56a2c0ed99c5 | 154 | omni.computeWheelVel(); |
| TanakaTarou | 0:56a2c0ed99c5 | 155 | omni.rescaleWheelVel(); |
| TanakaTarou | 0:56a2c0ed99c5 | 156 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 157 | |
| TanakaTarou | 0:56a2c0ed99c5 | 158 | controller getPropoData() |
| TanakaTarou | 0:56a2c0ed99c5 | 159 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 160 | float dead_zone = 0.05; |
| TanakaTarou | 0:56a2c0ed99c5 | 161 | controller propo; |
| TanakaTarou | 0:56a2c0ed99c5 | 162 | sbus.isFailSafe(); |
| TanakaTarou | 0:56a2c0ed99c5 | 163 | |
| TanakaTarou | 0:56a2c0ed99c5 | 164 | //propo電源off |
| TanakaTarou | 0:56a2c0ed99c5 | 165 | if(sbus.isFailSafe()) |
| TanakaTarou | 0:56a2c0ed99c5 | 166 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 167 | propo.LX = propo.LY = propo.RX = propo.RY = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 168 | propo.A = propo.D = propo.F = propo.H = propo.fail_safe = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 169 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 170 | else |
| TanakaTarou | 0:56a2c0ed99c5 | 171 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 172 | propo.LX = sbus.getStickVal(0) / 255.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 173 | propo.LY = sbus.getStickVal(1) / 255.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 174 | propo.RX = sbus.getStickVal(2) / 255.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 175 | propo.RY = sbus.getStickVal(3) / 255.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 176 | propo.A = sbus.getSwitchVal(0); |
| TanakaTarou | 0:56a2c0ed99c5 | 177 | propo.D = sbus.getSwitchVal(1); |
| TanakaTarou | 0:56a2c0ed99c5 | 178 | propo.F = sbus.getSwitchVal(2); |
| TanakaTarou | 0:56a2c0ed99c5 | 179 | propo.H = sbus.getSwitchVal(3); |
| TanakaTarou | 0:56a2c0ed99c5 | 180 | propo.fail_safe = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 181 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 182 | |
| TanakaTarou | 0:56a2c0ed99c5 | 183 | if(propo.RX < dead_zone && propo.RX > -dead_zone) propo.RX = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 184 | if(propo.RY < dead_zone && propo.RY > -dead_zone) propo.RY = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 185 | if(propo.LX < dead_zone && propo.LX > -dead_zone) propo.LX = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 186 | if(propo.LY < dead_zone && propo.LY > -dead_zone) propo.LY = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 187 | return propo; |
| TanakaTarou | 0:56a2c0ed99c5 | 188 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 189 | |
| TanakaTarou | 0:56a2c0ed99c5 | 190 | void canFnc() |
| TanakaTarou | 0:56a2c0ed99c5 | 191 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 192 | readSwitchData(); |
| TanakaTarou | 0:56a2c0ed99c5 | 193 | |
| TanakaTarou | 0:56a2c0ed99c5 | 194 | can.readF(); |
| TanakaTarou | 0:56a2c0ed99c5 | 195 | enc[4] = -can.get(4, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 196 | enc[5] = -can.get(4, 1); |
| TanakaTarou | 0:56a2c0ed99c5 | 197 | can.readF(); |
| TanakaTarou | 0:56a2c0ed99c5 | 198 | enc[6] = can.get(5, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 199 | enc[7] = can.get(5, 1); |
| TanakaTarou | 0:56a2c0ed99c5 | 200 | |
| TanakaTarou | 0:56a2c0ed99c5 | 201 | lift_val = pidLift.output; |
| TanakaTarou | 0:56a2c0ed99c5 | 202 | wide_val = pidWide.output; |
| TanakaTarou | 0:56a2c0ed99c5 | 203 | supply_val = pidSupply.output; |
| TanakaTarou | 0:56a2c0ed99c5 | 204 | |
| TanakaTarou | 0:56a2c0ed99c5 | 205 | static short int calibration[3] = {0, 0, 0}; |
| TanakaTarou | 0:56a2c0ed99c5 | 206 | float calibrate_output_a[3] = {-0.75, -0.45, 0.85}; //キャリブレーション, ON前出力値 |
| TanakaTarou | 0:56a2c0ed99c5 | 207 | float calibrate_output_b[3] = {0.13, 0.2, -0.2}; //キャリブレーション, ON後出力値 |
| TanakaTarou | 0:56a2c0ed99c5 | 208 | bool _sw[3] = {sw[1], sw[2], sw[3]}; |
| TanakaTarou | 0:56a2c0ed99c5 | 209 | |
| TanakaTarou | 0:56a2c0ed99c5 | 210 | if(calibration[0] != 2 || calibration[2] != 2) { |
| TanakaTarou | 0:56a2c0ed99c5 | 211 | servo_mode = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 212 | for(int i = 0; i < 4; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 213 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 214 | if(!_sw[i] && calibration[i] == 0) |
| TanakaTarou | 0:56a2c0ed99c5 | 215 | can.setI(1, i+4, (short int)(calibrate_output_a[i] * 200)); |
| TanakaTarou | 0:56a2c0ed99c5 | 216 | |
| TanakaTarou | 0:56a2c0ed99c5 | 217 | else if(_sw[i] && calibration[i] == 0) { |
| TanakaTarou | 0:56a2c0ed99c5 | 218 | can.setI(1, i+4, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 219 | calibration[i] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 220 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 221 | |
| TanakaTarou | 0:56a2c0ed99c5 | 222 | else if(_sw[i] && calibration[i] == 1) |
| TanakaTarou | 0:56a2c0ed99c5 | 223 | can.setI(1, i+4, (short int)(calibrate_output_b[i] * 200)); |
| TanakaTarou | 0:56a2c0ed99c5 | 224 | |
| TanakaTarou | 0:56a2c0ed99c5 | 225 | else if(!_sw[i] && calibration[i] == 1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 226 | can.setI(1, i+4, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 227 | calibration[i] = 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 228 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 229 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 230 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 231 | if(safety == 2) { |
| TanakaTarou | 0:56a2c0ed99c5 | 232 | for(int i = 0; i < 4; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 233 | can.setI(1, i, (short int)(omni.wheel_vel[i] * 200)); |
| TanakaTarou | 0:56a2c0ed99c5 | 234 | can.setI(1, 4, (short int)(lift_val * 200)); |
| TanakaTarou | 0:56a2c0ed99c5 | 235 | //can.setI(1, 5, (short int)(wide_val * 200)); |
| TanakaTarou | 0:56a2c0ed99c5 | 236 | can.setI(1, 5, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 237 | can.setI(1, 6, (short int)(supply_val * 200 * -1)); |
| TanakaTarou | 0:56a2c0ed99c5 | 238 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 239 | for(int i = 0; i < 7; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 240 | can.setI(1, i, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 241 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 242 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 243 | LED[0] = can.send(); |
| TanakaTarou | 0:56a2c0ed99c5 | 244 | |
| TanakaTarou | 0:56a2c0ed99c5 | 245 | encCorrection(); //スイッチを使ってenc値補正 |
| TanakaTarou | 0:56a2c0ed99c5 | 246 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 247 | |
| TanakaTarou | 0:56a2c0ed99c5 | 248 | void readSwitchData() |
| TanakaTarou | 0:56a2c0ed99c5 | 249 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 250 | int residue_data; |
| TanakaTarou | 0:56a2c0ed99c5 | 251 | |
| TanakaTarou | 0:56a2c0ed99c5 | 252 | can.readI(); |
| TanakaTarou | 0:56a2c0ed99c5 | 253 | read_sw_data[0] = can.get(2, 0); |
| TanakaTarou | 0:56a2c0ed99c5 | 254 | read_sw_data[1] = can.get(2, 1); |
| TanakaTarou | 0:56a2c0ed99c5 | 255 | read_line_data[1] = 0.5 * can.get(2, 2); |
| TanakaTarou | 0:56a2c0ed99c5 | 256 | read_line_data[0] = 0.5 * can.get(2, 3); |
| TanakaTarou | 0:56a2c0ed99c5 | 257 | |
| TanakaTarou | 0:56a2c0ed99c5 | 258 | residue_data = read_sw_data[0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 259 | |
| TanakaTarou | 0:56a2c0ed99c5 | 260 | if(residue_data >= 64) { |
| TanakaTarou | 0:56a2c0ed99c5 | 261 | residue_data -= 64; |
| TanakaTarou | 0:56a2c0ed99c5 | 262 | S_sw = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 263 | } else S_sw = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 264 | |
| TanakaTarou | 0:56a2c0ed99c5 | 265 | if(residue_data >= 32) { |
| TanakaTarou | 0:56a2c0ed99c5 | 266 | residue_data -= 32; |
| TanakaTarou | 0:56a2c0ed99c5 | 267 | B_sw[0] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 268 | } else B_sw[0] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 269 | |
| TanakaTarou | 0:56a2c0ed99c5 | 270 | if(residue_data >= 16) { |
| TanakaTarou | 0:56a2c0ed99c5 | 271 | residue_data -= 16; |
| TanakaTarou | 0:56a2c0ed99c5 | 272 | B_sw[1] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 273 | } else B_sw[1] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 274 | |
| TanakaTarou | 0:56a2c0ed99c5 | 275 | if(residue_data >= 8) { |
| TanakaTarou | 0:56a2c0ed99c5 | 276 | residue_data -= 8; |
| TanakaTarou | 0:56a2c0ed99c5 | 277 | B_sw[2] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 278 | } else B_sw[2] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 279 | |
| TanakaTarou | 0:56a2c0ed99c5 | 280 | if(residue_data >= 4) { |
| TanakaTarou | 0:56a2c0ed99c5 | 281 | residue_data -= 4; |
| TanakaTarou | 0:56a2c0ed99c5 | 282 | B_sw[3] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 283 | } else B_sw[3] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 284 | |
| TanakaTarou | 0:56a2c0ed99c5 | 285 | if(residue_data >= 2) { |
| TanakaTarou | 0:56a2c0ed99c5 | 286 | residue_data -= 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 287 | T_sw[0] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 288 | } else T_sw[0] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 289 | |
| TanakaTarou | 0:56a2c0ed99c5 | 290 | if(residue_data >= 1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 291 | residue_data -= 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 292 | T_sw[1] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 293 | } else T_sw[1] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 294 | |
| TanakaTarou | 0:56a2c0ed99c5 | 295 | residue_data = read_sw_data[1]; |
| TanakaTarou | 0:56a2c0ed99c5 | 296 | |
| TanakaTarou | 0:56a2c0ed99c5 | 297 | if(residue_data >= 8) { |
| TanakaTarou | 0:56a2c0ed99c5 | 298 | residue_data -= 8; |
| TanakaTarou | 0:56a2c0ed99c5 | 299 | T_sw[2] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 300 | } else T_sw[2] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 301 | |
| TanakaTarou | 0:56a2c0ed99c5 | 302 | if(residue_data >= 4) { |
| TanakaTarou | 0:56a2c0ed99c5 | 303 | residue_data -= 4; |
| TanakaTarou | 0:56a2c0ed99c5 | 304 | T_sw[3] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 305 | } else T_sw[3] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 306 | |
| TanakaTarou | 0:56a2c0ed99c5 | 307 | if(residue_data >= 2) { |
| TanakaTarou | 0:56a2c0ed99c5 | 308 | residue_data -= 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 309 | zone = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 310 | } else zone = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 311 | |
| TanakaTarou | 0:56a2c0ed99c5 | 312 | if(residue_data >= 1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 313 | residue_data -= 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 314 | match = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 315 | } else match = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 316 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 317 | |
| TanakaTarou | 0:56a2c0ed99c5 | 318 | void encCorrection() //swを使ってencの値を補正 |
| TanakaTarou | 0:56a2c0ed99c5 | 319 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 320 | bool _sw[3] = {sw[1], sw[2], sw[3]}; |
| TanakaTarou | 0:56a2c0ed99c5 | 321 | static float a_val[3]; |
| TanakaTarou | 0:56a2c0ed99c5 | 322 | float b_val[3] = {-0.040, -0.030, -0.035}; |
| TanakaTarou | 0:56a2c0ed99c5 | 323 | for(int i=0; i<3; i++) { |
| TanakaTarou | 0:56a2c0ed99c5 | 324 | if(_sw[i]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 325 | a_val[i] = enc[i+4] - b_val[i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 326 | enc[i+4] = b_val[i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 327 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 328 | else if(!_sw[i]) |
| TanakaTarou | 0:56a2c0ed99c5 | 329 | enc[i+4] -= a_val[i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 330 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 331 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 332 | |
| TanakaTarou | 0:56a2c0ed99c5 | 333 | state getTargetState() |
| TanakaTarou | 0:56a2c0ed99c5 | 334 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 335 | static bool start_flag = false; |
| TanakaTarou | 0:56a2c0ed99c5 | 336 | int state_num; |
| TanakaTarou | 0:56a2c0ed99c5 | 337 | int tops_num; |
| TanakaTarou | 0:56a2c0ed99c5 | 338 | int route_num; |
| TanakaTarou | 0:56a2c0ed99c5 | 339 | short int use_spline; |
| TanakaTarou | 0:56a2c0ed99c5 | 340 | static short int max_num, r_point, laundry, tops_state; |
| TanakaTarou | 0:56a2c0ed99c5 | 341 | float error_x, error_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 342 | //緊急座標調整の設定 |
| TanakaTarou | 0:56a2c0ed99c5 | 343 | if(zone) { |
| TanakaTarou | 0:56a2c0ed99c5 | 344 | error_x = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 345 | error_y = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 346 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 347 | error_x = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 348 | error_y = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 349 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 350 | |
| TanakaTarou | 0:56a2c0ed99c5 | 351 | bool start_sw = sw[0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 352 | |
| TanakaTarou | 0:56a2c0ed99c5 | 353 | if(start_sw && !do_roop) |
| TanakaTarou | 0:56a2c0ed99c5 | 354 | start_flag = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 355 | |
| TanakaTarou | 0:56a2c0ed99c5 | 356 | if(!start_flag) { |
| TanakaTarou | 0:56a2c0ed99c5 | 357 | state_num = now_state = tops_num = now_tops = route_num = now_route_num = odm.x = odm.y = imu.angle[2] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 358 | |
| TanakaTarou | 0:56a2c0ed99c5 | 359 | for(int i = 0; i < SPLINE_NUM; i++) { |
| TanakaTarou | 0:56a2c0ed99c5 | 360 | for(int j = 0; j < ROUTE_NUM; j++) |
| TanakaTarou | 0:56a2c0ed99c5 | 361 | spline[i][j].now_count = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 362 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 363 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 364 | else { |
| TanakaTarou | 0:56a2c0ed99c5 | 365 | tops_num = now_tops = updateTopsNum(laundry, tops_state); |
| TanakaTarou | 0:56a2c0ed99c5 | 366 | state_num = now_state = updateStateNum(max_num, r_point); |
| TanakaTarou | 0:56a2c0ed99c5 | 367 | do_roop = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 368 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 369 | |
| TanakaTarou | 0:56a2c0ed99c5 | 370 | if(state_num == 0) { |
| TanakaTarou | 0:56a2c0ed99c5 | 371 | start_flag = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 372 | //IMUキャリブレーションのタイミング |
| TanakaTarou | 0:56a2c0ed99c5 | 373 | if(do_roop) |
| TanakaTarou | 0:56a2c0ed99c5 | 374 | reset_sw = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 375 | else reset_sw = false; |
| TanakaTarou | 0:56a2c0ed99c5 | 376 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 377 | |
| TanakaTarou | 0:56a2c0ed99c5 | 378 | state tar; |
| TanakaTarou | 0:56a2c0ed99c5 | 379 | |
| TanakaTarou | 0:56a2c0ed99c5 | 380 | bool tops_sw = false; //スイッチングで動作確認をするための変数, ONで動作確認, OFFで通常動作 |
| TanakaTarou | 0:56a2c0ed99c5 | 381 | if(S_sw) { |
| TanakaTarou | 0:56a2c0ed99c5 | 382 | if((B_sw[0] && T_sw[0]) || (B_sw[1] && T_sw[1]) || (B_sw[2] && T_sw[2]) || (B_sw[3] && T_sw[3])) { |
| TanakaTarou | 0:56a2c0ed99c5 | 383 | if(B_sw[0] && T_sw[0]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 384 | for(int i=0; i<6; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 385 | state_lib[i] = Test_lib[state_num][i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 386 | max_num = TEST_NUM; |
| TanakaTarou | 0:56a2c0ed99c5 | 387 | r_point = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 388 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 389 | else if(B_sw[1] && T_sw[1]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 390 | state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 391 | state_lib[3] = -1; |
| TanakaTarou | 0:56a2c0ed99c5 | 392 | state_lib[5] = 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 393 | if(match) |
| TanakaTarou | 0:56a2c0ed99c5 | 394 | tar.lift = 4.20; |
| TanakaTarou | 0:56a2c0ed99c5 | 395 | else tar.lift = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 396 | tar.wide = tar.supply = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 397 | tops_sw = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 398 | max_num = r_point = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 399 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 400 | else if(B_sw[2] && T_sw[2]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 401 | state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 402 | state_lib[3] = -1; |
| TanakaTarou | 0:56a2c0ed99c5 | 403 | state_lib[5] = 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 404 | if(match) |
| TanakaTarou | 0:56a2c0ed99c5 | 405 | tar.wide = 2.35; |
| TanakaTarou | 0:56a2c0ed99c5 | 406 | else tar.wide = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 407 | tar.lift = tar.supply = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 408 | tops_sw = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 409 | max_num = r_point = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 410 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 411 | else if(B_sw[3] && T_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 412 | state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 413 | state_lib[3] = -1; |
| TanakaTarou | 0:56a2c0ed99c5 | 414 | state_lib[5] = 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 415 | if(match) |
| TanakaTarou | 0:56a2c0ed99c5 | 416 | tar.supply = 3.20; |
| TanakaTarou | 0:56a2c0ed99c5 | 417 | else tar.supply = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 418 | tar.lift = tar.wide = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 419 | tops_sw = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 420 | max_num = r_point = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 421 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 422 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 423 | if(!match) { |
| TanakaTarou | 0:56a2c0ed99c5 | 424 | for(int i=0; i<6; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 425 | state_lib[i] = SQ_lib[state_num][i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 426 | if(state_num > 2 && state_num < 6) { |
| TanakaTarou | 0:56a2c0ed99c5 | 427 | state_lib[0] += error_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 428 | state_lib[1] += error_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 429 | if(state_lib[1] == S_y_2) |
| TanakaTarou | 0:56a2c0ed99c5 | 430 | state_lib[1] += line_error; |
| TanakaTarou | 0:56a2c0ed99c5 | 431 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 432 | max_num = SQ_NUM; |
| TanakaTarou | 0:56a2c0ed99c5 | 433 | if(state_num < max_num - 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 434 | r_point = SQ_lib[state_num + 2][5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 435 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 436 | else { |
| TanakaTarou | 0:56a2c0ed99c5 | 437 | for(int i=0; i<6; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 438 | state_lib[i] = SF_lib[state_num][i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 439 | if(state_num > 2 && state_num < 18) { |
| TanakaTarou | 0:56a2c0ed99c5 | 440 | state_lib[0] += error_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 441 | state_lib[1] += error_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 442 | if(state_lib[1] == S_y_2 || state_lib[1] == S_y_3 || state_lib[1] == S_y_4) |
| TanakaTarou | 0:56a2c0ed99c5 | 443 | state_lib[1] += line_error; |
| TanakaTarou | 0:56a2c0ed99c5 | 444 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 445 | max_num = SF_NUM; |
| TanakaTarou | 0:56a2c0ed99c5 | 446 | if(state_num < max_num - 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 447 | r_point = SF_lib[state_num + 2][5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 448 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 449 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 450 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 451 | else if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3] || T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 452 | if(!match) { |
| TanakaTarou | 0:56a2c0ed99c5 | 453 | if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 454 | for(int i=0; i<6; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 455 | state_lib[i] = BQ_lib[state_num][i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 456 | if(state_num > 2 && state_num < 16) { |
| TanakaTarou | 0:56a2c0ed99c5 | 457 | state_lib[0] += error_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 458 | state_lib[1] += error_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 459 | if(state_lib[1] == B_y_2) |
| TanakaTarou | 0:56a2c0ed99c5 | 460 | state_lib[1] += line_error; |
| TanakaTarou | 0:56a2c0ed99c5 | 461 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 462 | max_num = BQ_NUM; |
| TanakaTarou | 0:56a2c0ed99c5 | 463 | if(state_num < max_num - 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 464 | r_point = BQ_lib[state_num + 2][5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 465 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 466 | else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 467 | for(int i=0; i<6; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 468 | state_lib[i] = TQ_lib[state_num][i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 469 | if(state_num > 2 && state_num < 16) { |
| TanakaTarou | 0:56a2c0ed99c5 | 470 | state_lib[0] += error_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 471 | state_lib[1] += error_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 472 | if(state_lib[1] == T_y_2) |
| TanakaTarou | 0:56a2c0ed99c5 | 473 | state_lib[1] += line_error; |
| TanakaTarou | 0:56a2c0ed99c5 | 474 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 475 | max_num = TQ_NUM; |
| TanakaTarou | 0:56a2c0ed99c5 | 476 | if(state_num < max_num - 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 477 | r_point = TQ_lib[state_num + 2][5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 478 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 479 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 480 | else if(match) { |
| TanakaTarou | 0:56a2c0ed99c5 | 481 | if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 482 | for(int i=0; i<6; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 483 | state_lib[i] = BF_lib[state_num][i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 484 | if(state_num > 2 && state_num < 21) { |
| TanakaTarou | 0:56a2c0ed99c5 | 485 | state_lib[0] += error_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 486 | state_lib[1] += error_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 487 | if(state_lib[1] == B_y_2) |
| TanakaTarou | 0:56a2c0ed99c5 | 488 | state_lib[1] += line_error; |
| TanakaTarou | 0:56a2c0ed99c5 | 489 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 490 | max_num = BF_NUM; |
| TanakaTarou | 0:56a2c0ed99c5 | 491 | if(state_num < max_num - 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 492 | r_point = BF_lib[state_num + 2][5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 493 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 494 | else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 495 | for(int i=0; i<6; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 496 | state_lib[i] = TF_lib[state_num][i]; |
| TanakaTarou | 0:56a2c0ed99c5 | 497 | if(state_num > 2 && state_num < 21) { |
| TanakaTarou | 0:56a2c0ed99c5 | 498 | state_lib[0] += error_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 499 | state_lib[1] += error_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 500 | if(state_lib[1] == T_y_2) |
| TanakaTarou | 0:56a2c0ed99c5 | 501 | state_lib[1] += line_error; |
| TanakaTarou | 0:56a2c0ed99c5 | 502 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 503 | max_num = TF_NUM; |
| TanakaTarou | 0:56a2c0ed99c5 | 504 | if(state_num < max_num - 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 505 | r_point = TF_lib[state_num + 2][5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 506 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 507 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 508 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 509 | state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 510 | state_lib[3] = -1; |
| TanakaTarou | 0:56a2c0ed99c5 | 511 | state_lib[5] = 2; |
| TanakaTarou | 0:56a2c0ed99c5 | 512 | tar.lift = tar.wide = tar.supply = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 513 | tops_sw = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 514 | max_num = r_point = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 515 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 516 | |
| TanakaTarou | 0:56a2c0ed99c5 | 517 | use_spline = state_lib[3]; |
| TanakaTarou | 0:56a2c0ed99c5 | 518 | laundry = state_lib[4]; |
| TanakaTarou | 0:56a2c0ed99c5 | 519 | tops_state = state_lib[5]; |
| TanakaTarou | 0:56a2c0ed99c5 | 520 | |
| TanakaTarou | 0:56a2c0ed99c5 | 521 | //直線距離の長いタイミングだけ最大出力を上げる |
| TanakaTarou | 0:56a2c0ed99c5 | 522 | if(state_num == 1 || state_num == 2 || state_num >= max_num - 3 || use_spline != -1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 523 | pidRobotX.setParameter(0.95, 0, 0.085, PID_PERIOD, 0.40); |
| TanakaTarou | 0:56a2c0ed99c5 | 524 | pidRobotY.setParameter(0.95, 0, 0.085, PID_PERIOD, 0.40); |
| TanakaTarou | 0:56a2c0ed99c5 | 525 | pidRobotX.allowable_error = 0.10; |
| TanakaTarou | 0:56a2c0ed99c5 | 526 | pidRobotX.allowable_error = 0.10; |
| TanakaTarou | 0:56a2c0ed99c5 | 527 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 528 | pidRobotX.setParameter(2.00, 0, 0.230, PID_PERIOD, 0.30); |
| TanakaTarou | 0:56a2c0ed99c5 | 529 | pidRobotY.setParameter(2.00, 0, 0.230, PID_PERIOD, 0.30); |
| TanakaTarou | 0:56a2c0ed99c5 | 530 | pidRobotX.allowable_error = 0.055; |
| TanakaTarou | 0:56a2c0ed99c5 | 531 | pidRobotX.allowable_error = 0.055; |
| TanakaTarou | 0:56a2c0ed99c5 | 532 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 533 | |
| TanakaTarou | 0:56a2c0ed99c5 | 534 | tops_lib[0][0] = para_lib[0].lift; |
| TanakaTarou | 0:56a2c0ed99c5 | 535 | tops_lib[1][0] = tops_lib[2][0] = 0.095; |
| TanakaTarou | 0:56a2c0ed99c5 | 536 | tops_lib[3][0] = tops_lib[4][0] = para_lib[laundry].lift; |
| TanakaTarou | 0:56a2c0ed99c5 | 537 | |
| TanakaTarou | 0:56a2c0ed99c5 | 538 | for(int i = 0; i < TOPSNUM; i++) |
| TanakaTarou | 0:56a2c0ed99c5 | 539 | tops_lib[i][1] = para_lib[laundry].wide; |
| TanakaTarou | 0:56a2c0ed99c5 | 540 | //tops_lib[3][1] = tops_lib[4][1] += 0.23; |
| TanakaTarou | 0:56a2c0ed99c5 | 541 | |
| TanakaTarou | 0:56a2c0ed99c5 | 542 | if(laundry == 5 || laundry == 6) { |
| TanakaTarou | 0:56a2c0ed99c5 | 543 | if(tops_state == 1) |
| TanakaTarou | 0:56a2c0ed99c5 | 544 | tops_lib[4][3] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 545 | else if(tops_state == 0) |
| TanakaTarou | 0:56a2c0ed99c5 | 546 | tops_lib[4][3] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 547 | tops_lib[4][4] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 548 | } else if(laundry == 3) { |
| TanakaTarou | 0:56a2c0ed99c5 | 549 | if(SF_lib[state_num - 1][4] == Clothespin || SF_lib[state_num + 1][4] == Clothespin) |
| TanakaTarou | 0:56a2c0ed99c5 | 550 | tops_lib[4][3] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 551 | else tops_lib[4][3] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 552 | tops_lib[4][4] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 553 | } else if(laundry == 4) { |
| TanakaTarou | 0:56a2c0ed99c5 | 554 | tops_lib[4][3] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 555 | tops_lib[4][4] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 556 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 557 | tops_lib[4][3] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 558 | tops_lib[4][4] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 559 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 560 | |
| TanakaTarou | 0:56a2c0ed99c5 | 561 | short int change_zone = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 562 | if(zone) |
| TanakaTarou | 0:56a2c0ed99c5 | 563 | change_zone = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 564 | else if(!zone) |
| TanakaTarou | 0:56a2c0ed99c5 | 565 | change_zone = -1; |
| TanakaTarou | 0:56a2c0ed99c5 | 566 | |
| TanakaTarou | 0:56a2c0ed99c5 | 567 | if(use_spline >= 0) { |
| TanakaTarou | 0:56a2c0ed99c5 | 568 | route_num = now_route_num = updateRouteNum(use_spline); |
| TanakaTarou | 0:56a2c0ed99c5 | 569 | tar.x = change_zone * (spline[use_spline][route_num].giveTarget[0]); |
| TanakaTarou | 0:56a2c0ed99c5 | 570 | tar.y = spline[use_spline][route_num].giveTarget[1]; |
| TanakaTarou | 0:56a2c0ed99c5 | 571 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 572 | tar.x = change_zone * state_lib[0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 573 | tar.y = state_lib[1]; |
| TanakaTarou | 0:56a2c0ed99c5 | 574 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 575 | tar.theta = state_lib[2]; |
| TanakaTarou | 0:56a2c0ed99c5 | 576 | if(!tops_sw) { |
| TanakaTarou | 0:56a2c0ed99c5 | 577 | if(!start_flag && !do_roop) |
| TanakaTarou | 0:56a2c0ed99c5 | 578 | tar.lift = 4.2; |
| TanakaTarou | 0:56a2c0ed99c5 | 579 | else tar.lift = tops_lib[tops_num][0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 580 | tar.wide = tops_lib[tops_num][1]; |
| TanakaTarou | 0:56a2c0ed99c5 | 581 | tar.supply = tops_lib[tops_num][2]; |
| TanakaTarou | 0:56a2c0ed99c5 | 582 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 583 | tar.lift = para_lib[0].lift; |
| TanakaTarou | 0:56a2c0ed99c5 | 584 | tar.wide = para_lib[0].wide; |
| TanakaTarou | 0:56a2c0ed99c5 | 585 | tar.supply = supply_lib[0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 586 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 587 | tar.hand = tops_lib[tops_num][3]; |
| TanakaTarou | 0:56a2c0ed99c5 | 588 | tar.hand_mode = tops_lib[tops_num][4]; |
| TanakaTarou | 0:56a2c0ed99c5 | 589 | tar.now_spline = use_spline; |
| TanakaTarou | 0:56a2c0ed99c5 | 590 | tar.now_route = route_num; |
| TanakaTarou | 0:56a2c0ed99c5 | 591 | |
| TanakaTarou | 0:56a2c0ed99c5 | 592 | return tar; |
| TanakaTarou | 0:56a2c0ed99c5 | 593 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 594 | |
| TanakaTarou | 0:56a2c0ed99c5 | 595 | bool is_release = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 596 | bool convergence_tops = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 597 | |
| TanakaTarou | 0:56a2c0ed99c5 | 598 | int updateStateNum(int lib_num, int release_point) |
| TanakaTarou | 0:56a2c0ed99c5 | 599 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 600 | float t = 0.7, plus_t; |
| TanakaTarou | 0:56a2c0ed99c5 | 601 | |
| TanakaTarou | 0:56a2c0ed99c5 | 602 | static int num; |
| TanakaTarou | 0:56a2c0ed99c5 | 603 | static bool before_count = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 604 | float tape_posi = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 605 | bool flag_x = 0, flag_y = 0, flag_yaw = 0, flag_tops = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 606 | |
| TanakaTarou | 0:56a2c0ed99c5 | 607 | if(S_sw && num > 1 && num < 12) |
| TanakaTarou | 0:56a2c0ed99c5 | 608 | plus_t = 1.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 609 | else if(num == lib_num - 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 610 | plus_t = -0.5; |
| TanakaTarou | 0:56a2c0ed99c5 | 611 | else plus_t = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 612 | |
| TanakaTarou | 0:56a2c0ed99c5 | 613 | if(pidRobotX.isConvergence(t + plus_t)) |
| TanakaTarou | 0:56a2c0ed99c5 | 614 | flag_x = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 615 | |
| TanakaTarou | 0:56a2c0ed99c5 | 616 | if(pidRobotY.isConvergence(t + plus_t)) |
| TanakaTarou | 0:56a2c0ed99c5 | 617 | flag_y = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 618 | |
| TanakaTarou | 0:56a2c0ed99c5 | 619 | if(pidRobotYaw.isConvergence(t)) |
| TanakaTarou | 0:56a2c0ed99c5 | 620 | flag_yaw = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 621 | |
| TanakaTarou | 0:56a2c0ed99c5 | 622 | LED[1] = flag_x; |
| TanakaTarou | 0:56a2c0ed99c5 | 623 | LED[2] = flag_y; |
| TanakaTarou | 0:56a2c0ed99c5 | 624 | LED[3] = flag_yaw; |
| TanakaTarou | 0:56a2c0ed99c5 | 625 | |
| TanakaTarou | 0:56a2c0ed99c5 | 626 | if(state_lib[5] == 1 || state_lib[4] == 4 || state_lib[4] == 5 || state_lib[4] == 6) { |
| TanakaTarou | 0:56a2c0ed99c5 | 627 | if(convergence_tops) |
| TanakaTarou | 0:56a2c0ed99c5 | 628 | flag_tops = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 629 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 630 | else { |
| TanakaTarou | 0:56a2c0ed99c5 | 631 | if(num < lib_num - 2 && release_point == 1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 632 | if(is_release) |
| TanakaTarou | 0:56a2c0ed99c5 | 633 | flag_tops = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 634 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 635 | else if(!before_count) |
| TanakaTarou | 0:56a2c0ed99c5 | 636 | flag_tops = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 637 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 638 | |
| TanakaTarou | 0:56a2c0ed99c5 | 639 | if(flag_x && flag_y && flag_yaw && flag_tops) { |
| TanakaTarou | 0:56a2c0ed99c5 | 640 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 641 | before_count = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 642 | //実際のフィールドのラインの誤差 |
| TanakaTarou | 0:56a2c0ed99c5 | 643 | if(S_sw) |
| TanakaTarou | 0:56a2c0ed99c5 | 644 | tape_posi = -0.018; |
| TanakaTarou | 0:56a2c0ed99c5 | 645 | else tape_posi = -0.013; |
| TanakaTarou | 0:56a2c0ed99c5 | 646 | if(state_lib[1] == T_y_1 || state_lib[1] == B_y_1 || state_lib[1] == S_y_1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 647 | //Y軸targetがライン上の時, ラインセンサで誤差を検出 |
| TanakaTarou | 0:56a2c0ed99c5 | 648 | state tar; |
| TanakaTarou | 0:56a2c0ed99c5 | 649 | if(read_line_data[1] == 0.0) |
| TanakaTarou | 0:56a2c0ed99c5 | 650 | line_error = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 651 | else if(read_line_data[1] > 0.0) |
| TanakaTarou | 0:56a2c0ed99c5 | 652 | line_error = ((read_line_data[1] - 8.5) * -0.012) - tape_posi + (tar.y - odm.y); |
| TanakaTarou | 0:56a2c0ed99c5 | 653 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 654 | } else before_count = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 655 | |
| TanakaTarou | 0:56a2c0ed99c5 | 656 | if(num >= lib_num) |
| TanakaTarou | 0:56a2c0ed99c5 | 657 | num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 658 | |
| TanakaTarou | 0:56a2c0ed99c5 | 659 | num = changeNextNumber(num); |
| TanakaTarou | 0:56a2c0ed99c5 | 660 | |
| TanakaTarou | 0:56a2c0ed99c5 | 661 | return num; |
| TanakaTarou | 0:56a2c0ed99c5 | 662 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 663 | |
| TanakaTarou | 0:56a2c0ed99c5 | 664 | int updateTopsNum(int _laundry, int _tops) |
| TanakaTarou | 0:56a2c0ed99c5 | 665 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 666 | float t = 0.35; |
| TanakaTarou | 0:56a2c0ed99c5 | 667 | |
| TanakaTarou | 0:56a2c0ed99c5 | 668 | static int num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 669 | bool flag_lift = 0, flag_wide = 0, flag_supply = 0, flag_hand = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 670 | static bool last_hand = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 671 | static float start_time; |
| TanakaTarou | 0:56a2c0ed99c5 | 672 | |
| TanakaTarou | 0:56a2c0ed99c5 | 673 | updateSupplyTarget(num, _laundry); |
| TanakaTarou | 0:56a2c0ed99c5 | 674 | |
| TanakaTarou | 0:56a2c0ed99c5 | 675 | if(pidLift.isConvergence(t)) |
| TanakaTarou | 0:56a2c0ed99c5 | 676 | flag_lift = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 677 | else flag_lift = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 678 | /* |
| TanakaTarou | 0:56a2c0ed99c5 | 679 | if(pidWide.isConvergence(t)) |
| TanakaTarou | 0:56a2c0ed99c5 | 680 | flag_wide = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 681 | else flag_wide = 0;*/ |
| TanakaTarou | 0:56a2c0ed99c5 | 682 | flag_wide = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 683 | |
| TanakaTarou | 0:56a2c0ed99c5 | 684 | if(pidSupply.isConvergence(t)) |
| TanakaTarou | 0:56a2c0ed99c5 | 685 | flag_supply = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 686 | else flag_supply = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 687 | |
| TanakaTarou | 0:56a2c0ed99c5 | 688 | if(last_hand != (bool)tops_lib[num][3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 689 | float plus_t; |
| TanakaTarou | 0:56a2c0ed99c5 | 690 | |
| TanakaTarou | 0:56a2c0ed99c5 | 691 | if(_laundry == 3 || _laundry == 4 || _laundry == 5) |
| TanakaTarou | 0:56a2c0ed99c5 | 692 | plus_t = 0.35; |
| TanakaTarou | 0:56a2c0ed99c5 | 693 | else { |
| TanakaTarou | 0:56a2c0ed99c5 | 694 | if(last_hand == 0) |
| TanakaTarou | 0:56a2c0ed99c5 | 695 | plus_t = 0.0; |
| TanakaTarou | 0:56a2c0ed99c5 | 696 | else if(last_hand == 1) |
| TanakaTarou | 0:56a2c0ed99c5 | 697 | plus_t = 0.75; |
| TanakaTarou | 0:56a2c0ed99c5 | 698 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 699 | |
| TanakaTarou | 0:56a2c0ed99c5 | 700 | if(timer.read() - start_time > t + plus_t) { |
| TanakaTarou | 0:56a2c0ed99c5 | 701 | flag_hand = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 702 | last_hand = (bool)tops_lib[num][3]; |
| TanakaTarou | 0:56a2c0ed99c5 | 703 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 704 | else flag_hand = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 705 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 706 | flag_hand = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 707 | start_time = timer.read(); |
| TanakaTarou | 0:56a2c0ed99c5 | 708 | last_hand = (bool)tops_lib[num][3]; |
| TanakaTarou | 0:56a2c0ed99c5 | 709 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 710 | |
| TanakaTarou | 0:56a2c0ed99c5 | 711 | if(flag_lift && flag_wide && flag_supply && flag_hand) |
| TanakaTarou | 0:56a2c0ed99c5 | 712 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 713 | if(num == 3) { |
| TanakaTarou | 0:56a2c0ed99c5 | 714 | is_release = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 715 | if(_tops == 1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 716 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 717 | is_release = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 718 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 719 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 720 | |
| TanakaTarou | 0:56a2c0ed99c5 | 721 | else if(num == 4) { |
| TanakaTarou | 0:56a2c0ed99c5 | 722 | convergence_tops = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 723 | if(_laundry == 1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 724 | if(odm.y < 2.45) |
| TanakaTarou | 0:56a2c0ed99c5 | 725 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 726 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 727 | else if(_laundry == 2) { |
| TanakaTarou | 0:56a2c0ed99c5 | 728 | if(odm.y < 4.70) |
| TanakaTarou | 0:56a2c0ed99c5 | 729 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 730 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 731 | else if(_laundry == 3) { |
| TanakaTarou | 0:56a2c0ed99c5 | 732 | if(odm.y < 6.45 && _tops == 0) |
| TanakaTarou | 0:56a2c0ed99c5 | 733 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 734 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 735 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 736 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 737 | convergence_tops = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 738 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 739 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 740 | else convergence_tops = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 741 | |
| TanakaTarou | 0:56a2c0ed99c5 | 742 | if(_laundry == 6 && _tops == 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 743 | convergence_tops = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 744 | |
| TanakaTarou | 0:56a2c0ed99c5 | 745 | if(num >= TOPSNUM) |
| TanakaTarou | 0:56a2c0ed99c5 | 746 | num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 747 | if(_tops == 2) |
| TanakaTarou | 0:56a2c0ed99c5 | 748 | num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 749 | |
| TanakaTarou | 0:56a2c0ed99c5 | 750 | return num; |
| TanakaTarou | 0:56a2c0ed99c5 | 751 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 752 | |
| TanakaTarou | 0:56a2c0ed99c5 | 753 | void updateSupplyTarget(int tops_num, int _laundry) |
| TanakaTarou | 0:56a2c0ed99c5 | 754 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 755 | static int num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 756 | static int pre_tops_num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 757 | |
| TanakaTarou | 0:56a2c0ed99c5 | 758 | if(pre_tops_num == TOPSNUM - 1 && tops_num == 0) |
| TanakaTarou | 0:56a2c0ed99c5 | 759 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 760 | |
| TanakaTarou | 0:56a2c0ed99c5 | 761 | if(_laundry == 1) { |
| TanakaTarou | 0:56a2c0ed99c5 | 762 | if(num >= 4) |
| TanakaTarou | 0:56a2c0ed99c5 | 763 | num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 764 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 765 | else if(_laundry == 2) { |
| TanakaTarou | 0:56a2c0ed99c5 | 766 | if(num >= 4) |
| TanakaTarou | 0:56a2c0ed99c5 | 767 | num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 768 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 769 | else if(_laundry == 3) { |
| TanakaTarou | 0:56a2c0ed99c5 | 770 | if(num >= 1) |
| TanakaTarou | 0:56a2c0ed99c5 | 771 | num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 772 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 773 | else num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 774 | |
| TanakaTarou | 0:56a2c0ed99c5 | 775 | pre_tops_num = tops_num; |
| TanakaTarou | 0:56a2c0ed99c5 | 776 | |
| TanakaTarou | 0:56a2c0ed99c5 | 777 | tops_lib[0][2] = tops_lib[1][2] = tops_lib[2][2] = supply_lib[num]; |
| TanakaTarou | 0:56a2c0ed99c5 | 778 | |
| TanakaTarou | 0:56a2c0ed99c5 | 779 | if(enc[4] > 0.5) { |
| TanakaTarou | 0:56a2c0ed99c5 | 780 | if(num >= 3 || S_sw) |
| TanakaTarou | 0:56a2c0ed99c5 | 781 | tops_lib[3][2] = tops_lib[4][2] = supply_lib[0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 782 | else tops_lib[3][2] = tops_lib[4][2] = supply_lib[num + 1]; |
| TanakaTarou | 0:56a2c0ed99c5 | 783 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 784 | tops_lib[3][2] = tops_lib[4][2] = supply_lib[num]; |
| TanakaTarou | 0:56a2c0ed99c5 | 785 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 786 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 787 | |
| TanakaTarou | 0:56a2c0ed99c5 | 788 | int updateRouteNum(int spline_num) |
| TanakaTarou | 0:56a2c0ed99c5 | 789 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 790 | static int num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 791 | static int last_spline_num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 792 | |
| TanakaTarou | 0:56a2c0ed99c5 | 793 | if(last_spline_num != spline_num) |
| TanakaTarou | 0:56a2c0ed99c5 | 794 | num = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 795 | |
| TanakaTarou | 0:56a2c0ed99c5 | 796 | if(spline[spline_num][num].now_count >= spline[spline_num][num].line - 1) |
| TanakaTarou | 0:56a2c0ed99c5 | 797 | num++; |
| TanakaTarou | 0:56a2c0ed99c5 | 798 | |
| TanakaTarou | 0:56a2c0ed99c5 | 799 | if(num >= ROUTE_NUM) |
| TanakaTarou | 0:56a2c0ed99c5 | 800 | num = ROUTE_NUM - 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 801 | |
| TanakaTarou | 0:56a2c0ed99c5 | 802 | last_spline_num = spline_num; |
| TanakaTarou | 0:56a2c0ed99c5 | 803 | |
| TanakaTarou | 0:56a2c0ed99c5 | 804 | return num; |
| TanakaTarou | 0:56a2c0ed99c5 | 805 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 806 | |
| TanakaTarou | 0:56a2c0ed99c5 | 807 | void imuCalibration() |
| TanakaTarou | 0:56a2c0ed99c5 | 808 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 809 | LED[0] = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 810 | wait(1); |
| TanakaTarou | 0:56a2c0ed99c5 | 811 | imu.performCalibration(); |
| TanakaTarou | 0:56a2c0ed99c5 | 812 | imu.startAngleComputing(); |
| TanakaTarou | 0:56a2c0ed99c5 | 813 | LED[0] = 1; |
| TanakaTarou | 0:56a2c0ed99c5 | 814 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 815 | |
| TanakaTarou | 0:56a2c0ed99c5 | 816 | void reset_System() |
| TanakaTarou | 0:56a2c0ed99c5 | 817 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 818 | bool caliblation_sw = sw[0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 819 | static bool last_sw = false; |
| TanakaTarou | 0:56a2c0ed99c5 | 820 | if(!caliblation_sw && last_sw) |
| TanakaTarou | 0:56a2c0ed99c5 | 821 | NVIC_SystemReset(); |
| TanakaTarou | 0:56a2c0ed99c5 | 822 | else if(caliblation_sw && reset_sw) |
| TanakaTarou | 0:56a2c0ed99c5 | 823 | last_sw = true; |
| TanakaTarou | 0:56a2c0ed99c5 | 824 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 825 | |
| TanakaTarou | 0:56a2c0ed99c5 | 826 | int changeNextNumber(int num) |
| TanakaTarou | 0:56a2c0ed99c5 | 827 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 828 | if(!S_sw) { |
| TanakaTarou | 0:56a2c0ed99c5 | 829 | if(!match) { |
| TanakaTarou | 0:56a2c0ed99c5 | 830 | if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 831 | if(!B_sw[0] && num > 1 && num < 6) { |
| TanakaTarou | 0:56a2c0ed99c5 | 832 | num = 6; |
| TanakaTarou | 0:56a2c0ed99c5 | 833 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 834 | if(!B_sw[1] && num > 5 && num < 11) { |
| TanakaTarou | 0:56a2c0ed99c5 | 835 | num = 11; |
| TanakaTarou | 0:56a2c0ed99c5 | 836 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 837 | if(!B_sw[2] && num > 10 && num < 16) { |
| TanakaTarou | 0:56a2c0ed99c5 | 838 | num = 16; |
| TanakaTarou | 0:56a2c0ed99c5 | 839 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 840 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 841 | else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 842 | if(!T_sw[0] && num > 1 && num < 6) { |
| TanakaTarou | 0:56a2c0ed99c5 | 843 | num = 6; |
| TanakaTarou | 0:56a2c0ed99c5 | 844 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 845 | if(!T_sw[1] && num > 5 && num < 11) { |
| TanakaTarou | 0:56a2c0ed99c5 | 846 | num = 11; |
| TanakaTarou | 0:56a2c0ed99c5 | 847 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 848 | if(!T_sw[2] && num > 10 && num < 16) { |
| TanakaTarou | 0:56a2c0ed99c5 | 849 | num = 16; |
| TanakaTarou | 0:56a2c0ed99c5 | 850 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 851 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 852 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 853 | else if(match) { |
| TanakaTarou | 0:56a2c0ed99c5 | 854 | if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 855 | if(!B_sw[0] && num > 1 && num < 6) { |
| TanakaTarou | 0:56a2c0ed99c5 | 856 | num = 6; |
| TanakaTarou | 0:56a2c0ed99c5 | 857 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 858 | if(!B_sw[1] && num > 5 && num < 11) { |
| TanakaTarou | 0:56a2c0ed99c5 | 859 | num = 11; |
| TanakaTarou | 0:56a2c0ed99c5 | 860 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 861 | if(!B_sw[2] && num > 10 && num < 16) { |
| TanakaTarou | 0:56a2c0ed99c5 | 862 | num = 16; |
| TanakaTarou | 0:56a2c0ed99c5 | 863 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 864 | if(!B_sw[3] && num > 15 && num < 21) { |
| TanakaTarou | 0:56a2c0ed99c5 | 865 | num = 21; |
| TanakaTarou | 0:56a2c0ed99c5 | 866 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 867 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 868 | else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) { |
| TanakaTarou | 0:56a2c0ed99c5 | 869 | if(!T_sw[0] && num > 1 && num < 6) { |
| TanakaTarou | 0:56a2c0ed99c5 | 870 | num = 6; |
| TanakaTarou | 0:56a2c0ed99c5 | 871 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 872 | if(!T_sw[1] && num > 5 && num < 11) { |
| TanakaTarou | 0:56a2c0ed99c5 | 873 | num = 11; |
| TanakaTarou | 0:56a2c0ed99c5 | 874 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 875 | if(!T_sw[2] && num > 10 && num < 16) { |
| TanakaTarou | 0:56a2c0ed99c5 | 876 | num = 16; |
| TanakaTarou | 0:56a2c0ed99c5 | 877 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 878 | if(!T_sw[3] && num > 15 && num < 21) { |
| TanakaTarou | 0:56a2c0ed99c5 | 879 | num = 21; |
| TanakaTarou | 0:56a2c0ed99c5 | 880 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 881 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 882 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 883 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 884 | |
| TanakaTarou | 0:56a2c0ed99c5 | 885 | return num; |
| TanakaTarou | 0:56a2c0ed99c5 | 886 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 887 | |
| TanakaTarou | 0:56a2c0ed99c5 | 888 | elements getRobotVelocity(state tar) |
| TanakaTarou | 0:56a2c0ed99c5 | 889 | { |
| TanakaTarou | 0:56a2c0ed99c5 | 890 | elements vel; |
| TanakaTarou | 0:56a2c0ed99c5 | 891 | |
| TanakaTarou | 0:56a2c0ed99c5 | 892 | *pidRobotYaw.target = tar.theta; |
| TanakaTarou | 0:56a2c0ed99c5 | 893 | vel.yaw = pidRobotYaw.output; |
| TanakaTarou | 0:56a2c0ed99c5 | 894 | |
| TanakaTarou | 0:56a2c0ed99c5 | 895 | *pidRobotX.target = tar.x; |
| TanakaTarou | 0:56a2c0ed99c5 | 896 | *pidRobotY.target = tar.y; |
| TanakaTarou | 0:56a2c0ed99c5 | 897 | *pidLift.target = tar.lift; |
| TanakaTarou | 0:56a2c0ed99c5 | 898 | *pidWide.target = tar.wide; |
| TanakaTarou | 0:56a2c0ed99c5 | 899 | *pidSupply.target = tar.supply; |
| TanakaTarou | 0:56a2c0ed99c5 | 900 | |
| TanakaTarou | 0:56a2c0ed99c5 | 901 | //シーツを引っ張る際, hand機構を片方だけ閉じさせる処理 |
| TanakaTarou | 0:56a2c0ed99c5 | 902 | if(tar.hand_mode == false) { |
| TanakaTarou | 0:56a2c0ed99c5 | 903 | if(zone) { |
| TanakaTarou | 0:56a2c0ed99c5 | 904 | servo_right = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 905 | servo_left = tar.hand; |
| TanakaTarou | 0:56a2c0ed99c5 | 906 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 907 | servo_right = tar.hand; |
| TanakaTarou | 0:56a2c0ed99c5 | 908 | servo_left = 0; |
| TanakaTarou | 0:56a2c0ed99c5 | 909 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 910 | } else servo_right = servo_left = tar.hand; |
| TanakaTarou | 0:56a2c0ed99c5 | 911 | servo_mode = tar.hand_mode; |
| TanakaTarou | 0:56a2c0ed99c5 | 912 | |
| TanakaTarou | 0:56a2c0ed99c5 | 913 | if(tar.now_spline >= 0) { |
| TanakaTarou | 0:56a2c0ed99c5 | 914 | spline[tar.now_spline][tar.now_route].rescaleVel(pidRobotX.output, pidRobotY.output); //PIDを使うと起こる問題を解決する処理 |
| TanakaTarou | 0:56a2c0ed99c5 | 915 | vel.x = spline[tar.now_spline][tar.now_route].velocity[0]; |
| TanakaTarou | 0:56a2c0ed99c5 | 916 | vel.y = spline[tar.now_spline][tar.now_route].velocity[1]; |
| TanakaTarou | 0:56a2c0ed99c5 | 917 | } else { |
| TanakaTarou | 0:56a2c0ed99c5 | 918 | vel.x = pidRobotX.output; |
| TanakaTarou | 0:56a2c0ed99c5 | 919 | vel.y = pidRobotY.output; |
| TanakaTarou | 0:56a2c0ed99c5 | 920 | } |
| TanakaTarou | 0:56a2c0ed99c5 | 921 | |
| TanakaTarou | 0:56a2c0ed99c5 | 922 | return vel; |
| TanakaTarou | 0:56a2c0ed99c5 | 923 | } |