File content as of revision 11:39f44a1dc256:

#include "robot.h"

Robot::Robot() :
      spin(P, I, D, interval),
      pc(USBTX, USBRX, 115200),
      shot(kicker),
      drib(ESCpin)
{
    
    omni.wheel[0].setRadian(PI/4 * 1);
    omni.wheel[1].setRadian(PI/4 * 3);
    omni.wheel[2].setRadian(PI/4 * 5);
    omni.wheel[3].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);
    spin.setOutputLimits(-0.4,0.4);
    spin.setBias(0);
    spin.setMode(1);
    spin.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) {
        startb=1;
        spin.setProcessValue(sensor_.angleLimit);
        theta = sensor_.ballAngle * ballExtra * PI / 180.0;
        spin_power = spin.compute();
        if (sensor_.ballKeep) {
            /*ボールをつかんでるなら*/
//            drib.setspeed(0.5);
            if(Team)
            {
                /*あおに向かうよ*/
                /*if(!sensor.bluex)*/ omni.computeCircular(1,0,0/* spin_power*/);
//                else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power);
        
            }
            else
            {
                /*きいろに向かうよ*/
                /*if(!sensor.yellowx)*/ omni.computeCircular(1,0,0/* spin_power*/);
                /*else*/ omni.computeCircular(sensor_.yellowRange,PI / 180.0 * sensor_.yellowAngle, 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);
        }
//        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::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);
}

//        theta = sensor.ballAngle * ballExtra * PI / 180.0;
//            omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power);