Quadrifoglio
main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
Diff: robot.cpp
- Revision:
- 15:da88e62863a1
- Parent:
- 13:d61a64c81c16
- Child:
- 16:e3846421554b
--- a/robot.cpp Fri Mar 05 06:30:37 2021 +0000
+++ b/robot.cpp Sat Mar 06 07:26:44 2021 +0000
@@ -2,6 +2,7 @@
Robot::Robot() :
spin(P, I, D, interval),
+ sgoal(P, I, D, interval),
pc(USBTX, USBRX, 115200),
shot(kicker),
drib(ESCpin)
@@ -16,10 +17,15 @@
motor[2] = new KohiMD(PA_7);
motor[3] = new KohiMD(PB_6);
spin.setInputLimits(-180, 180);
+ sgoal.setInputLimits(-180, 180);
spin.setOutputLimits(-0.4,0.4);
+ sgoal.setOutputLimits(-0.4,0.4);
spin.setBias(0);
+ sgoal.setBias(0);
spin.setMode(1);
+ sgoal.setMode(1);
spin.setSetPoint(0);
+ sgoal.setSetPoint(0);
shot.setkickperiod(2.0);
shot.setoutputtime(0.1);
drib.setspeed(0.0);
@@ -28,45 +34,108 @@
void Robot::start(uint8_t Team, uint8_t Algorithm)
{
-
- while (1) {
- startb=1;
- spin.setProcessValue(sensor_.angleLimit);
- theta = sensor_.ballAngle * ballExtra * PI / 180.0;
+ while(1){
+ printf("%d %d ", (int)sensor_.irVal ,(int)sensor_.ballKeep);
+ spin.setProcessValue(sensor_.angleLimit/*-sensor_.ballAngle*/);
spin_power = spin.compute();
- if (/*sensor_.ballKeep*/0) {
- /*ボールをつかんでるなら*/
-// drib.setspeed(0.5);
- if(Team)
- {
- /*あおに向かうよ*/
-// /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
-// omni.computeCircular(sensor_.blueRange,PI / 180.0 * sensor_.blueAngle, spin_power);
-
- }
- else
- {
- /*きいろに向かうよ*/
-// /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
- }
+
+// theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ;
+// theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1);
+// theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
+ theta = ((sensor_.ballAngle>0) - (sensor_.ballAngle<0)) * (2.0/(2-fabs(sensor_.ballAngle)/180) - 1) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
+ printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 100));
+ omni.computeCircular(/*0*/1.1*sensor_.ballRange / 100.0,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power);
+ for (int i=0; i<4; i++) {
+ thisSpeed[i] = omni.wheel[i];
}
- else
- /*つかんでないなら*/
- {
-// for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
-// pc.printf("\r\n");
- drib.setspeed(0.0);
-// omni.computeCircular(sensor_.ballRange, (PI / 180.0) * sensor_.ballAngle + theta, spin_power);
- omni.computeCircular(sensor_.ballRange, sensor_.ballAngle, spin_power);
- }
-// printf("%d ",(int)sensor.ballKeep);
+ if((fabs(sensor_.ballAngle) < 50 && sensor_.ballRange) || sensor_.ballKeep) dribbleCheck(0.6);
+ else dribbleCheck(0.0);
+ motor[0]->setSpeed(thisSpeed[0]);
+ motor[1]->setSpeed(thisSpeed[1]);
+ motor[2]->setSpeed(-1*thisSpeed[2]);
+ motor[3]->setSpeed(-1*thisSpeed[3]);
+ }
+// 昔
+// while (1) {
+// startb=1;
+// spin.setProcessValue(sensor_.angleLimit);
+// theta = sensor_.ballAngle * ballExtra * PI / 180.0;
+// spin_power = spin.compute();
+// if (/*sensor_.ballKeep*/0) {
+// /*ボールをつかんでるなら*/
+//// drib.setspeed(0.5);
+// if(Team)
+// {
+// /*あおに向かうよ*/
+//// /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
+//// omni.computeCircular(sensor_.blueRange,PI / 180.0 * sensor_.blueAngle, spin_power);
+//
+// }
+// else
+// {
+// /*きいろに向かうよ*/
+//// /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
+// }
+// }
+// else
+// /*つかんでないなら*/
+// {
+//// for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
+//// pc.printf("\r\n");
+// drib.setspeed(0.0);
+//// omni.computeCircular(sensor_.ballRange, (PI / 180.0) * sensor_.ballAngle + theta, spin_power);
+// omni.computeCircular(sensor_.ballRange, sensor_.ballAngle, spin_power);
+// }
+//// printf("%d ",(int)sensor.ballKeep);
+// if(sensor_.line[0] || sensor_.line[1])
+// {
+// omni.computeCircular(1,-1*PI/2.0,spin_power);
+// }
+// if(sensor_.line[2] || sensor_.line[3])
+// {
+// omni.computeCircular(1,PI/2.0,spin_power);
+// }
+// if(sensor_.line[4])
+// {
+// omni.computeCircular(1,0,spin_power);
+// }
+// if(sensor_.line[5])
+// {
+// omni.computeCircular(1,PI,spin_power);
+// }
+//
+// if (startb) {
+// omni2wheel = omni.wheel[2];
+// omni3wheel = omni.wheel[3];
+//// motor[0]->setSpeed(omni.wheel[0]);
+//// motor[1]->setSpeed(omni.wheel[1]);
+//// motor[2]->setSpeed(omni.wheel[2]);
+//// motor[3]->setSpeed(omni.wheel[3]);
+//// motor[3]->setSpeed(-omni3wheel);
+// shot.outPut();
+//
+// } else {
+// motor[0]->setSpeed(0);
+// motor[1]->setSpeed(0);
+// motor[2]->setSpeed(0);
+// motor[3]->setSpeed(0);
+// }
+// }
+}
+
+void Robot::pidtest()
+{
+ while(1){
+ spin.setProcessValue(sensor_.angleLimit);
+ spin_power = spin.compute();
+ omni.computeCircular(0,0,spin_power);
if(sensor_.line[0] || sensor_.line[1])
{
- omni.computeCircular(1,-1*PI/2.0,spin_power);
+ omni.computeCircular(1,PI/2.0,spin_power);
}
if(sensor_.line[2] || sensor_.line[3])
{
- omni.computeCircular(1,PI/2.0,spin_power);
+ omni.computeCircular(1,-1*PI/2.0,spin_power);
}
if(sensor_.line[4])
{
@@ -76,32 +145,6 @@
{
omni.computeCircular(1,PI,spin_power);
}
-
- if (startb) {
- omni2wheel = omni.wheel[2];
- omni3wheel = omni.wheel[3];
-// motor[0]->setSpeed(omni.wheel[0]);
-// motor[1]->setSpeed(omni.wheel[1]);
-// motor[2]->setSpeed(omni.wheel[2]);
-// motor[3]->setSpeed(omni.wheel[3]);
-// motor[3]->setSpeed(-omni3wheel);
- shot.outPut();
-
- } else {
- motor[0]->setSpeed(0);
- motor[1]->setSpeed(0);
- motor[2]->setSpeed(0);
- motor[3]->setSpeed(0);
- }
- }
-}
-
-void Robot::pidtest()
-{
- while(1){
- spin.setProcessValue(sensor_.angleLimit);
- spin_power = spin.compute();
- omni.computeCircular(0,0,spin_power);
for (int i=0; i<4; i++) {
thisSpeed[i] = omni.wheel[i];
}
@@ -142,18 +185,66 @@
void Robot::moveTest()
{
while(1){
- spin.setProcessValue(sensor_.angleLimit);
+// printf("%d %d ", (int)sensor_.irVal ,(int)sensor_.ballKeep);
+ spin.setProcessValue(sensor_.angleLimit/*-sensor_.ballAngle*/);
+ if(sensor_.team) sgoal.setProcessValue(-sensor_.blueAngle/*-sensor_.ballAngle*/);
+ else sgoal.setProcessValue(-sensor_.yellowAngle/*-sensor_.ballAngle*/);
spin_power = spin.compute();
- theta = sensor_.ballAngle * ballExtra * PI / 180.0;
- printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 10));
- omni.computeCircular(sensor_.ballRange / 100.0,PI / 180.0 * sensor_.ballAngle + theta, spin_power);
+// theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ;
+// theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1);
+// theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
+ // printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 100));
+ if(sensor_.line[0] || sensor_.line[1])
+ {
+ omni.computeCircular(1,PI/2.0,spin_power);
+ lineflag = 1;
+ }
+ if(sensor_.line[2] || sensor_.line[3])
+ {
+ omni.computeCircular(1,-1*PI/2.0,spin_power);
+ lineflag = 1;
+ }
+ if(sensor_.line[4])
+ {
+ omni.computeCircular(1,0,spin_power);
+ lineflag = 1;
+ }
+ if(sensor_.line[5])
+ {
+ omni.computeCircular(1,PI,spin_power);
+ lineflag = 1;
+ }
+ if(lineflag == 0 && !sensor_.ballkeepcount){
+ theta = ((sensor_.ballAngle>0) - (sensor_.ballAngle<0)) * (2.0/(2-fabs(sensor_.ballAngle)/180) - 1) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
+ omni.computeCircular(/*0*/1.1*sensor_.ballRange / 100.0,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power);
+ }
+ else if(sensor_.ballkeepcount)
+ {
+ spin_power = sgoal.compute();
+ if(sensor_.team)
+ {
+ omni.computeCircular(sensor_.blueRange / 100.0/*0.6*/,sensor_.blueAngle,spin_power);
+ }
+ else
+ {
+ omni.computeCircular(sensor_.yellowRange / 100.0/*0.6*/,sensor_.yellowAngle,spin_power);
+ }
+ if(lineflag) kickCheck();
+ }
+ lineflag = 0;
+// 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]);
+// printf("%d %d %d %d\r\n",(int)(sensor_.b0),(int)sensor_.tgl1,(int)sensor_.tgl2,(int)sensor_.tgl3);
for (int i=0; i<4; i++) {
thisSpeed[i] = omni.wheel[i];
}
+ if(sensor_.b0) sensor_.jy.yawcalibrate();
+ if((fabs(sensor_.ballAngle) < 50 && sensor_.ballRange) || sensor_.ballKeep) dribbleCheck(0.6);
+ else dribbleCheck(0.0);
motor[0]->setSpeed(thisSpeed[0]);
motor[1]->setSpeed(thisSpeed[1]);
motor[2]->setSpeed(-1*thisSpeed[2]);
motor[3]->setSpeed(-1*thisSpeed[3]);
+// thread_sleep_for(40);
}
}
\ No newline at end of file