ichinoseki_Bteam_2019 / Mbed 2 deprecated 2019_AR_Itsuki

Dependencies:   IMU mbed Odometer PID MDD RotaryEncoder UART Mycan DriveConroller

Committer:
TanakaTarou
Date:
Sun Jan 12 08:12:42 2020 +0000
Revision:
1:0f33a68d1390
Parent:
0:56a2c0ed99c5
Itsuki

Who changed what in which revision?

UserRevisionLine numberNew 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 }