File content as of revision 4:a4f94f186ba0:

#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.2,0.2);
    spin.setBias(0);
    spin.setMode(1);
    spin.setSetPoint(0);
    shot.setkickperiod(0.8);
    shot.setoutputtime(0.01);
    drib.setspeed(0.0);
    but.mode(PullUp);
    
}

void Robot::start()
{
    
    while (true) {
        if (!b2) 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(dip1)
            {
                shot.outPut();
                if(!sensor.bluex) omni.computeCircular(1,0, spin_power);
                omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, -spin_power);
        
            }
            else
            {
                shot.outPut();
                if(!sensor.yellowx) omni.computeCircular(1,0, spin_power);
                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");
        
            omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power);
        }
        pc.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];
            motor[0]->setSpeed(omni.wheel[0]);
            motor[1]->setSpeed(omni.wheel[1]);
            motor[2]->setSpeed(-omni2wheel);
            motor[3]->setSpeed(omni.wheel[3]);
        } 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();
                }
            }
        }
            
    }
}