Quadrifoglio / Robot

main

Dependents:   00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange

robot.cpp@14:8dde838ce79e, 2021-03-05 (annotated)

Committer:
THtakahiro702286
Date:
Fri Mar 05 14:01:03 2021 +0000
Revision:
14:8dde838ce79e
Parent:
13:d61a64c81c16 
uuum

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),
piroro4560 1:6a5065829cfc 5 pc(USBTX, USBRX, 115200),
piroro4560 1:6a5065829cfc 6 shot(kicker),
piroro4560 9:904dac75a729 7 drib(ESCpin)
piroro4560 0:3f87ec23c3cf 8 {
piroro4560 4:a4f94f186ba0 9
THtakahiro702286 13:d61a64c81c16 10 omni.wheel[2].setRadian(PI/4 * 1);
THtakahiro702286 13:d61a64c81c16 11 omni.wheel[3].setRadian(PI/4 * 3);
THtakahiro702286 13:d61a64c81c16 12 omni.wheel[0].setRadian(PI/4 * 5);
THtakahiro702286 13:d61a64c81c16 13 omni.wheel[1].setRadian(PI/4 * 7);
piroro4560 9:904dac75a729 14 motor[0] = new KohiMD(PA_15);
piroro4560 9:904dac75a729 15 motor[1] = new KohiMD(PA_6);
piroro4560 9:904dac75a729 16 motor[2] = new KohiMD(PA_7);
piroro4560 9:904dac75a729 17 motor[3] = new KohiMD(PB_6);
piroro4560 1:6a5065829cfc 18 spin.setInputLimits(-180, 180);
THtakahiro702286 5:f5e79163d0eb 19 spin.setOutputLimits(-0.4,0.4);
piroro4560 1:6a5065829cfc 20 spin.setBias(0);
piroro4560 1:6a5065829cfc 21 spin.setMode(1);
piroro4560 1:6a5065829cfc 22 spin.setSetPoint(0);
piroro4560 9:904dac75a729 23 shot.setkickperiod(2.0);
piroro4560 9:904dac75a729 24 shot.setoutputtime(0.1);
piroro4560 4:a4f94f186ba0 25 drib.setspeed(0.0);
THtakahiro702286 5:f5e79163d0eb 26 startb = 0;
piroro4560 0:3f87ec23c3cf 27 }
piroro4560 0:3f87ec23c3cf 28
piroro4560 9:904dac75a729 29 void Robot::start(uint8_t Team, uint8_t Algorithm)
piroro4560 0:3f87ec23c3cf 30 {
piroro4560 4:a4f94f186ba0 31
piroro4560 9:904dac75a729 32 while (1) {
piroro4560 9:904dac75a729 33 startb=1;
piroro4560 9:904dac75a729 34 spin.setProcessValue(sensor_.angleLimit);
piroro4560 9:904dac75a729 35 theta = sensor_.ballAngle * ballExtra * PI / 180.0;
piroro4560 4:a4f94f186ba0 36 spin_power = spin.compute();
piroro4560 12:19149697667d 37 if (/*sensor_.ballKeep*/0) {
piroro4560 9:904dac75a729 38 /*ボールをつかんでるなら*/
THtakahiro702286 7:dcc0faa86da7 39 // drib.setspeed(0.5);
piroro4560 9:904dac75a729 40 if(Team)
piroro4560 4:a4f94f186ba0 41 {
piroro4560 9:904dac75a729 42 /*あおに向かうよ*/
piroro4560 12:19149697667d 43 // /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
piroro4560 12:19149697667d 44 // omni.computeCircular(sensor_.blueRange,PI / 180.0 * sensor_.blueAngle, spin_power);
piroro4560 4:a4f94f186ba0 45
piroro4560 4:a4f94f186ba0 46 }
piroro4560 4:a4f94f186ba0 47 else
piroro4560 4:a4f94f186ba0 48 {
piroro4560 9:904dac75a729 49 /*きいろに向かうよ*/
piroro4560 12:19149697667d 50 // /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
piroro4560 4:a4f94f186ba0 51 }
piroro4560 9:904dac75a729 52 }
piroro4560 4:a4f94f186ba0 53 else
piroro4560 9:904dac75a729 54 /*つかんでないなら*/
piroro4560 4:a4f94f186ba0 55 {
piroro4560 9:904dac75a729 56 // for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
piroro4560 9:904dac75a729 57 // pc.printf("\r\n");
THtakahiro702286 5:f5e79163d0eb 58 drib.setspeed(0.0);
piroro4560 12:19149697667d 59 // omni.computeCircular(sensor_.ballRange, (PI / 180.0) * sensor_.ballAngle + theta, spin_power);
piroro4560 12:19149697667d 60 omni.computeCircular(sensor_.ballRange, sensor_.ballAngle, spin_power);
piroro4560 4:a4f94f186ba0 61 }
piroro4560 9:904dac75a729 62 // printf("%d ",(int)sensor.ballKeep);
piroro4560 9:904dac75a729 63 if(sensor_.line[0] || sensor_.line[1])
piroro4560 4:a4f94f186ba0 64 {
piroro4560 4:a4f94f186ba0 65 omni.computeCircular(1,-1*PI/2.0,spin_power);
piroro4560 4:a4f94f186ba0 66 }
piroro4560 9:904dac75a729 67 if(sensor_.line[2] || sensor_.line[3])
piroro4560 4:a4f94f186ba0 68 {
piroro4560 4:a4f94f186ba0 69 omni.computeCircular(1,PI/2.0,spin_power);
piroro4560 4:a4f94f186ba0 70 }
piroro4560 9:904dac75a729 71 if(sensor_.line[4])
piroro4560 4:a4f94f186ba0 72 {
piroro4560 4:a4f94f186ba0 73 omni.computeCircular(1,0,spin_power);
piroro4560 4:a4f94f186ba0 74 }
piroro4560 9:904dac75a729 75 if(sensor_.line[5])
piroro4560 4:a4f94f186ba0 76 {
piroro4560 4:a4f94f186ba0 77 omni.computeCircular(1,PI,spin_power);
piroro4560 4:a4f94f186ba0 78 }
piroro4560 4:a4f94f186ba0 79
piroro4560 4:a4f94f186ba0 80 if (startb) {
piroro4560 4:a4f94f186ba0 81 omni2wheel = omni.wheel[2];
THtakahiro702286 5:f5e79163d0eb 82 omni3wheel = omni.wheel[3];
piroro4560 9:904dac75a729 83 // motor[0]->setSpeed(omni.wheel[0]);
piroro4560 9:904dac75a729 84 // motor[1]->setSpeed(omni.wheel[1]);
piroro4560 9:904dac75a729 85 // motor[2]->setSpeed(omni.wheel[2]);
piroro4560 9:904dac75a729 86 // motor[3]->setSpeed(omni.wheel[3]);
THtakahiro702286 5:f5e79163d0eb 87 // motor[3]->setSpeed(-omni3wheel);
THtakahiro702286 7:dcc0faa86da7 88 shot.outPut();
THtakahiro702286 7:dcc0faa86da7 89
piroro4560 4:a4f94f186ba0 90 } else {
piroro4560 12:19149697667d 91 motor[0]->setSpeed(0);
piroro4560 12:19149697667d 92 motor[1]->setSpeed(0);
piroro4560 12:19149697667d 93 motor[2]->setSpeed(0);
piroro4560 12:19149697667d 94 motor[3]->setSpeed(0);
piroro4560 4:a4f94f186ba0 95 }
piroro4560 2:fc5545ddf69a 96 }
piroro4560 0:3f87ec23c3cf 97 }
THtakahiro702286 8:713bbc1fb58f 98
piroro4560 12:19149697667d 99 void Robot::pidtest()
piroro4560 12:19149697667d 100 {
piroro4560 12:19149697667d 101 while(1){
piroro4560 12:19149697667d 102 spin.setProcessValue(sensor_.angleLimit);
piroro4560 12:19149697667d 103 spin_power = spin.compute();
piroro4560 12:19149697667d 104 omni.computeCircular(0,0,spin_power);
THtakahiro702286 14:8dde838ce79e 105 if(sensor_.line[0] || sensor_.line[1])
THtakahiro702286 14:8dde838ce79e 106 {
THtakahiro702286 14:8dde838ce79e 107 omni.computeCircular(1,-1*PI/2.0,spin_power);
THtakahiro702286 14:8dde838ce79e 108 }
THtakahiro702286 14:8dde838ce79e 109 if(sensor_.line[2] || sensor_.line[3])
THtakahiro702286 14:8dde838ce79e 110 {
THtakahiro702286 14:8dde838ce79e 111 omni.computeCircular(1,PI/2.0,spin_power);
THtakahiro702286 14:8dde838ce79e 112 }
THtakahiro702286 14:8dde838ce79e 113 if(sensor_.line[4])
THtakahiro702286 14:8dde838ce79e 114 {
THtakahiro702286 14:8dde838ce79e 115 omni.computeCircular(1,0,spin_power);
THtakahiro702286 14:8dde838ce79e 116 }
THtakahiro702286 14:8dde838ce79e 117 if(sensor_.line[5])
THtakahiro702286 14:8dde838ce79e 118 {
THtakahiro702286 14:8dde838ce79e 119 omni.computeCircular(1,PI,spin_power);
THtakahiro702286 14:8dde838ce79e 120 }
piroro4560 12:19149697667d 121 for (int i=0; i<4; i++) {
piroro4560 12:19149697667d 122 thisSpeed[i] = omni.wheel[i];
piroro4560 12:19149697667d 123 }
piroro4560 12:19149697667d 124 motor[0]->setSpeed(thisSpeed[0]);
piroro4560 12:19149697667d 125 motor[1]->setSpeed(thisSpeed[1]);
piroro4560 12:19149697667d 126 motor[2]->setSpeed(-1*thisSpeed[2]);
piroro4560 12:19149697667d 127 motor[3]->setSpeed(-1*thisSpeed[3]);
piroro4560 12:19149697667d 128 }
piroro4560 12:19149697667d 129 }
piroro4560 12:19149697667d 130
THtakahiro702286 8:713bbc1fb58f 131 void Robot::motorCheck(int motorNumber, float power)
THtakahiro702286 8:713bbc1fb58f 132 {
THtakahiro702286 8:713bbc1fb58f 133 _motorNumber = motorNumber;
piroro4560 9:904dac75a729 134 for(i = 0; i < 4; i++)
THtakahiro702286 8:713bbc1fb58f 135 {
piroro4560 9:904dac75a729 136 if(i == _motorNumber) motor[i]->setSpeed(power);
piroro4560 9:904dac75a729 137 else motor[i]->setSpeed(0);
THtakahiro702286 8:713bbc1fb58f 138 }
THtakahiro702286 8:713bbc1fb58f 139 }
THtakahiro702286 8:713bbc1fb58f 140
piroro4560 9:904dac75a729 141 void Robot::motorStop(double pwm)
THtakahiro702286 8:713bbc1fb58f 142 {
THtakahiro702286 11:39f44a1dc256 143 // motorSpeed = 0.5;
THtakahiro702286 11:39f44a1dc256 144 // printf("%d\r\n", (int)(motorSpeed*10));
THtakahiro702286 11:39f44a1dc256 145 for(i=0; i<4; i++) motor[i]->setSpeed(pwm);
THtakahiro702286 8:713bbc1fb58f 146 }
THtakahiro702286 8:713bbc1fb58f 147
THtakahiro702286 8:713bbc1fb58f 148 void Robot::kickCheck()
THtakahiro702286 8:713bbc1fb58f 149 {
THtakahiro702286 8:713bbc1fb58f 150 shot.outPut();
THtakahiro702286 8:713bbc1fb58f 151 }
THtakahiro702286 8:713bbc1fb58f 152
THtakahiro702286 8:713bbc1fb58f 153 void Robot::dribbleCheck(float power)
THtakahiro702286 8:713bbc1fb58f 154 {
THtakahiro702286 8:713bbc1fb58f 155 drib.setspeed(power);
THtakahiro702286 8:713bbc1fb58f 156 }
THtakahiro702286 8:713bbc1fb58f 157
THtakahiro702286 13:d61a64c81c16 158 void Robot::moveTest()
THtakahiro702286 13:d61a64c81c16 159 {
THtakahiro702286 13:d61a64c81c16 160 while(1){
THtakahiro702286 14:8dde838ce79e 161 printf("%d", (int)sensor_.irVal * 100);
THtakahiro702286 13:d61a64c81c16 162 spin.setProcessValue(sensor_.angleLimit);
THtakahiro702286 13:d61a64c81c16 163 spin_power = spin.compute();
THtakahiro702286 13:d61a64c81c16 164
THtakahiro702286 14:8dde838ce79e 165 // theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ;
THtakahiro702286 14:8dde838ce79e 166 // theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1);
THtakahiro702286 14:8dde838ce79e 167 theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
THtakahiro702286 14:8dde838ce79e 168 printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(fabs(sensor_.ballAngle) > 25.0));
THtakahiro702286 13:d61a64c81c16 169 omni.computeCircular(sensor_.ballRange / 100.0,PI / 180.0 * sensor_.ballAngle + theta, spin_power);
THtakahiro702286 13:d61a64c81c16 170 for (int i=0; i<4; i++) {
THtakahiro702286 13:d61a64c81c16 171 thisSpeed[i] = omni.wheel[i];
THtakahiro702286 13:d61a64c81c16 172 }
THtakahiro702286 13:d61a64c81c16 173 motor[0]->setSpeed(thisSpeed[0]);
THtakahiro702286 13:d61a64c81c16 174 motor[1]->setSpeed(thisSpeed[1]);
THtakahiro702286 13:d61a64c81c16 175 motor[2]->setSpeed(-1*thisSpeed[2]);
THtakahiro702286 13:d61a64c81c16 176 motor[3]->setSpeed(-1*thisSpeed[3]);
THtakahiro702286 13:d61a64c81c16 177 }
THtakahiro702286 13:d61a64c81c16 178 }