main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
robot.cpp
- Committer:
- THtakahiro702286
- Date:
- 2020-02-01
- Revision:
- 5:f5e79163d0eb
- Parent:
- 4:a4f94f186ba0
- Child:
- 6:dee6041c3d15
- Child:
- 7:dcc0faa86da7
File content as of revision 5:f5e79163d0eb:
#include "robot.h" Robot::Robot() : spin(P, I, D, interval), pc(USBTX, USBRX, 115200), shot(kicker), drib(ESC), but(USER_BUTTON), dip1(dip1in), dip2(dip2in), dip3(dip3in), b1(b1in), b2(b2in), lcd(PB_9, PB_8) { omni.wheel[0].setRadian(PI/4 * 1); omni.wheel[1].setRadian(PI/4 * 3); omni.wheel[2].setRadian(PI/4 * 5); omni.wheel[3].setRadian(PI/4 * 7); motor[0] = new KohiMD(motor1); motor[1] = new KohiMD(motor2); motor[2] = new KohiMD(motor3); motor[3] = new KohiMD(motor4); spin.setInputLimits(-180, 180); spin.setOutputLimits(-0.4,0.4); spin.setBias(0); spin.setMode(1); spin.setSetPoint(0); shot.setkickperiod(0.8); shot.setoutputtime(0.01); drib.setspeed(0.0); but.mode(PullUp); startb = 0; } void Robot::start() { while (true) { 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(dip1) { 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(); 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"); drib.setspeed(0.0); omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power); } pc.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(-omni2wheel); motor[3]->setSpeed(omni.wheel[3]); // motor[3]->setSpeed(-omni3wheel); } else { motor[0]->setSpeed(0); motor[1]->setSpeed(0); motor[2]->setSpeed(0); motor[3]->setSpeed(0); } if (!startb) { if (!b1) { if (dip3) { sensor.jy.jyroReset(); } else { shot.outPut(); } } } } }