Quadrifoglio / Robot

main

Dependents:   00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange

robot.cpp@12:19149697667d, 2021-03-05 (annotated)

Committer:
piroro4560
Date:
Fri Mar 05 05:02:32 2021 +0000
Revision:
12:19149697667d
Parent:
11:39f44a1dc256
Child:
13:d61a64c81c16
can move

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
piroro4560 0:3f87ec23c3cf 10 omni.wheel[0].setRadian(PI/4 * 1);
piroro4560 0:3f87ec23c3cf 11 omni.wheel[1].setRadian(PI/4 * 3);
piroro4560 0:3f87ec23c3cf 12 omni.wheel[2].setRadian(PI/4 * 5);
piroro4560 0:3f87ec23c3cf 13 omni.wheel[3].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);
piroro4560 12:19149697667d 105 for (int i=0; i<4; i++) {
piroro4560 12:19149697667d 106 thisSpeed[i] = omni.wheel[i];
piroro4560 12:19149697667d 107 }
piroro4560 12:19149697667d 108 motor[0]->setSpeed(thisSpeed[0]);
piroro4560 12:19149697667d 109 motor[1]->setSpeed(thisSpeed[1]);
piroro4560 12:19149697667d 110 motor[2]->setSpeed(-1*thisSpeed[2]);
piroro4560 12:19149697667d 111 motor[3]->setSpeed(-1*thisSpeed[3]);
piroro4560 12:19149697667d 112 }
piroro4560 12:19149697667d 113 }
piroro4560 12:19149697667d 114
THtakahiro702286 8:713bbc1fb58f 115 void Robot::motorCheck(int motorNumber, float power)
THtakahiro702286 8:713bbc1fb58f 116 {
THtakahiro702286 8:713bbc1fb58f 117 _motorNumber = motorNumber;
piroro4560 9:904dac75a729 118 for(i = 0; i < 4; i++)
THtakahiro702286 8:713bbc1fb58f 119 {
piroro4560 9:904dac75a729 120 if(i == _motorNumber) motor[i]->setSpeed(power);
piroro4560 9:904dac75a729 121 else motor[i]->setSpeed(0);
THtakahiro702286 8:713bbc1fb58f 122 }
THtakahiro702286 8:713bbc1fb58f 123 }
THtakahiro702286 8:713bbc1fb58f 124
piroro4560 9:904dac75a729 125 void Robot::motorStop(double pwm)
THtakahiro702286 8:713bbc1fb58f 126 {
THtakahiro702286 11:39f44a1dc256 127 // motorSpeed = 0.5;
THtakahiro702286 11:39f44a1dc256 128 // printf("%d\r\n", (int)(motorSpeed*10));
THtakahiro702286 11:39f44a1dc256 129 for(i=0; i<4; i++) motor[i]->setSpeed(pwm);
THtakahiro702286 8:713bbc1fb58f 130 }
THtakahiro702286 8:713bbc1fb58f 131
THtakahiro702286 8:713bbc1fb58f 132 void Robot::kickCheck()
THtakahiro702286 8:713bbc1fb58f 133 {
THtakahiro702286 8:713bbc1fb58f 134 shot.outPut();
THtakahiro702286 8:713bbc1fb58f 135 }
THtakahiro702286 8:713bbc1fb58f 136
THtakahiro702286 8:713bbc1fb58f 137 void Robot::dribbleCheck(float power)
THtakahiro702286 8:713bbc1fb58f 138 {
THtakahiro702286 8:713bbc1fb58f 139 drib.setspeed(power);
THtakahiro702286 8:713bbc1fb58f 140 }
THtakahiro702286 8:713bbc1fb58f 141
THtakahiro702286 8:713bbc1fb58f 142 // theta = sensor.ballAngle * ballExtra * PI / 180.0;
THtakahiro702286 8:713bbc1fb58f 143 // omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power);