![](/media/cache/profiles/IMG_4892.JPG.50x50_q85.jpg)
前のやつです。
Dependencies: FEP OmniPosition PID QEI R1307 TFmini ikarashiMDC linesSnsor omni_wheel
Diff: gakuBot/gakubot.cpp
- Revision:
- 3:8f4c81ad256a
- Parent:
- 1:af8bee219a3a
- Child:
- 5:c7643ae5835f
diff -r af8bee219a3a -r 8f4c81ad256a gakuBot/gakubot.cpp --- a/gakuBot/gakubot.cpp Sun Aug 05 02:47:05 2018 +0000 +++ b/gakuBot/gakubot.cpp Tue Sep 11 15:05:51 2018 +0000 @@ -3,66 +3,471 @@ GakuBot::GakuBot() : led(LED2), pid(KP,KI,KD,RATE), + anglePID(angleKP, angleKI, angleKD, RATE), omni(4), - angle1_1(air1_0),angle1_2(air1_1), + 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(PC_10,PC_11,115200), + RS485(MDTX,MDRX,115200), debugpc(USBTX,USBRX,115200), - wheels( { - ikarashiMDC(&RS485control,0,0,SM,&RS485), - ikarashiMDC(&RS485control,0,1,SM,&RS485), - ikarashiMDC(&RS485control,0,2,SM,&RS485), - ikarashiMDC(&RS485control,0,3,SM,&RS485) -}), -fire( { - ikarashiMDC(&RS485control,1,0,SM,&RS485), - ikarashiMDC(&RS485control,1,1,SM,&RS485), - ikarashiMDC(&RS485control,1,2,SM,&RS485) -}) + receiveSuccessed(0) + { - stick[4] = {0}, speed[4] = {0}, Output_PID = 0; - airFlag = 0,airFlag2 = 0,airStatus = 0,airStatus2 = 0; - int distance = 970,distanceOfset = 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; + wheels[i]->braking = true; } - for(int i = 0; i < 3; i++) { - armMotor[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, 1600); + pid.setInputLimits(-100, 2300); pid.setOutputLimits(-1.0, 1.0); -// pid.setSetPoint(distance);// 目標値 + pid.setSetPoint(distance);// 目標値 pid.setBias(0.0); // outputの変わり目 + + 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() { - -} -void GakuBot::controllerMode1() -{ - -} -void GakuBot::autoMode1() -{ + 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::autoMech() -{ - -} \ No newline at end of file +//void GakuBot::autoMode1() +//{ +// +// for(int i = 0; i < 3; i++) fire[i]->setSpeed(0); +// +// switch(mode) {/*modeをテーブルごとに分ける。example > modeT1, modeT2, modeT3, modeT4*/ +// case 0: +// demoX = 0.0; +// demoY = 0.0; +// break; +// case 1: +// demoX = -0.5; +// demoY = 0.0; +// break; +// case 2: +// /*line収束処理*/ +// 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: +// /*line収束処理*/ +// 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: +// /*line収束処理*/ +// 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: +// /*line収束処理*/ +// 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); +//} \ No newline at end of file