File content as of revision 3:8f4c81ad256a:

#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);   // 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()
{
    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) {/*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);
//}