main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
robot.cpp
- Committer:
- THtakahiro702286
- Date:
- 2021-03-05
- Revision:
- 14:8dde838ce79e
- Parent:
- 13:d61a64c81c16
File content as of revision 14:8dde838ce79e:
#include "robot.h" Robot::Robot() : spin(P, I, D, interval), pc(USBTX, USBRX, 115200), shot(kicker), drib(ESCpin) { omni.wheel[2].setRadian(PI/4 * 1); omni.wheel[3].setRadian(PI/4 * 3); omni.wheel[0].setRadian(PI/4 * 5); omni.wheel[1].setRadian(PI/4 * 7); motor[0] = new KohiMD(PA_15); motor[1] = new KohiMD(PA_6); motor[2] = new KohiMD(PA_7); motor[3] = new KohiMD(PB_6); spin.setInputLimits(-180, 180); spin.setOutputLimits(-0.4,0.4); spin.setBias(0); spin.setMode(1); spin.setSetPoint(0); shot.setkickperiod(2.0); shot.setoutputtime(0.1); drib.setspeed(0.0); startb = 0; } void Robot::start(uint8_t Team, uint8_t Algorithm) { 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); } 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); } for (int i=0; i<4; i++) { thisSpeed[i] = omni.wheel[i]; } motor[0]->setSpeed(thisSpeed[0]); motor[1]->setSpeed(thisSpeed[1]); motor[2]->setSpeed(-1*thisSpeed[2]); motor[3]->setSpeed(-1*thisSpeed[3]); } } void Robot::motorCheck(int motorNumber, float power) { _motorNumber = motorNumber; for(i = 0; i < 4; i++) { if(i == _motorNumber) motor[i]->setSpeed(power); else motor[i]->setSpeed(0); } } void Robot::motorStop(double pwm) { // motorSpeed = 0.5; // printf("%d\r\n", (int)(motorSpeed*10)); for(i=0; i<4; i++) motor[i]->setSpeed(pwm); } void Robot::kickCheck() { shot.outPut(); } void Robot::dribbleCheck(float power) { drib.setspeed(power); } void Robot::moveTest() { while(1){ printf("%d", (int)sensor_.irVal * 100); spin.setProcessValue(sensor_.angleLimit); spin_power = spin.compute(); // 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)(fabs(sensor_.ballAngle) > 25.0)); omni.computeCircular(sensor_.ballRange / 100.0,PI / 180.0 * sensor_.ballAngle + theta, spin_power); for (int i=0; i<4; i++) { thisSpeed[i] = omni.wheel[i]; } motor[0]->setSpeed(thisSpeed[0]); motor[1]->setSpeed(thisSpeed[1]); motor[2]->setSpeed(-1*thisSpeed[2]); motor[3]->setSpeed(-1*thisSpeed[3]); } }