naoki tanabe
前のやつです。
Dependencies: FEP OmniPosition PID QEI R1307 TFmini ikarashiMDC linesSnsor omni_wheel
Diff: gakuBot/gakubot.cpp
- Revision:
- 3:8f4c81ad256a
- Parent:
- 1:af8bee219a3a
- Child:
- 5:c7643ae5835f
--- 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