main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
robot.cpp@15:da88e62863a1, 2021-03-06 (annotated)
- Committer:
- THtakahiro702286
- Date:
- Sat Mar 06 07:26:44 2021 +0000
- Revision:
- 15:da88e62863a1
- Parent:
- 13:d61a64c81c16
- Child:
- 16:e3846421554b
keepokasii
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:3f87ec23c3cf | 1 | #include "robot.h" |
piroro4560 | 0:3f87ec23c3cf | 2 | |
piroro4560 | 4:a4f94f186ba0 | 3 | Robot::Robot() : |
piroro4560 | 4:a4f94f186ba0 | 4 | spin(P, I, D, interval), |
THtakahiro702286 | 15:da88e62863a1 | 5 | sgoal(P, I, D, interval), |
piroro4560 | 1:6a5065829cfc | 6 | pc(USBTX, USBRX, 115200), |
piroro4560 | 1:6a5065829cfc | 7 | shot(kicker), |
piroro4560 | 9:904dac75a729 | 8 | drib(ESCpin) |
piroro4560 | 0:3f87ec23c3cf | 9 | { |
piroro4560 | 4:a4f94f186ba0 | 10 | |
THtakahiro702286 | 13:d61a64c81c16 | 11 | omni.wheel[2].setRadian(PI/4 * 1); |
THtakahiro702286 | 13:d61a64c81c16 | 12 | omni.wheel[3].setRadian(PI/4 * 3); |
THtakahiro702286 | 13:d61a64c81c16 | 13 | omni.wheel[0].setRadian(PI/4 * 5); |
THtakahiro702286 | 13:d61a64c81c16 | 14 | omni.wheel[1].setRadian(PI/4 * 7); |
piroro4560 | 9:904dac75a729 | 15 | motor[0] = new KohiMD(PA_15); |
piroro4560 | 9:904dac75a729 | 16 | motor[1] = new KohiMD(PA_6); |
piroro4560 | 9:904dac75a729 | 17 | motor[2] = new KohiMD(PA_7); |
piroro4560 | 9:904dac75a729 | 18 | motor[3] = new KohiMD(PB_6); |
piroro4560 | 1:6a5065829cfc | 19 | spin.setInputLimits(-180, 180); |
THtakahiro702286 | 15:da88e62863a1 | 20 | sgoal.setInputLimits(-180, 180); |
THtakahiro702286 | 5:f5e79163d0eb | 21 | spin.setOutputLimits(-0.4,0.4); |
THtakahiro702286 | 15:da88e62863a1 | 22 | sgoal.setOutputLimits(-0.4,0.4); |
piroro4560 | 1:6a5065829cfc | 23 | spin.setBias(0); |
THtakahiro702286 | 15:da88e62863a1 | 24 | sgoal.setBias(0); |
piroro4560 | 1:6a5065829cfc | 25 | spin.setMode(1); |
THtakahiro702286 | 15:da88e62863a1 | 26 | sgoal.setMode(1); |
piroro4560 | 1:6a5065829cfc | 27 | spin.setSetPoint(0); |
THtakahiro702286 | 15:da88e62863a1 | 28 | sgoal.setSetPoint(0); |
piroro4560 | 9:904dac75a729 | 29 | shot.setkickperiod(2.0); |
piroro4560 | 9:904dac75a729 | 30 | shot.setoutputtime(0.1); |
piroro4560 | 4:a4f94f186ba0 | 31 | drib.setspeed(0.0); |
THtakahiro702286 | 5:f5e79163d0eb | 32 | startb = 0; |
piroro4560 | 0:3f87ec23c3cf | 33 | } |
piroro4560 | 0:3f87ec23c3cf | 34 | |
piroro4560 | 9:904dac75a729 | 35 | void Robot::start(uint8_t Team, uint8_t Algorithm) |
piroro4560 | 0:3f87ec23c3cf | 36 | { |
THtakahiro702286 | 15:da88e62863a1 | 37 | while(1){ |
THtakahiro702286 | 15:da88e62863a1 | 38 | printf("%d %d ", (int)sensor_.irVal ,(int)sensor_.ballKeep); |
THtakahiro702286 | 15:da88e62863a1 | 39 | spin.setProcessValue(sensor_.angleLimit/*-sensor_.ballAngle*/); |
piroro4560 | 4:a4f94f186ba0 | 40 | spin_power = spin.compute(); |
THtakahiro702286 | 15:da88e62863a1 | 41 | |
THtakahiro702286 | 15:da88e62863a1 | 42 | // theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ; |
THtakahiro702286 | 15:da88e62863a1 | 43 | // theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1); |
THtakahiro702286 | 15:da88e62863a1 | 44 | // theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2; |
THtakahiro702286 | 15:da88e62863a1 | 45 | theta = ((sensor_.ballAngle>0) - (sensor_.ballAngle<0)) * (2.0/(2-fabs(sensor_.ballAngle)/180) - 1) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2; |
THtakahiro702286 | 15:da88e62863a1 | 46 | printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 100)); |
THtakahiro702286 | 15:da88e62863a1 | 47 | omni.computeCircular(/*0*/1.1*sensor_.ballRange / 100.0,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 48 | for (int i=0; i<4; i++) { |
THtakahiro702286 | 15:da88e62863a1 | 49 | thisSpeed[i] = omni.wheel[i]; |
piroro4560 | 9:904dac75a729 | 50 | } |
THtakahiro702286 | 15:da88e62863a1 | 51 | if((fabs(sensor_.ballAngle) < 50 && sensor_.ballRange) || sensor_.ballKeep) dribbleCheck(0.6); |
THtakahiro702286 | 15:da88e62863a1 | 52 | else dribbleCheck(0.0); |
THtakahiro702286 | 15:da88e62863a1 | 53 | motor[0]->setSpeed(thisSpeed[0]); |
THtakahiro702286 | 15:da88e62863a1 | 54 | motor[1]->setSpeed(thisSpeed[1]); |
THtakahiro702286 | 15:da88e62863a1 | 55 | motor[2]->setSpeed(-1*thisSpeed[2]); |
THtakahiro702286 | 15:da88e62863a1 | 56 | motor[3]->setSpeed(-1*thisSpeed[3]); |
THtakahiro702286 | 15:da88e62863a1 | 57 | } |
THtakahiro702286 | 15:da88e62863a1 | 58 | // 昔 |
THtakahiro702286 | 15:da88e62863a1 | 59 | // while (1) { |
THtakahiro702286 | 15:da88e62863a1 | 60 | // startb=1; |
THtakahiro702286 | 15:da88e62863a1 | 61 | // spin.setProcessValue(sensor_.angleLimit); |
THtakahiro702286 | 15:da88e62863a1 | 62 | // theta = sensor_.ballAngle * ballExtra * PI / 180.0; |
THtakahiro702286 | 15:da88e62863a1 | 63 | // spin_power = spin.compute(); |
THtakahiro702286 | 15:da88e62863a1 | 64 | // if (/*sensor_.ballKeep*/0) { |
THtakahiro702286 | 15:da88e62863a1 | 65 | // /*ボールをつかんでるなら*/ |
THtakahiro702286 | 15:da88e62863a1 | 66 | //// drib.setspeed(0.5); |
THtakahiro702286 | 15:da88e62863a1 | 67 | // if(Team) |
THtakahiro702286 | 15:da88e62863a1 | 68 | // { |
THtakahiro702286 | 15:da88e62863a1 | 69 | // /*あおに向かうよ*/ |
THtakahiro702286 | 15:da88e62863a1 | 70 | //// /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/); |
THtakahiro702286 | 15:da88e62863a1 | 71 | //// omni.computeCircular(sensor_.blueRange,PI / 180.0 * sensor_.blueAngle, spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 72 | // |
THtakahiro702286 | 15:da88e62863a1 | 73 | // } |
THtakahiro702286 | 15:da88e62863a1 | 74 | // else |
THtakahiro702286 | 15:da88e62863a1 | 75 | // { |
THtakahiro702286 | 15:da88e62863a1 | 76 | // /*きいろに向かうよ*/ |
THtakahiro702286 | 15:da88e62863a1 | 77 | //// /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/); |
THtakahiro702286 | 15:da88e62863a1 | 78 | // } |
THtakahiro702286 | 15:da88e62863a1 | 79 | // } |
THtakahiro702286 | 15:da88e62863a1 | 80 | // else |
THtakahiro702286 | 15:da88e62863a1 | 81 | // /*つかんでないなら*/ |
THtakahiro702286 | 15:da88e62863a1 | 82 | // { |
THtakahiro702286 | 15:da88e62863a1 | 83 | //// for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]); |
THtakahiro702286 | 15:da88e62863a1 | 84 | //// pc.printf("\r\n"); |
THtakahiro702286 | 15:da88e62863a1 | 85 | // drib.setspeed(0.0); |
THtakahiro702286 | 15:da88e62863a1 | 86 | //// omni.computeCircular(sensor_.ballRange, (PI / 180.0) * sensor_.ballAngle + theta, spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 87 | // omni.computeCircular(sensor_.ballRange, sensor_.ballAngle, spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 88 | // } |
THtakahiro702286 | 15:da88e62863a1 | 89 | //// printf("%d ",(int)sensor.ballKeep); |
THtakahiro702286 | 15:da88e62863a1 | 90 | // if(sensor_.line[0] || sensor_.line[1]) |
THtakahiro702286 | 15:da88e62863a1 | 91 | // { |
THtakahiro702286 | 15:da88e62863a1 | 92 | // omni.computeCircular(1,-1*PI/2.0,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 93 | // } |
THtakahiro702286 | 15:da88e62863a1 | 94 | // if(sensor_.line[2] || sensor_.line[3]) |
THtakahiro702286 | 15:da88e62863a1 | 95 | // { |
THtakahiro702286 | 15:da88e62863a1 | 96 | // omni.computeCircular(1,PI/2.0,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 97 | // } |
THtakahiro702286 | 15:da88e62863a1 | 98 | // if(sensor_.line[4]) |
THtakahiro702286 | 15:da88e62863a1 | 99 | // { |
THtakahiro702286 | 15:da88e62863a1 | 100 | // omni.computeCircular(1,0,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 101 | // } |
THtakahiro702286 | 15:da88e62863a1 | 102 | // if(sensor_.line[5]) |
THtakahiro702286 | 15:da88e62863a1 | 103 | // { |
THtakahiro702286 | 15:da88e62863a1 | 104 | // omni.computeCircular(1,PI,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 105 | // } |
THtakahiro702286 | 15:da88e62863a1 | 106 | // |
THtakahiro702286 | 15:da88e62863a1 | 107 | // if (startb) { |
THtakahiro702286 | 15:da88e62863a1 | 108 | // omni2wheel = omni.wheel[2]; |
THtakahiro702286 | 15:da88e62863a1 | 109 | // omni3wheel = omni.wheel[3]; |
THtakahiro702286 | 15:da88e62863a1 | 110 | //// motor[0]->setSpeed(omni.wheel[0]); |
THtakahiro702286 | 15:da88e62863a1 | 111 | //// motor[1]->setSpeed(omni.wheel[1]); |
THtakahiro702286 | 15:da88e62863a1 | 112 | //// motor[2]->setSpeed(omni.wheel[2]); |
THtakahiro702286 | 15:da88e62863a1 | 113 | //// motor[3]->setSpeed(omni.wheel[3]); |
THtakahiro702286 | 15:da88e62863a1 | 114 | //// motor[3]->setSpeed(-omni3wheel); |
THtakahiro702286 | 15:da88e62863a1 | 115 | // shot.outPut(); |
THtakahiro702286 | 15:da88e62863a1 | 116 | // |
THtakahiro702286 | 15:da88e62863a1 | 117 | // } else { |
THtakahiro702286 | 15:da88e62863a1 | 118 | // motor[0]->setSpeed(0); |
THtakahiro702286 | 15:da88e62863a1 | 119 | // motor[1]->setSpeed(0); |
THtakahiro702286 | 15:da88e62863a1 | 120 | // motor[2]->setSpeed(0); |
THtakahiro702286 | 15:da88e62863a1 | 121 | // motor[3]->setSpeed(0); |
THtakahiro702286 | 15:da88e62863a1 | 122 | // } |
THtakahiro702286 | 15:da88e62863a1 | 123 | // } |
THtakahiro702286 | 15:da88e62863a1 | 124 | } |
THtakahiro702286 | 15:da88e62863a1 | 125 | |
THtakahiro702286 | 15:da88e62863a1 | 126 | void Robot::pidtest() |
THtakahiro702286 | 15:da88e62863a1 | 127 | { |
THtakahiro702286 | 15:da88e62863a1 | 128 | while(1){ |
THtakahiro702286 | 15:da88e62863a1 | 129 | spin.setProcessValue(sensor_.angleLimit); |
THtakahiro702286 | 15:da88e62863a1 | 130 | spin_power = spin.compute(); |
THtakahiro702286 | 15:da88e62863a1 | 131 | omni.computeCircular(0,0,spin_power); |
piroro4560 | 9:904dac75a729 | 132 | if(sensor_.line[0] || sensor_.line[1]) |
piroro4560 | 4:a4f94f186ba0 | 133 | { |
THtakahiro702286 | 15:da88e62863a1 | 134 | omni.computeCircular(1,PI/2.0,spin_power); |
piroro4560 | 4:a4f94f186ba0 | 135 | } |
piroro4560 | 9:904dac75a729 | 136 | if(sensor_.line[2] || sensor_.line[3]) |
piroro4560 | 4:a4f94f186ba0 | 137 | { |
THtakahiro702286 | 15:da88e62863a1 | 138 | omni.computeCircular(1,-1*PI/2.0,spin_power); |
piroro4560 | 4:a4f94f186ba0 | 139 | } |
piroro4560 | 9:904dac75a729 | 140 | if(sensor_.line[4]) |
piroro4560 | 4:a4f94f186ba0 | 141 | { |
piroro4560 | 4:a4f94f186ba0 | 142 | omni.computeCircular(1,0,spin_power); |
piroro4560 | 4:a4f94f186ba0 | 143 | } |
piroro4560 | 9:904dac75a729 | 144 | if(sensor_.line[5]) |
piroro4560 | 4:a4f94f186ba0 | 145 | { |
piroro4560 | 4:a4f94f186ba0 | 146 | omni.computeCircular(1,PI,spin_power); |
piroro4560 | 4:a4f94f186ba0 | 147 | } |
piroro4560 | 12:19149697667d | 148 | for (int i=0; i<4; i++) { |
piroro4560 | 12:19149697667d | 149 | thisSpeed[i] = omni.wheel[i]; |
piroro4560 | 12:19149697667d | 150 | } |
piroro4560 | 12:19149697667d | 151 | motor[0]->setSpeed(thisSpeed[0]); |
piroro4560 | 12:19149697667d | 152 | motor[1]->setSpeed(thisSpeed[1]); |
piroro4560 | 12:19149697667d | 153 | motor[2]->setSpeed(-1*thisSpeed[2]); |
piroro4560 | 12:19149697667d | 154 | motor[3]->setSpeed(-1*thisSpeed[3]); |
piroro4560 | 12:19149697667d | 155 | } |
piroro4560 | 12:19149697667d | 156 | } |
piroro4560 | 12:19149697667d | 157 | |
THtakahiro702286 | 8:713bbc1fb58f | 158 | void Robot::motorCheck(int motorNumber, float power) |
THtakahiro702286 | 8:713bbc1fb58f | 159 | { |
THtakahiro702286 | 8:713bbc1fb58f | 160 | _motorNumber = motorNumber; |
piroro4560 | 9:904dac75a729 | 161 | for(i = 0; i < 4; i++) |
THtakahiro702286 | 8:713bbc1fb58f | 162 | { |
piroro4560 | 9:904dac75a729 | 163 | if(i == _motorNumber) motor[i]->setSpeed(power); |
piroro4560 | 9:904dac75a729 | 164 | else motor[i]->setSpeed(0); |
THtakahiro702286 | 8:713bbc1fb58f | 165 | } |
THtakahiro702286 | 8:713bbc1fb58f | 166 | } |
THtakahiro702286 | 8:713bbc1fb58f | 167 | |
piroro4560 | 9:904dac75a729 | 168 | void Robot::motorStop(double pwm) |
THtakahiro702286 | 8:713bbc1fb58f | 169 | { |
THtakahiro702286 | 11:39f44a1dc256 | 170 | // motorSpeed = 0.5; |
THtakahiro702286 | 11:39f44a1dc256 | 171 | // printf("%d\r\n", (int)(motorSpeed*10)); |
THtakahiro702286 | 11:39f44a1dc256 | 172 | for(i=0; i<4; i++) motor[i]->setSpeed(pwm); |
THtakahiro702286 | 8:713bbc1fb58f | 173 | } |
THtakahiro702286 | 8:713bbc1fb58f | 174 | |
THtakahiro702286 | 8:713bbc1fb58f | 175 | void Robot::kickCheck() |
THtakahiro702286 | 8:713bbc1fb58f | 176 | { |
THtakahiro702286 | 8:713bbc1fb58f | 177 | shot.outPut(); |
THtakahiro702286 | 8:713bbc1fb58f | 178 | } |
THtakahiro702286 | 8:713bbc1fb58f | 179 | |
THtakahiro702286 | 8:713bbc1fb58f | 180 | void Robot::dribbleCheck(float power) |
THtakahiro702286 | 8:713bbc1fb58f | 181 | { |
THtakahiro702286 | 8:713bbc1fb58f | 182 | drib.setspeed(power); |
THtakahiro702286 | 8:713bbc1fb58f | 183 | } |
THtakahiro702286 | 8:713bbc1fb58f | 184 | |
THtakahiro702286 | 13:d61a64c81c16 | 185 | void Robot::moveTest() |
THtakahiro702286 | 13:d61a64c81c16 | 186 | { |
THtakahiro702286 | 13:d61a64c81c16 | 187 | while(1){ |
THtakahiro702286 | 15:da88e62863a1 | 188 | // printf("%d %d ", (int)sensor_.irVal ,(int)sensor_.ballKeep); |
THtakahiro702286 | 15:da88e62863a1 | 189 | spin.setProcessValue(sensor_.angleLimit/*-sensor_.ballAngle*/); |
THtakahiro702286 | 15:da88e62863a1 | 190 | if(sensor_.team) sgoal.setProcessValue(-sensor_.blueAngle/*-sensor_.ballAngle*/); |
THtakahiro702286 | 15:da88e62863a1 | 191 | else sgoal.setProcessValue(-sensor_.yellowAngle/*-sensor_.ballAngle*/); |
THtakahiro702286 | 13:d61a64c81c16 | 192 | spin_power = spin.compute(); |
THtakahiro702286 | 13:d61a64c81c16 | 193 | |
THtakahiro702286 | 15:da88e62863a1 | 194 | // theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ; |
THtakahiro702286 | 15:da88e62863a1 | 195 | // theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1); |
THtakahiro702286 | 15:da88e62863a1 | 196 | // theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2; |
THtakahiro702286 | 15:da88e62863a1 | 197 | // printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 100)); |
THtakahiro702286 | 15:da88e62863a1 | 198 | if(sensor_.line[0] || sensor_.line[1]) |
THtakahiro702286 | 15:da88e62863a1 | 199 | { |
THtakahiro702286 | 15:da88e62863a1 | 200 | omni.computeCircular(1,PI/2.0,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 201 | lineflag = 1; |
THtakahiro702286 | 15:da88e62863a1 | 202 | } |
THtakahiro702286 | 15:da88e62863a1 | 203 | if(sensor_.line[2] || sensor_.line[3]) |
THtakahiro702286 | 15:da88e62863a1 | 204 | { |
THtakahiro702286 | 15:da88e62863a1 | 205 | omni.computeCircular(1,-1*PI/2.0,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 206 | lineflag = 1; |
THtakahiro702286 | 15:da88e62863a1 | 207 | } |
THtakahiro702286 | 15:da88e62863a1 | 208 | if(sensor_.line[4]) |
THtakahiro702286 | 15:da88e62863a1 | 209 | { |
THtakahiro702286 | 15:da88e62863a1 | 210 | omni.computeCircular(1,0,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 211 | lineflag = 1; |
THtakahiro702286 | 15:da88e62863a1 | 212 | } |
THtakahiro702286 | 15:da88e62863a1 | 213 | if(sensor_.line[5]) |
THtakahiro702286 | 15:da88e62863a1 | 214 | { |
THtakahiro702286 | 15:da88e62863a1 | 215 | omni.computeCircular(1,PI,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 216 | lineflag = 1; |
THtakahiro702286 | 15:da88e62863a1 | 217 | } |
THtakahiro702286 | 15:da88e62863a1 | 218 | if(lineflag == 0 && !sensor_.ballkeepcount){ |
THtakahiro702286 | 15:da88e62863a1 | 219 | theta = ((sensor_.ballAngle>0) - (sensor_.ballAngle<0)) * (2.0/(2-fabs(sensor_.ballAngle)/180) - 1) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2; |
THtakahiro702286 | 15:da88e62863a1 | 220 | omni.computeCircular(/*0*/1.1*sensor_.ballRange / 100.0,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 221 | } |
THtakahiro702286 | 15:da88e62863a1 | 222 | else if(sensor_.ballkeepcount) |
THtakahiro702286 | 15:da88e62863a1 | 223 | { |
THtakahiro702286 | 15:da88e62863a1 | 224 | spin_power = sgoal.compute(); |
THtakahiro702286 | 15:da88e62863a1 | 225 | if(sensor_.team) |
THtakahiro702286 | 15:da88e62863a1 | 226 | { |
THtakahiro702286 | 15:da88e62863a1 | 227 | omni.computeCircular(sensor_.blueRange / 100.0/*0.6*/,sensor_.blueAngle,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 228 | } |
THtakahiro702286 | 15:da88e62863a1 | 229 | else |
THtakahiro702286 | 15:da88e62863a1 | 230 | { |
THtakahiro702286 | 15:da88e62863a1 | 231 | omni.computeCircular(sensor_.yellowRange / 100.0/*0.6*/,sensor_.yellowAngle,spin_power); |
THtakahiro702286 | 15:da88e62863a1 | 232 | } |
THtakahiro702286 | 15:da88e62863a1 | 233 | if(lineflag) kickCheck(); |
THtakahiro702286 | 15:da88e62863a1 | 234 | } |
THtakahiro702286 | 15:da88e62863a1 | 235 | lineflag = 0; |
THtakahiro702286 | 15:da88e62863a1 | 236 | // printf("r:%d %d l:%d %d b:%d f:%d\r\n",sensor_.line[0],sensor_.line[1],sensor_.line[2],sensor_.line[3],sensor_.line[4],sensor_.line[5]); |
THtakahiro702286 | 15:da88e62863a1 | 237 | // printf("%d %d %d %d\r\n",(int)(sensor_.b0),(int)sensor_.tgl1,(int)sensor_.tgl2,(int)sensor_.tgl3); |
THtakahiro702286 | 13:d61a64c81c16 | 238 | for (int i=0; i<4; i++) { |
THtakahiro702286 | 13:d61a64c81c16 | 239 | thisSpeed[i] = omni.wheel[i]; |
THtakahiro702286 | 13:d61a64c81c16 | 240 | } |
THtakahiro702286 | 15:da88e62863a1 | 241 | if(sensor_.b0) sensor_.jy.yawcalibrate(); |
THtakahiro702286 | 15:da88e62863a1 | 242 | if((fabs(sensor_.ballAngle) < 50 && sensor_.ballRange) || sensor_.ballKeep) dribbleCheck(0.6); |
THtakahiro702286 | 15:da88e62863a1 | 243 | else dribbleCheck(0.0); |
THtakahiro702286 | 13:d61a64c81c16 | 244 | motor[0]->setSpeed(thisSpeed[0]); |
THtakahiro702286 | 13:d61a64c81c16 | 245 | motor[1]->setSpeed(thisSpeed[1]); |
THtakahiro702286 | 13:d61a64c81c16 | 246 | motor[2]->setSpeed(-1*thisSpeed[2]); |
THtakahiro702286 | 13:d61a64c81c16 | 247 | motor[3]->setSpeed(-1*thisSpeed[3]); |
THtakahiro702286 | 15:da88e62863a1 | 248 | // thread_sleep_for(40); |
THtakahiro702286 | 13:d61a64c81c16 | 249 | } |
THtakahiro702286 | 13:d61a64c81c16 | 250 | } |