File content as of revision 6:dee6041c3d15:

#include "robot.h"

Robot::Robot() :
      spin(P, I, D, interval),
      pc(USBTX, USBRX, 115200),
      shot(kicker),
      drib(ESC),
      
      but(USER_BUTTON),
      dip1(dip1in),
      dip2(dip2in),
      dip3(dip3in),
      b1(b1in),
      b2(b2in),
      lcd(PB_9, PB_8)
      
{
    
    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(motor1);
    motor[1] = new KohiMD(motor2);
    motor[2] = new KohiMD(motor3);
    motor[3] = new KohiMD(motor4);
    spin.setInputLimits(-180, 180);
    spin.setOutputLimits(-0.4,0.4);
    spin.setBias(0);
    spin.setMode(1);
    spin.setSetPoint(0);
    shot.setkickperiod(0.8);
    shot.setoutputtime(0.01);
    drib.setspeed(0.0);
    but.mode(PullUp);
    startb = 0;
    linecount[0] = 0;
    linecount[1] = 0;
    linecount[2] = 0;
    linecount[3] = 0;
    lineAngle = 0;
}

void Robot::start()
{
    
    while (true) {
        pc.printf("%f\r\n", sensor.angle );
//        sensor.blueAngle, sensor.blueRange/*, sensor.yellowAngle, sensor.yellowRange, sensor.ballAngle, sensor.ballRange */);
        if (!/*but*/2) startb=1;
        spin.setProcessValue(sensor.angleLimit);
        theta = sensor.ballAngle * ballExtra * PI / 180.0;
        spin_power = spin.compute();
        if (sensor.ballkeepcount > 100) {
            drib.setspeed(0.5); 
            shot.outPut();
            if(dip1)
            {
//                shot.outPut();
                if(!sensor.bluex) omni.computeCircular(1,0, spin_power);
                else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power);
            }
            else
            {
//                shot.outPut();
                if(!sensor.yellowx) omni.computeCircular(1,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);
        }
        
//        pc.printf("%d ",(int)sensor.ballKeep);

        if (sensor.ballTimeout > 1.0 && sensor.ballTimeout < 2.0)
        {
            omni.computeCircular(1,PI,spin_power);
        }
        
        if(sensor.line[0] || sensor.line[1])
        {
            linecount[0]+=2;
        }
        else
        {
               linecount[0]--;
        }
        if(linecount[0] && !(linecount[1] + linecount[2] + linecount[3]) ) lineAngle = -1*PI/2.0;
        if(sensor.line[2] || sensor.line[3]) 
        {
            linecount[1]+=2;
        }
        else
        {
            linecount[1]--;
        }
        if(linecount[1] && !(linecount[0] + linecount[2] + linecount[3])) lineAngle = PI/2;
        if(sensor.line[4])
        {
            linecount[2]+=2;
        }
        else
        {
            linecount[2]--;    
        }
        if(linecount[2] && !(linecount[1] + linecount[0] + linecount[3])) lineAngle = 0;
        if(sensor.line[5])
        {
            linecount[3]+=2;
        }
        else
        {
            linecount[3]-=2;
        }
        if(linecount[3] && !(linecount[1] + linecount[2] + linecount[0])) lineAngle = PI;
        
        if(!(linecount[0] + linecount[1] + linecount[2] + linecount[3])) lineAngle = 0;
       
        for(int i = 0; i < 4; i++)
        {
            if(linecount[i] > 50) linecount[i] = 50;
            if(linecount[i] < 0) linecount[i] = 0;
        }
        if(linecount[0] + linecount[1] + linecount[2] + linecount[3]) omni.computeCircular(1,lineAngle,spin_power);
        //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) {
            omni0wheel = omni.wheel[0];
            omni1wheel = omni.wheel[1];
            omni2wheel = omni.wheel[2];
            omni3wheel = omni.wheel[3];
            motor[0]->setSpeed(omni0wheel);
            motor[1]->setSpeed(omni1wheel);
            motor[2]->setSpeed(omni2wheel);///////////////////////////////////
            motor[3]->setSpeed(omni3wheel);
//            motor[3]->setSpeed(-omni3wheel);
        } else {
            motor[0]->setSpeed(0);
            motor[1]->setSpeed(0);
            motor[2]->setSpeed(0);
            motor[3]->setSpeed(0);
        }
        
        
        if (!startb) {
            if (!b1) {
                if (dip3) {
                    sensor.jy.jyroReset();
                } else {
                    shot.outPut();
                }
            }
        }
            
    }
}