前のやつです。
Dependencies: FEP OmniPosition PID QEI R1307 TFmini ikarashiMDC linesSnsor omni_wheel
gakuBot/gakubot.cpp
- Committer:
- tanabe2000
- Date:
- 2018-09-11
- Revision:
- 5:c7643ae5835f
- Parent:
- 3:8f4c81ad256a
File content as of revision 5:c7643ae5835f:
#include "gakubot.h" GakuBot::GakuBot() : led(LED2), pid(KP,KI,KD,RATE), anglePID(angleKP, angleKI, angleKD, RATE), omni(4), angle1_1(air1_0), angle1_2(air1_1), angle2_1(air2_0),angle2_2(air2_1), con(XBee2TX, XBee2RX, ADDR), wheel1 (PA_11,PA_12,NC, 2048, QEI::X4_ENCODING), // r1370(R1370TX, R1370RX), RS485control(PA_4), RS485(MDTX,MDRX,115200), debugpc(USBTX,USBRX,115200), receiveSuccessed(0) { for(int i = 0; i < 4; i++) stick[i] = 0; for(int i = 0; i < 3; i++) speed[i] = 0, firePwm[i] = 0; // for(int i = 0; i < 5; i++) loadingPwm[i] = 0; Output_PID = 0; nowAngle = 0; ofsetNowAngle = 0; airFlag = 0; airFlag2 = 0; airStatus = 0; airStatus2 = 0; distance = 800; distanceOfset = 0; nowPals = 0; mode = 0; demoX = 0; demoY = 0; dt = 0; attachAngle = 0; wheels[0] = new ikarashiMDC(&RS485control,0,0,SM,&RS485); wheels[1] = new ikarashiMDC(&RS485control,0,1,SM,&RS485); wheels[2] = new ikarashiMDC(&RS485control,0,2,SM,&RS485); wheels[3] = new ikarashiMDC(&RS485control,0,3,SM,&RS485); fire[0] = new ikarashiMDC(&RS485control,1,0,SM,&RS485); fire[1] = new ikarashiMDC(&RS485control,1,1,SM,&RS485); // fire[2] = new ikarashiMDC(&RS485control,1,2,SM,&RS485); // Loading[0] = new ikarashiMDC(&RS485control,2,0,SM,&RS485); // Loading[1] = new ikarashiMDC(&RS485control,2,1,SM,&RS485); // Loading[2] = new ikarashiMDC(&RS485control,2,2,SM,&RS485); // Loading[3] = new ikarashiMDC(&RS485control,2,3,SM,&RS485); // Loading[4] = new ikarashiMDC(&RS485control,1,3,SM,&RS485); omni.wheel[0].setRadian(PI / 4.0 * 1.0); omni.wheel[1].setRadian(PI / 4.0 * 3.0); omni.wheel[2].setRadian(PI / 4.0 * 5.0); omni.wheel[3].setRadian(PI / 4.0 * 7.0); for(int i = 0; i < 4; i++) { wheels[i]->braking = true; } for(int i = 0; i < 2; i++) { fire[i]->braking = true; } //for(int i = 0; i < 5; i++) { // Loading[i]->braking = true; // } pid.setMode(AUTO_MODE); pid.setInputLimits(-100, 2300); pid.setOutputLimits(-1.0, 1.0); pid.setSetPoint(distance); pid.setBias(0.0); anglePID.setMode(AUTO_MODE); anglePID.setInputLimits(-360, 360); anglePID.setOutputLimits(-1.0, 1.0); anglePID.setMode(AUTO_MODE); anglePID.setSetPoint(0.0); anglePID.setBias(0.0); t.start(); confirmT.start(); } void GakuBot::botConfirm() { if(con.getButton2(2)==0) distance += 10; if(con.getButton2(3)==0) distance -= 10; receiveSuccessed = con.receiveState(); nowPals = wheel1.getPulses(); pid.setSetPoint(distance); pid.setProcessValue(nowPals - distanceOfset); Output_PID = -1*pid.compute(); // // //r1370.update(); //// nowAngle = r1370.getRate(); // nowAngle = 0.0; // anglePID.setSetPoint(attachAngle); // anglePID.setProcessValue(nowAngle - ofsetNowAngle); } //void GakuBot::controllerMode1() //{ // if(receiveSuccessed) { //// for(int i = 0; i < 3; i++) fire[i]->setSpeed(0); // for(int i = 0; i < 4; i++) stick[i] = con.getStick(i); // // if((-0.1 < stick[2] )&& (stick[2] < 0.1)) omni.computeXY(stick[0]/3,stick[1]/3, -1*anglePID.compute()); // else { // omni.computeXY(stick[0]/3,stick[1]/3, -1*stick[2]/4.0); // ofsetNowAngle = nowAngle; // attachAngle = 0; // } // // // confirmDt = confirmT.read(); // if((con.getButton1(0)==0) && (confirmDt >= 0.5))attachAngle += 10, confirmT.reset(); // if((con.getButton1(1)==0) && (confirmDt >= 0.5))attachAngle -= 10, confirmT.reset(); // debugpc.printf("attach = %f\r\n", attachAngle); // // for (int i = 0; i < 4; i++) { // speed[i] = omni.wheel[i]; // wheels[i]->setSpeed(speed[i]); //// pc.printf("%1d: %.3f, ", i, speed[i]); // } // } else { // // for (int i = 0; i < 4; i++) { // wheels[i]->setSpeed(0); // } // } //} void GakuBot::controllerMode2() { if(receiveSuccessed) { // for(int i = 0; i < 3; i++) fire[i]->setSpeed(0); for(int i = 0; i < 4; i++) stick[i] = con.getStick(i); omni.computeXY(stick[0]/3.0 ,stick[1]/3.0 , -1*stick[2]/5.0); for (int i = 0; i < 4; i++) { speed[i] = omni.wheel[i]; wheels[i]->setSpeed(speed[i]); // debugpc.printf("%1d: %.3f, ", i, speed[i]); } } else { debugpc.printf("error\r\n"); for (int i = 0; i < 4; i++) { wheels[i]->setSpeed(0); } } } void GakuBot::controllMech() { if(receiveSuccessed) { debugpc.printf("pals = %d,distance = %d, distanceOfset = %d", nowPals - distanceOfset,distance,distanceOfset); if((con.getButton1(5)==0)&&(airFlag == 0)) { if(airStatus==1) { angle1_1=0; angle1_2=1; airFlag=1; airStatus=0; led = 0; // wait(0.01); } else if(airStatus==0) { angle1_1=1; angle1_2=0; airFlag=1; airStatus=1; led = 1; distanceOfset = nowPals; // wait(0.01); } // fire[0]->setSpeed(0.0); } else { airFlag=0; angle1_1=0; angle1_2=0; // led = 0; if(airStatus == 1) { if(con.getButton1(3)==0) { // distance = 970; firePwm[0] = Output_PID; } else { // distance = nowPals; if(con.getButton1(2)==0) firePwm[0] = 0.9; if(con.getButton1(6)==0) firePwm[0] = -0.9; if(con.getButton1(2) && con.getButton1(6)) firePwm[0] = 0.0; } } //fire[0]->setSpeed(Output_PID); // fire[0]->setSpeed(0.0); // if(con.getButton1(2) == 1 && con.getButton1(6) == 1) fire[0]->setSpeed(0.0); // if(con.getButton1(0)==0) loadingPwm[0] = 0.5, loadingPwm[1] = -0.5; // if(con.getButton1(1)==0) loadingPwm[0] = -0.5, loadingPwm[1] = 0.5; // if(con.getButton1(0) && con.getButton1(1)) loadingPwm[0] = 0.0, loadingPwm[1] = 0.0; // for(int i = 0; i < 5; i++) Loading[i]->setSpeed(loadingPwm[i]); for(int i = 0; i < 2; i++) fire[i]->setSpeed(firePwm[i]); } debugpc.printf("pid: %f\r\n", Output_PID); } else { for(int i = 0; i < 0; i++) fire[i]->setSpeed(0.0); // for(int i = 0; i < 5; i++) Loading[i]->setSpeed(0.0); } } //void GakuBot::autoMode1() //{ // // for(int i = 0; i < 3; i++) fire[i]->setSpeed(0); // // switch(mode) { // case 0: // demoX = 0.0; // demoY = 0.0; // break; // case 1: // demoX = -0.5; // demoY = 0.0; // break; // case 2: // // demoX = 0.0; // demoY = 0.0; // // break; // case 3: // demoX = 0.0; // demoY = 0.5; // // break; // case 4: // demoX = 0.0; // demoY = 0.0; // break; // case 5: // demoX = 0.0; // demoY = 0.0; // angle1_1=1; // angle1_2=0; // break; // case 6: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=0; // fire[0]->setSpeed(Output_PID); // break; // case 7: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=1; // break; // case 8: // demoX = 0.0; // demoY = -0.5; // angle1_1=0; // angle1_2=0; // break; // case 9: // demoX = 0.0; // demoY = 0.0; // break; // case 10: // demoX = 0.5; // demoY = 0.0; // break; // case 11: // // demoX = 0.0; // demoY = 0.0; // // break; // case 12: // demoX = 0.0; // demoY = 0.5; // // break; // case 13: // demoX = 0.0; // demoY = 0.0; // break; // case 14: // demoX = 0.0; // demoY = 0.0; // angle1_1=1; // angle1_2=0; // break; // case 15: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=0; // fire[0]->setSpeed(Output_PID); // break; // case 16: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=1; // break; // case 17: // demoX = 0.0; // demoY = -0.5; // angle1_1=0; // angle1_2=0; // break; // case 18: // demoX = 0.0; // demoY = 0.0; // break; // case 19: // demoX = 0.5; // demoY = 0.0; // break; // case 20: // // demoX = 0.0; // demoY = 0.0; // // break; // case 21: // demoX = 0.0; // demoY = 0.5; // // break; // case 22: // demoX = 0.0; // demoY = 0.0; // break; // case 23: // demoX = 0.0; // demoY = 0.0; // angle1_1=1; // angle1_2=0; // break; // case 24: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=0; // fire[0]->setSpeed(Output_PID); // break; // case 25: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=1; // break; // case 26: // demoX = 0.0; // demoY = -0.5; // angle1_1=0; // angle1_2=0; // break; // case 27: // demoX = 0.0; // demoY = 0.0; // break; // case 28: // demoX = 0.5; // demoY = 0.0; // break; // case 29: // // demoX = 0.0; // demoY = 0.0; // // break; // case 30: // demoX = 0.0; // demoY = 0.5; // // break; // case 31: // demoX = 0.0; // demoY = 0.0; // break; // case 32: // demoX = 0.0; // demoY = 0.0; // angle1_1=1; // angle1_2=0; // break; // case 33: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=0; // fire[0]->setSpeed(Output_PID); // break; // case 34: // demoX = 0.0; // demoY = 0.0; // angle1_1=0; // angle1_2=1; // break; // case 35: // demoX = 0.0; // demoY = -0.5; // angle1_1=0; // angle1_2=0; // break; // case 36: // demoX = 0.0; // demoY = 0.0; // break; // case 37: // demoX = 0.5; // demoY = 0.0; // break; // // } // // if(!receiveSuccessed) omni.computeXY(0.0, 0.0, 0.0); // else { // if(con.getButton2(1) == 0) { // for(int i = 0; i < 4; i++) stick[i] = con.getStick(i); // if((-0.1 < stick[2] )&& (stick[2] < 0.1)) omni.computeXY(stick[0]/3,stick[1]/3, -1*anglePID.compute()); // else { // omni.computeXY(stick[0]/3,stick[1]/3, -1*stick[2]/4.0); // ofsetNowAngle = nowAngle; // attachAngle = 0; // } // } else omni.computeXY(demoX,demoY, -1*anglePID.compute()); // } // for (int i = 0; i < 4; i++) { // speed[i] = omni.wheel[i]; // wheels[i]->setSpeed(speed[i]); //// debugpc.printf("%1d: %.3f, ", i, speed[i]); // } // //// } // dt = t.read(); // Calculate delta time // // if((con.getButton2(0) == 0) && (dt >= 0.5)) { // mode++; // t.reset(); // } // if(mode > 37) mode = 0; //// debugpc.printf("mode = %d,time = %f\r\n",mode,dt); // // //} // //void GakuBot::autoMode2() //{ // for(int i = 0; i < 3; i++) fire[i]->setSpeed(0); // // // omni.computeXY(demoX,demoY, -1*anglePID.compute()); // for (int i = 0; i < 4; i++) { // speed[i] = omni.wheel[i]; // wheels[i]->setSpeed(speed[i]); //// debugpc.printf("%1d: %.3f, ", i, speed[i]); // } //// } // dt = t.read(); // // if((con.getButton2(0) == 0) && (dt >= 0.5)) { // mode++; // t.reset(); // } // if(mode > 33) mode = 0; //// debugpc.printf("mode = %d,time = %f\r\n",mode,dt); //}