main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
Diff: robot.cpp
- Revision:
- 6:dee6041c3d15
- Parent:
- 5:f5e79163d0eb
diff -r f5e79163d0eb -r dee6041c3d15 robot.cpp --- a/robot.cpp Sat Feb 01 03:02:18 2020 +0000 +++ b/robot.cpp Sat Feb 01 15:12:19 2020 +0000 @@ -34,65 +34,126 @@ drib.setspeed(0.0); but.mode(PullUp); startb = 0; + linecount[0] = 0; + linecount[1] = 0; + linecount[2] = 0; + linecount[3] = 0; + lineAngle = 0; } void Robot::start() { while (true) { - if (!but/*2*/) startb=1; + pc.printf("%f\r\n", sensor.angle ); +// sensor.blueAngle, sensor.blueRange/*, sensor.yellowAngle, sensor.yellowRange, sensor.ballAngle, sensor.ballRange */); + if (!/*but*/2) startb=1; spin.setProcessValue(sensor.angleLimit); theta = sensor.ballAngle * ballExtra * PI / 180.0; spin_power = spin.compute(); - if (sensor.ballKeep) { - drib.setspeed(0.5); + if (sensor.ballkeepcount > 100) { + drib.setspeed(0.5); + shot.outPut(); if(dip1) { - shot.outPut(); +// shot.outPut(); if(!sensor.bluex) omni.computeCircular(1,0, spin_power); else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power); - } else { - shot.outPut(); +// shot.outPut(); if(!sensor.yellowx) omni.computeCircular(1,0, spin_power); else omni.computeCircular(sensor.yellowRange,PI / 180.0 * sensor.yellowAngle, spin_power); } } else { - for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]); - pc.printf("\r\n"); +// 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); } - pc.printf("%d ",(int)sensor.ballKeep); + +// pc.printf("%d ",(int)sensor.ballKeep); + + if (sensor.ballTimeout > 1.0 && sensor.ballTimeout < 2.0) + { + omni.computeCircular(1,PI,spin_power); + } + if(sensor.line[0] || sensor.line[1]) { - omni.computeCircular(1,-1*PI/2.0,spin_power); - + linecount[0]+=2; } + else + { + linecount[0]--; + } + if(linecount[0] && !(linecount[1] + linecount[2] + linecount[3]) ) lineAngle = -1*PI/2.0; if(sensor.line[2] || sensor.line[3]) { - omni.computeCircular(1,PI/2.0,spin_power); + linecount[1]+=2; } + else + { + linecount[1]--; + } + if(linecount[1] && !(linecount[0] + linecount[2] + linecount[3])) lineAngle = PI/2; if(sensor.line[4]) { - omni.computeCircular(1,0,spin_power); + linecount[2]+=2; } + else + { + linecount[2]--; + } + if(linecount[2] && !(linecount[1] + linecount[0] + linecount[3])) lineAngle = 0; if(sensor.line[5]) { - omni.computeCircular(1,PI,spin_power); + linecount[3]+=2; + } + else + { + linecount[3]-=2; + } + if(linecount[3] && !(linecount[1] + linecount[2] + linecount[0])) lineAngle = PI; + + if(!(linecount[0] + linecount[1] + linecount[2] + linecount[3])) lineAngle = 0; + + for(int i = 0; i < 4; i++) + { + if(linecount[i] > 50) linecount[i] = 50; + if(linecount[i] < 0) linecount[i] = 0; } + if(linecount[0] + linecount[1] + linecount[2] + linecount[3]) omni.computeCircular(1,lineAngle,spin_power); + //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) { + omni0wheel = omni.wheel[0]; + omni1wheel = omni.wheel[1]; omni2wheel = omni.wheel[2]; omni3wheel = omni.wheel[3]; - motor[0]->setSpeed(omni.wheel[0]); - motor[1]->setSpeed(omni.wheel[1]); - motor[2]->setSpeed(-omni2wheel); - motor[3]->setSpeed(omni.wheel[3]); + motor[0]->setSpeed(omni0wheel); + motor[1]->setSpeed(omni1wheel); + motor[2]->setSpeed(omni2wheel);/////////////////////////////////// + motor[3]->setSpeed(omni3wheel); // motor[3]->setSpeed(-omni3wheel); } else { motor[0]->setSpeed(0);