File content as of revision 15:da88e62863a1:

#include "robot.h"

Robot::Robot() :
      spin(P, I, D, interval),
      sgoal(P, I, D, interval),
      pc(USBTX, USBRX, 115200),
      shot(kicker),
      drib(ESCpin)
{
    
    omni.wheel[2].setRadian(PI/4 * 1);
    omni.wheel[3].setRadian(PI/4 * 3);
    omni.wheel[0].setRadian(PI/4 * 5);
    omni.wheel[1].setRadian(PI/4 * 7);
    motor[0] = new KohiMD(PA_15);
    motor[1] = new KohiMD(PA_6);
    motor[2] = new KohiMD(PA_7);
    motor[3] = new KohiMD(PB_6);
    spin.setInputLimits(-180, 180);
    sgoal.setInputLimits(-180, 180);
    spin.setOutputLimits(-0.4,0.4);
    sgoal.setOutputLimits(-0.4,0.4);
    spin.setBias(0);
    sgoal.setBias(0);
    spin.setMode(1);
    sgoal.setMode(1);
    spin.setSetPoint(0);
    sgoal.setSetPoint(0);
    shot.setkickperiod(2.0);
    shot.setoutputtime(0.1);
    drib.setspeed(0.0);
    startb = 0;
}

void Robot::start(uint8_t Team, uint8_t Algorithm)
{
    while(1){
        printf("%d %d ", (int)sensor_.irVal ,(int)sensor_.ballKeep);
        spin.setProcessValue(sensor_.angleLimit/*-sensor_.ballAngle*/);
        spin_power = spin.compute();

//        theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ;
//        theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1);
//        theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
        theta = ((sensor_.ballAngle>0) - (sensor_.ballAngle<0)) * (2.0/(2-fabs(sensor_.ballAngle)/180) - 1) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
        printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 100));
        omni.computeCircular(/*0*/1.1*sensor_.ballRange / 100.0,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power);
        for (int i=0; i<4; i++) {
            thisSpeed[i] = omni.wheel[i];
        }
        if((fabs(sensor_.ballAngle) < 50 && sensor_.ballRange) || sensor_.ballKeep) dribbleCheck(0.6);
        else dribbleCheck(0.0);
        motor[0]->setSpeed(thisSpeed[0]);
        motor[1]->setSpeed(thisSpeed[1]);
        motor[2]->setSpeed(-1*thisSpeed[2]);
        motor[3]->setSpeed(-1*thisSpeed[3]);
    }
//    昔
//    while (1) {
//        startb=1;
//        spin.setProcessValue(sensor_.angleLimit);
//        theta = sensor_.ballAngle * ballExtra * PI / 180.0;
//        spin_power = spin.compute();
//        if (/*sensor_.ballKeep*/0) {
//            /*ボールをつかんでるなら*/
////            drib.setspeed(0.5);
//            if(Team)
//            {
//                /*あおに向かうよ*/
////                /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
////                omni.computeCircular(sensor_.blueRange,PI / 180.0 * sensor_.blueAngle, spin_power);
//        
//            }
//            else
//            {
//                /*きいろに向かうよ*/
////                /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
//            }
//        }
//        else
//            /*つかんでないなら*/
//        {
////            for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
////            pc.printf("\r\n");
//            drib.setspeed(0.0);
////            omni.computeCircular(sensor_.ballRange, (PI / 180.0) * sensor_.ballAngle + theta, spin_power);
//            omni.computeCircular(sensor_.ballRange, sensor_.ballAngle, spin_power);
//        }
////        printf("%d ",(int)sensor.ballKeep);
//        if(sensor_.line[0] || sensor_.line[1])
//        {
//            omni.computeCircular(1,-1*PI/2.0,spin_power);
//        }
//        if(sensor_.line[2] || sensor_.line[3]) 
//        {
//            omni.computeCircular(1,PI/2.0,spin_power);
//        }
//        if(sensor_.line[4])
//        {
//             omni.computeCircular(1,0,spin_power);
//        }
//        if(sensor_.line[5])
//        {
//             omni.computeCircular(1,PI,spin_power);
//        }
//        
//        if (startb) {
//            omni2wheel = omni.wheel[2];
//            omni3wheel = omni.wheel[3];
////            motor[0]->setSpeed(omni.wheel[0]);
////            motor[1]->setSpeed(omni.wheel[1]);
////            motor[2]->setSpeed(omni.wheel[2]);
////            motor[3]->setSpeed(omni.wheel[3]);
////            motor[3]->setSpeed(-omni3wheel);
//            shot.outPut();
//
//        } else {
//            motor[0]->setSpeed(0);
//            motor[1]->setSpeed(0);
//            motor[2]->setSpeed(0);
//            motor[3]->setSpeed(0);
//        }
//    }
}

void Robot::pidtest()
{
    while(1){
        spin.setProcessValue(sensor_.angleLimit);
        spin_power = spin.compute();
        omni.computeCircular(0,0,spin_power);
        if(sensor_.line[0] || sensor_.line[1])
        {
            omni.computeCircular(1,PI/2.0,spin_power);
        }
        if(sensor_.line[2] || sensor_.line[3]) 
        {
            omni.computeCircular(1,-1*PI/2.0,spin_power);
        }
        if(sensor_.line[4])
        {
             omni.computeCircular(1,0,spin_power);
        }
        if(sensor_.line[5])
        {
             omni.computeCircular(1,PI,spin_power);
        }
        for (int i=0; i<4; i++) {
            thisSpeed[i] = omni.wheel[i];
        }
        motor[0]->setSpeed(thisSpeed[0]);
        motor[1]->setSpeed(thisSpeed[1]);
        motor[2]->setSpeed(-1*thisSpeed[2]);
        motor[3]->setSpeed(-1*thisSpeed[3]);
    }
}

void Robot::motorCheck(int motorNumber, float power)
{
    _motorNumber = motorNumber;
    for(i = 0; i < 4; i++)
    {
        if(i == _motorNumber) motor[i]->setSpeed(power);
        else motor[i]->setSpeed(0);
    }
}

void Robot::motorStop(double pwm)
{
//    motorSpeed = 0.5;
//    printf("%d\r\n", (int)(motorSpeed*10));
    for(i=0; i<4; i++) motor[i]->setSpeed(pwm);
}

void Robot::kickCheck()
{
    shot.outPut();
}

void Robot::dribbleCheck(float power)
{
    drib.setspeed(power);
}

void Robot::moveTest()
{
    while(1){
//        printf("%d %d ", (int)sensor_.irVal ,(int)sensor_.ballKeep);
        spin.setProcessValue(sensor_.angleLimit/*-sensor_.ballAngle*/);
        if(sensor_.team) sgoal.setProcessValue(-sensor_.blueAngle/*-sensor_.ballAngle*/);
        else sgoal.setProcessValue(-sensor_.yellowAngle/*-sensor_.ballAngle*/);
        spin_power = spin.compute();

//        theta = sensor_.ballAngle * ballExtra * PI / 180.0 * ((120-sensor_.ballRange)/240.0) * (fabs(sensor_.ballAngle) > 25.0) ;
//        theta =30 * sensor_.ballAngle*sensor_.ballAngle * 0.5 * PI / 180.0 / (1.5*sensor_.ballRange + 1);
//        theta = (sensor_.ballAngle / 180.0) * (sensor_.ballAngle / 180.0) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
        //        printf("%3d %3d %3d %3d\r\n",(int)sensor_.ballRange, (int)sensor_.ballAngle, (int)(theta * 180/PI), (int)(spin_power * 100));
        if(sensor_.line[0] || sensor_.line[1])
        {
            omni.computeCircular(1,PI/2.0,spin_power);
            lineflag = 1;
        }
        if(sensor_.line[2] || sensor_.line[3]) 
        {
            omni.computeCircular(1,-1*PI/2.0,spin_power);
            lineflag = 1;
        }
        if(sensor_.line[4])
        {
             omni.computeCircular(1,0,spin_power);
            lineflag = 1;
        }
        if(sensor_.line[5])
        {
             omni.computeCircular(1,PI,spin_power);
             lineflag = 1;
        }
        if(lineflag == 0 && !sensor_.ballkeepcount){
            theta = ((sensor_.ballAngle>0) - (sensor_.ballAngle<0)) * (2.0/(2-fabs(sensor_.ballAngle)/180) - 1) * (100 / (100 + sensor_.ballRange) - 0.5) * PI * 1.2;
            omni.computeCircular(/*0*/1.1*sensor_.ballRange / 100.0,/*0*/PI / 180.0 * sensor_.ballAngle + theta, spin_power);
        }
        else if(sensor_.ballkeepcount)
        {
        spin_power = sgoal.compute();
            if(sensor_.team)
            {
                omni.computeCircular(sensor_.blueRange / 100.0/*0.6*/,sensor_.blueAngle,spin_power);
            }
            else
            {
                omni.computeCircular(sensor_.yellowRange / 100.0/*0.6*/,sensor_.yellowAngle,spin_power);
            }
            if(lineflag) kickCheck();
        }
        lineflag = 0;
//        printf("r:%d %d l:%d %d b:%d f:%d\r\n",sensor_.line[0],sensor_.line[1],sensor_.line[2],sensor_.line[3],sensor_.line[4],sensor_.line[5]);
//        printf("%d %d %d %d\r\n",(int)(sensor_.b0),(int)sensor_.tgl1,(int)sensor_.tgl2,(int)sensor_.tgl3);
        for (int i=0; i<4; i++) {
            thisSpeed[i] = omni.wheel[i];
        }
        if(sensor_.b0) sensor_.jy.yawcalibrate();
        if((fabs(sensor_.ballAngle) < 50 && sensor_.ballRange) || sensor_.ballKeep) dribbleCheck(0.6);
        else dribbleCheck(0.0);
        motor[0]->setSpeed(thisSpeed[0]);
        motor[1]->setSpeed(thisSpeed[1]);
        motor[2]->setSpeed(-1*thisSpeed[2]);
        motor[3]->setSpeed(-1*thisSpeed[3]);
//        thread_sleep_for(40);
    }
}