Quadrifoglio / Robot

main

Dependents:   00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange

robot.cpp@16:e3846421554b, 2021-03-07 (annotated)

Committer:
THtakahiro702286
Date:
Sun Mar 07 12:23:16 2021 +0000
Revision:
16:e3846421554b
Parent:
15:da88e62863a1 
victory!!!!

Who changed what in which revision?

UserRevisionLine numberNew 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 16:e3846421554b 218
THtakahiro702286 16:e3846421554b 219 if(lineflag == 0 && (!sensor_.ballkeepcount || sensor_.ballRange)){
THtakahiro702286 15:da88e62863a1 220 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 16:e3846421554b 221 omni.computeCircular(/*0*/ 0.5,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power);
THtakahiro702286 15:da88e62863a1 222 }
THtakahiro702286 16:e3846421554b 223
THtakahiro702286 16:e3846421554b 224 if(lineflag == 0 && sensor_.ballkeepcount)
THtakahiro702286 15:da88e62863a1 225 {
THtakahiro702286 16:e3846421554b 226 // spin_power = sgoal.compute();
THtakahiro702286 15:da88e62863a1 227 if(sensor_.team)
THtakahiro702286 15:da88e62863a1 228 {
THtakahiro702286 16:e3846421554b 229 // spin_power = sensor_.blueAngle / 180.0 * 0.4;
THtakahiro702286 16:e3846421554b 230 omni.computeCircular(/*sensor_.blueRange / 100.0*//*0.45*/0.45,PI / 180.0 * sensor_.blueAngle,spin_power);
THtakahiro702286 15:da88e62863a1 231 }
THtakahiro702286 15:da88e62863a1 232 else
THtakahiro702286 15:da88e62863a1 233 {
THtakahiro702286 16:e3846421554b 234 // spin_power = sensor_.yellowAngle / 180.0 * 0.4;
THtakahiro702286 16:e3846421554b 235 omni.computeCircular(/*sensor_.yellowRange / 100.0*/0.45,PI / 180.0 * sensor_.yellowAngle,spin_power);
THtakahiro702286 15:da88e62863a1 236 }
THtakahiro702286 15:da88e62863a1 237 }
THtakahiro702286 16:e3846421554b 238
THtakahiro702286 16:e3846421554b 239 if(lineflag == 0 && !sensor_.ballkeepcount && !sensor_.ballRange)
THtakahiro702286 16:e3846421554b 240 {
THtakahiro702286 16:e3846421554b 241 if(sensor_.team)
THtakahiro702286 16:e3846421554b 242 {
THtakahiro702286 16:e3846421554b 243 omni.computeCircular(0.4,PI / 180.0 * sensor_.yellowAngle,spin_power);
THtakahiro702286 16:e3846421554b 244 }
THtakahiro702286 16:e3846421554b 245 else
THtakahiro702286 16:e3846421554b 246 {
THtakahiro702286 16:e3846421554b 247 omni.computeCircular(0.4,PI / 180.0 * sensor_.blueAngle,spin_power);
THtakahiro702286 16:e3846421554b 248 }
THtakahiro702286 16:e3846421554b 249 }
THtakahiro702286 16:e3846421554b 250 if((lineflag || (sensor_.yellowRange < 60)) && sensor_.ballkeepcount) kickCheck();
THtakahiro702286 15:da88e62863a1 251 lineflag = 0;
THtakahiro702286 15:da88e62863a1 252 // 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 16:e3846421554b 253 // printf("%d %d %d %d\r\n",(int)(sensor_.b0),(int)sensor_.test,(int)sensor_.team,(int)sensor_.start);
THtakahiro702286 13:d61a64c81c16 254 for (int i=0; i<4; i++) {
THtakahiro702286 13:d61a64c81c16 255 thisSpeed[i] = omni.wheel[i];
THtakahiro702286 13:d61a64c81c16 256 }
THtakahiro702286 15:da88e62863a1 257 if(sensor_.b0) sensor_.jy.yawcalibrate();
THtakahiro702286 15:da88e62863a1 258 if((fabs(sensor_.ballAngle) < 50 && sensor_.ballRange) || sensor_.ballKeep) dribbleCheck(0.6);
THtakahiro702286 15:da88e62863a1 259 else dribbleCheck(0.0);
THtakahiro702286 13:d61a64c81c16 260 motor[0]->setSpeed(thisSpeed[0]);
THtakahiro702286 13:d61a64c81c16 261 motor[1]->setSpeed(thisSpeed[1]);
THtakahiro702286 13:d61a64c81c16 262 motor[2]->setSpeed(-1*thisSpeed[2]);
THtakahiro702286 13:d61a64c81c16 263 motor[3]->setSpeed(-1*thisSpeed[3]);
THtakahiro702286 15:da88e62863a1 264 // thread_sleep_for(40);
THtakahiro702286 13:d61a64c81c16 265 }
THtakahiro702286 13:d61a64c81c16 266 }