NagaokaRoboticsClub_mbedTeam
未完成
Dependencies: linemiconget petbottle_Loadin5port Kinect OmniPosition PID QEI led R1307 S-ShapeModel SerialMultiByte TFmini fep2 ikarashiMDC linesSnsor omni_wheel solenoid_valve
gakuBot/gakubot.cpp
- Committer:
- tanabe2000
- Date:
- 2018-10-12
- Revision:
- 0:7822f4172b0c
- Child:
- 2:c501f6845500
File content as of revision 0:7822f4172b0c:
#include "gakubot.h"
GakuBot::GakuBot():
led(LED2),
kinect(fepTX, fepRX,151,115200),
lmicon(LineTX, LineRX),
getposition(measuringTX, measuringRX),
valveFire1(solenoidValve1_1, solenoidValve1_2),
valveFire2(solenoidValve3_1, solenoidValve3_2),
omni(4),
anglePID(angleKP, angleKI, angleKD, 0.001),
fire1Pid(fire1KP, fire1KI, fire1KD, RATE),
fire2Pid(fire2KP, fire2KI, fire2KD, RATE),
XPid(XKP, XIP, XDP, RATE),
YPid(YKP, YIP, YDP, RATE),
distanceFrontPid(frontKP, frontKP, frontKD, RATE),
distanceBehindPid(behindKP, behindKI, behindKD, RATE),
RS485control(PA_4),
RS485(MDTX,MDRX,115200),
debugpc(USBTX,USBRX,115200),
receiveSuccessed(0),
limit1(limitswich2_1),
limit2(limitswich2_2),
startSW(start)
{
for(int i = 0; i < 4; i++)wheels[i] = new ikarashiMDC(&RS485control,0,i,SM,&RS485), wheels[i]->braking = true;
for(int i = 0; i < 2; i++)fire[i] = new ikarashiMDC(&RS485control,2,i,SM,&RS485), fire[i]->braking = false;
conposition = new PositionController(500.0,3000.0,0.35,-0.2,0.7);
for(int i = 0; i < 4; i++)omni.wheel[i].setRadian(PI / 4.0 * (2.0*i+1.0));
Xdemo[0] = 0, Xdemo[1] = 3912, Xdemo[2] = 3921, Xdemo[3] = 3912, Xdemo[4] = -1000;
Ydemo[0] = 0, Ydemo[1] = -836, Ydemo[2] = -4112, Ydemo[3] = -836, Ydemo[4] = 0;
suzuki[0] = 4700;
suzuki[1] = 3000;
suzuki[2] = 2000;
startSW.mode(PullUp);
limit1.mode(PullUp);
limit2.mode(PullUp);
fire1Pid.setMode(AUTO_MODE);
fire1Pid.setInputLimits(-100, 2300);
fire1Pid.setOutputLimits(-0.3, 1.0);
fire1Pid.setBias(0.0);
fire2Pid.setMode(AUTO_MODE);
fire2Pid.setInputLimits(-100, 2300);
fire2Pid.setOutputLimits(-0.3, 1.0);
fire2Pid.setSetPoint(fireDistance2);
XPid.setBias(0.0);
XPid.setMode(AUTO_MODE);
XPid.setInputLimits(-10000, 10000);
XPid.setOutputLimits(-0.8, 0.8);
YPid.setBias(0.0);
YPid.setMode(AUTO_MODE);
YPid.setInputLimits(-10000, 10000);
YPid.setOutputLimits(-0.8, 0.8);
anglePID.setMode(AUTO_MODE);
anglePID.setInputLimits(-360, 360);
anglePID.setOutputLimits(-1.0, 1.0);
anglePID.setSetPoint(0.0);
anglePID.setBias(0.0);
distanceFrontPid.setBias(0.0);
distanceFrontPid.setMode(AUTO_MODE);
distanceFrontPid.setInputLimits(30.0, 35.0);
distanceFrontPid.setOutputLimits(-0.5, 0.0);
distanceFrontPid.setSetPoint(40);
distanceBehindPid.setBias(0.0);
distanceBehindPid.setMode(AUTO_MODE);
distanceBehindPid.setInputLimits(30.0, 70.0);
distanceBehindPid.setOutputLimits(-0.7, 0.7);
distanceBehindPid.setSetPoint(40);
// conposition->targetXY(2000.0, 1000.0);
pt.start();
confirmT.start();
fireFrag1time.start();
pidT.start();
t.start();
getx = 0;
gety = 0;
swflag = 1;
airFlag = 1;
}
void GakuBot::botConfirm()
{
//if(con.getButton2(2)==0) fireDistance1 += 10, fireDistance2 += 10;
// if(con.getButton2(3)==0) fireDistance1 -= 10, fireDistance2 -= 10;
// receiveSuccessed = con.receiveState();
kinect.newdata();
kinectDistance = kinect.get_distance();
kinectmode = kinect.get_mode();
lmicon.receiveState();
nowPals = -1*lmicon.getEncoder(2);
nowPals2 = lmicon.getEncoder(0);
fire1Pid.setSetPoint(fireDistance1);
fire1Pid.setProcessValue(nowPals - distanceOfset);
Output_PID = -1*fire1Pid.compute();
fire2Pid.setSetPoint(fireDistance2);
fire2Pid.setProcessValue(nowPals2 - distanceOfset2);
Output_PID2 = -1*fire2Pid.compute();
yawdegree = getposition.getTheta();
if((yawdegree - beforeYaw) > 350)yawMode--;
else if((yawdegree - beforeYaw) < -350)yawMode++;
beforeYaw = yawdegree;
nowAngle = 360*yawMode + yawdegree;
anglePID.setSetPoint(attachAngle);
anglePID.setProcessValue(nowAngle - ofsetNowAngle);
distanceBehindPid.setProcessValue(lmicon.getDistance(1));
if((int)startSW==0 && (pdt > 0.5)) {
//getx = getposition.getX();
// gety = getposition.getY();
// pt.reset();
//demomode++;
// if(demomode > 3)demomode = 0;
}
//getx = Xdemo[demomode];
// gety = Ydemo[demomode];
// debugpc.printf("xVector = %f, yVector = %f\r\n", conposition->getVelocityX(), conposition->getVelocityY());
//debugpc.printf("XPid.compute()<%f>, YPid.compute()<%f>\r\n", XPid.compute(), YPid.compute());
debugpc.printf("x = %d, y = %d\r\n", getposition.getX(), getposition.getY());
// debugpc.printf("limit1 = %d, limit2 = %d", (int)limit1, (int)limit2);
// debugpc.printf("rad<%f>", targetradians*180/PI);
// debugpc.printf("getx<%d>, gety<%d>\r\n", getx, gety);
}
//void GakuBot::controllerMode1()
//{
//// if(receiveSuccessed) {
// for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
// if(con.getButton2(1)==0) {
//
//// omni.computeXY(conposition->getVelocityX(), conposition->getVelocityY(), -1*anglePID.compute());
//// omni.computeXY(X, Y, -1*anglePID.compute());
//
//
//
//// debugpc.printf("X<%f>, Y<%f>\r\n", X, Y);
// omni.computeXY(X, Y, -1*anglePID.compute());
//// debugpc.printf("con.getButton1(4) = %d\r\n", con.getButton1(4));
//
// //if((limit1 == 1) && (limit2 == 1)) {
//// omni.computeXY(0.5, 0.0, -1*anglePID.compute());
//// } else if((limit1 != 1) && (limit2 == 1)) {
//// omni.computeXY(0.5, -0.08, -1*anglePID.compute()+0.025);
//// } else if((limit1 == 1) && (limit2 != 1)) {
//// omni.computeXY(0.5, -0.08, -1*anglePID.compute()-0.025);
//// } else {
//// omni.computeXY(0.5, -0.15, -1*anglePID.compute());
//// }
// } else if((-0.1 < stick[2] )&& (stick[2] < 0.1)) {
// omni.computeXY(stick[0]/2,stick[1]/2, -1*anglePID.compute());
//// debugpc.printf("- distanceBehindPid.compute()<%f>", distanceBehindPid.compute());
// Xlopassb = 0;
// Ylopassb = 0;
// } else {
// omni.computeXY(stick[0]/3.0,stick[1]/3.0, -1*stick[2]/6.0);
// ofsetNowAngle = nowAngle;
// attachAngle = 0;
// }
//
// confirmDt = confirmT.read();
// //if((con.getButton2(0)==0) && (confirmDt >= 0.5))attachAngle += 10, confirmT.reset();
//// if((con.getButton2(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]);
// }
// //} else {
//// debugpc.printf("error\r\n");
//// for(int i = 0; i < 4; i++)wheels[i]->setSpeed(0);
////
//// }
//}
//
//void GakuBot::controllerMode2()
//{
// if(receiveSuccessed) {
// 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]);
// }
// } else {
// debugpc.printf("error\r\n");
// for (int i = 0; i < 4; i++)wheels[i]->setSpeed(0);
// }
//}
//
//void GakuBot::controllMech1()
//{
//// if(receiveSuccessed) {
// pdt = pt.read();
// if((con.getButton1(1) == 0) && (pdt > 0.5)) loadingmode++, pt.reset();
// if(loadingmode == 2 ) loadingmode++;
// if(loadingmode > 3) loadingmode = 0;
// loading.petbottlemode(loadingmode);
// debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
//// debugpc.printf("pals2 = %d,fireDistance2 = %d, distanceOfset2 = %d", nowPals2 - distanceOfset2,fireDistance2,distanceOfset2);
// if((con.getButton1(5)==0)&&(airFlag == 0) && (pdt > 0.5))airFlag = 1, pt.reset();
// pdt = pt.read();
// if((con.getButton1(5)==0)&&(airFlag == 1) && (pdt > 0.5))airFlag = 0, pt.reset();
//
// //if(airFlag == 1) {
//// led = 1;
//// valveFire1.inputState(1);
//// if(con.getButton1(3)==0) {
//// if(sw3flag == 1) {
//// fireDistance1 = 750;
//// sw3flag = 0;
//// } else firePwm[0] = Output_PID;
//// } else if((con.getButton1(0) == 0)) {
////// fireDistance1 = 1928;
//// fireDistance1 = 1847;
//// firePwm[0] = Output_PID;
//// sw3flag = 1;
//// } else {
//// 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;
//// }
//// } else {
//// sw3flag = 1;
//// valveFire1.inputState(0);
//// distanceOfset = nowPals;
//// }
//
// if(airFlag == 1) {
// led = 1;
// valveFire2.inputState(1);
// if(con.getButton1(3)==0) {
// if(sw3flag == 1) {
// fireDistance2 = 810;
// sw3flag = 0;
// } else firePwm[1] = Output_PID2;
// } else if((con.getButton1(0) == 0)) {
// // fireDistance1 = 1928;
// fireDistance2 = 1847;
// firePwm[1] = Output_PID2;
// sw3flag = 1;
// } else {
// if(con.getButton1(2)==0) firePwm[1] = 0.9;
// if(con.getButton1(6)==0) firePwm[1] = -0.9;
// if(con.getButton1(2) && con.getButton1(6)) firePwm[1] = 0.0;
// }
// } else {
// sw3flag = 1;
// valveFire2.inputState(0);
// distanceOfset2 = nowPals2;
// }
//
//
// //debugpc.printf("Output_PID:%f",Output_PID);
// //for(int i = 0; i < 5; i++)debugpc.printf("<%4d>",lmicon.getLine1(i));
//// for(int i = 0; i < 5; i++)debugpc.printf("<%4d>",lmicon.getLine2(i));
//// for(int i = 0; i < 2; i++)debugpc.printf("<%4d>",lmicon.getDistance(i));
// for(int i = 0; i < 3; i++)debugpc.printf("<%4d>",lmicon.getEncoder(i));
//// debugpc.printf("sw<%2d>",(int)startSW);
//
// debugpc.printf("\r\n");
// fire[0]->setSpeed(firePwm[0]);
// fire[1]->setSpeed(firePwm[1]);
//// } else for(int i = 0; i < 0; i++) fire[i]->setSpeed(0.0);
//}
void GakuBot::autoMode1()
{
piddt = pidT.read();
XPid.setSetPoint(getx);
XPid.setProcessValue(getposition.getX());
YPid.setSetPoint(gety);
YPid.setProcessValue(getposition.getY());
conposition->targetXY(getx, gety);
conposition->compute(getposition.getX(), getposition.getY());
targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
if(cos(targetradians) < 0)Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*(-1*cos(targetradians)));
else Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*cos(targetradians));
if(sin(targetradians) < 0) Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*(-1*sin(targetradians)));
else Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*sin(targetradians));
if((getx - getposition.getX()) >=0 ) {
if(Xlopassb > Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
else Xlopassb = Xlopass;
} else {
if(Xlopassb < Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
else Xlopassb = Xlopass;
}
if((gety - getposition.getY()) >=0) {
if(Ylopassb > Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
else Ylopassb = Ylopass;
} else {
if(Ylopassb < Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
else Ylopassb = Ylopass;
}
X = (Xlopass*cos((float)nowAngle*PI/180.0) - Ylopass*sin((float)nowAngle*PI/180.0));
Y = (Xlopass*sin((float)nowAngle*PI/180.0) + Ylopass*cos((float)nowAngle*PI/180.0));
dt = t.read();
// debugpc.printf("piddt <%f>dpetbotle<%d>\r\n",piddt, dpetbotle);
debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
// debugpc.printf("kinectDistance<%d>,kinectmode<%d>\n\r",kinectDistance,kinectmode);
if((kinectmode == 2) && (kinectFlag == 0))kinectFlag = 1,suzuki[0] = kinectDistance ;
if((kinectmode == 4) && (kinectFlag == 1))kinectFlag = 2, suzuki[1] = kinectDistance;
if((kinectmode == 6) && (kinectFlag == 2))kinectFlag = 3, suzuki[2] = kinectDistance;
//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
if((int)startSW == 0)start_ = 2;
if(start_ == 2){
if(airFlag == 1 && firecount <= 2) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = loadingParamator, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 1120;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
}
// if((int)startSW == 0)start_ = 1;
if((start_ == 0)) {
getx =0;
gety =0;
}
if((mode == 0) && (start_ == 1)) {
mode++;
getx = dash1X;
gety = dash1Y;
}
if(mode == 1 && getposition.getX() >= dash1X && getposition.getY() <= dash1Y) {
mode++;
getx = dash2X;
gety = dash2Y;
}
if(mode == 2 && getposition.getX() >= dash2X && getposition.getY() <= dash2Y) {
mode++;
getx = dash3X;
gety = dash3Y;
}
if(mode == 3 && getposition.getX() >= dash3X-200 && getposition.getY() <= dash3Y+200) {
//if(firecount > 3 && (dt >= 1.0)) {
// mode++;
// }
if(airFlag == 1 && firecount <= 2) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 1120;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
// mode++;
}
if(mode == 4 && kinectFlag == 1) {
if(getposition.getX() > table1800X - 200) {
mode++;
getx = table1800X;
gety = -1*suzuki[0]-1000;
kabeFlag = 0;
swflag = 1;
airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
omni.computeXY(0.3, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0.3, -0.15, -1*anglePID.compute());
}
kabeFlag = 1;
}
}
if(mode == 5) {
if(kinectmode == 3){
mode++;
}else{
if(airFlag == 1) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 1010;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
}
//if(((int)startSW == 0) && (dt >= 0.5)) {
// mode++;
// t.reset();
// }
/*発射機構動く*/
}
if(mode == 6 && (-1*suzuki[1]-1000 < wallmodeY) && kinectFlag == 2) {
// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
if(getposition.getY() < walldashY) {
if(getposition.getX() < table1500X + 400) {
mode++;
getx = table1500X;
gety = -1*suzuki[1]-1000;
kabeFlag = 0;swflag = 1;
airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0, -0.18, -1*anglePID.compute());
}
kabeFlag = 1;
}
} else {
gety = wallmodeY;
}
}
if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
swflag = 1;
airFlag = 1;
if(suzuki[0] < suzuki[1]) {
gety = -1*suzuki[1]-1000;
if(getposition.getY() < -1*suzuki[1]-1000) {
mode++;
getx = table1500X;
}
} else {
getx = table1500X;
if(getposition.getX() < table1500X ) {
mode++;
gety = -1*suzuki[1]-1000;
}
}
}
if(mode == 7) {
if(kinectmode == 5){
mode++;
}else{
if(airFlag == 1) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 820;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
//if(/*suzuki倒立mode == */) {
// mode++;
// }
/*発射機構動く*/
}
}
if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
swflag = 1;
airFlag = 1;
if(getposition.getY() < walldashY) {
if(getposition.getX() < table1200X + 200) {
mode++;
getx = table1200X;
gety = -1*suzuki[2]-1200;
kabeFlag = 0;
} else {
if((limit1 == 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0, -0.18, -1*anglePID.compute());
}
kabeFlag = 1;
}
} else {
gety = walldashY;
}
}
if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
swflag = 1;
airFlag = 1;
if(suzuki[1] < suzuki[2]) {
gety = -1*suzuki[2]-1200;
if(getposition.getY() < -1*suzuki[2]-800) {
mode++;
getx = table1200X;
}
} else {
getx = table1200X;
if(getposition.getX() < table1200X) {
mode++;
gety = -1*suzuki[2]-1200;
}
}
}
if(mode == 9) {
if(kinectmode == 7){
}else{
if(airFlag == 1) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 870;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
//if(/*suzuki倒立mode == */) {
// mode++;
// }
/*発射機構動く*/
}
//if(/*suzuki倒立mode == */) {
// mode++;
// }
/*発射機構動く*/
}
// if(con.getButton2(1)==0) {
if(kabeFlag == 1) {
for (int i = 0; i < 4; i++) {
speed[i] = omni.wheel[i];
wheels[i]->setSpeed(speed[i]);
}
} else {
for (int i = 0; i < 4; i++) {
omni.computeXY(X, Y, -1*anglePID.compute());
speed[i] = omni.wheel[i];
wheels[i]->setSpeed(speed[i]);
}
}
for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
valveFire1.inputState(solenoidValve1is);
valveFire2.inputState(solenoidValve2is);
loading.petbottlemode(loadingmode);
// if(mode > 37) mode = 0;
// debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
}
void GakuBot::autoMode2()
{
piddt = pidT.read();
XPid.setSetPoint(-1.0*getx);
XPid.setProcessValue(getposition.getX());
YPid.setSetPoint(-1.0*gety);
YPid.setProcessValue(getposition.getY());
conposition->compute(getposition.getX(), getposition.getY());
targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
if(cos(targetradians) < 0)Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*(-1*cos(targetradians)));
else Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*cos(targetradians));
if(sin(targetradians) < 0) Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*(-1*sin(targetradians)));
else Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*sin(targetradians));
if((getx - getposition.getX()) >=0 ) {
if(Xlopassb > Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
else Xlopassb = Xlopass;
} else {
if(Xlopassb < Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
else Xlopassb = Xlopass;
}
if((gety - getposition.getY()) >=0) {
if(Ylopassb > Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
else Ylopassb = Ylopass;
} else {
if(Ylopassb < Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
else Ylopassb = Ylopass;
}
X = (Xlopass*cos((float)nowAngle*PI/180.0) - Ylopass*sin((float)nowAngle*PI/180.0));
Y = (Xlopass*sin((float)nowAngle*PI/180.0) + Ylopass*cos((float)nowAngle*PI/180.0));
dt = t.read();
// debugpc.printf("piddt <%f>dpetbotle<%d>\r\n",piddt, dpetbotle);
debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
// debugpc.printf("kinectDistance<%d>,kinectmode<%d>\n\r",kinectDistance,kinectmode);
if((kinectmode == 2) && (kinectFlag == 0))kinectFlag = 1,suzuki[0] = kinectDistance ;
if((kinectmode == 4) && (kinectFlag == 1))kinectFlag = 2, suzuki[1] = kinectDistance;
if((kinectmode == 6) && (kinectFlag == 2))kinectFlag = 3, suzuki[2] = kinectDistance;
//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
if((int)startSW == 0)start_ = 1;
if((start_ == 0)) {
getx =0;
gety =0;
}
if((mode == 0) && (start_ == 1)) {
mode++;
getx = dash1X;
gety = dash1Y;
}
if(mode == 1 && getposition.getX() <= dash1X && getposition.getY() <= dash1Y) {
mode++;
getx = dash2X;
gety = dash2Y;
}
if(mode == 2 && getposition.getX() <= dash2X && getposition.getY() <= dash2Y) {
mode++;
getx = dash3X;
gety = dash3Y;
}
if(mode == 3 && getposition.getX() <= dash3X+200 && getposition.getY() <= dash3Y+200) {
//if(firecount > 3 && (dt >= 1.0)) {
// mode++;
// }
if(airFlag == 1 && firecount <= 2) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 1120;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
// mode++;
}
if(mode == 4 && kinectFlag == 1) {
if(getposition.getX() < table1800X + 200) {
mode++;
getx = table1800X;
gety = -1*suzuki[0]-1000;
kabeFlag = 0;
swflag = 1;
airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
omni.computeXY(0.3, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0.3, -0.15, -1*anglePID.compute());
}
kabeFlag = 1;
}
}
if(mode == 5) {
if(kinectmode == 3){
mode++;
}else{
if(airFlag == 1) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 1010;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
}
//if(((int)startSW == 0) && (dt >= 0.5)) {
// mode++;
// t.reset();
// }
/*発射機構動く*/
}
if(mode == 6 && (-1*suzuki[1]+1000 > wallmodeY) && kinectFlag == 2) {
// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
if(getposition.getY() < walldashY) {
if(getposition.getX() > table1500X + 400) {
mode++;
getx = table1500X;
gety = -1*suzuki[1]-1000;
kabeFlag = 0;swflag = 1;
airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0, -0.18, -1*anglePID.compute());
}
kabeFlag = 1;
}
} else {
gety = wallmodeY;
}
}
if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
swflag = 1;
airFlag = 1;
if(suzuki[0] < suzuki[1]) {
gety = -1*suzuki[1]-1000;
if(getposition.getY() < -1*suzuki[1]-1000) {
mode++;
getx = table1500X;
}
} else {
getx = table1500X;
if(getposition.getX() > table1500X ) {
mode++;
gety = -1*suzuki[1]-1000;
}
}
}
if(mode == 7) {
if(kinectmode == 5){
mode++;
}else{
if(airFlag == 1) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 820;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
//if(/*suzuki倒立mode == */) {
// mode++;
// }
/*発射機構動く*/
}
}
if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
swflag = 1;
airFlag = 1;
if(getposition.getY() < walldashY) {
if(getposition.getX() > table1200X - 200) {
mode++;
getx = table1200X;
gety = -1*suzuki[2]-1200;
kabeFlag = 0;
} else {
if((limit1 == 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0, -0.18, -1*anglePID.compute());
}
kabeFlag = 1;
}
} else {
gety = walldashY;
}
}
if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
swflag = 1;
airFlag = 1;
if(suzuki[1] < suzuki[2]) {
gety = -1*suzuki[2]-1200;
if(getposition.getY() < -1*suzuki[2]-800) {
mode++;
getx = table1200X;
}
} else {
getx = table1200X;
if(getposition.getX() > table1200X) {
mode++;
gety = -1*suzuki[2]-1200;
}
}
}
if(mode == 9) {
if(kinectmode == 7){
}else{
if(airFlag == 1) {
led = 1;
solenoidValve1is = 1;
if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
pidT.reset();
dpetbotle = 1;
}
piddt = pidT.read();
if((piddt > 4.0) && (dpetbotle == 1)) {
loadingmode = 3;
firePwm[0] = 0.0;
}
if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
if(swflag == 1) {
fireDistance1 = 870;
swflag = 0;
pidT.reset();
}
if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
firePwm[0] = Output_PID;
}
} else {
if(swflag == 0){
pidT.reset();
swflag = 1;
firecount++;
}
piddt = pidT.read();
if((piddt > 2.0))airFlag = 1;
dpetbotle = 0;
led = 0;
solenoidValve1is = 0;
loadingmode = 0;
distanceOfset = -1*lmicon.getEncoder(2);
}
//if(/*suzuki倒立mode == */) {
// mode++;
// }
/*発射機構動く*/
}
//if(/*suzuki倒立mode == */) {
// mode++;
// }
/*発射機構動く*/
}
// if(con.getButton2(1)==0) {
if(kabeFlag == 1) {
for (int i = 0; i < 4; i++) {
speed[i] = omni.wheel[i];
wheels[i]->setSpeed(speed[i]);
}
} else {
for (int i = 0; i < 4; i++) {
omni.computeXY(X, Y, -1*anglePID.compute());
speed[i] = omni.wheel[i];
wheels[i]->setSpeed(speed[i]);
}
}
for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
valveFire1.inputState(solenoidValve1is);
valveFire2.inputState(solenoidValve2is);
loading.petbottlemode(loadingmode);
// if(mode > 37) mode = 0;
// debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
}