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