File content as of revision 2:10fa708a59e1:

#include "sensorGen.h"

sensor::sensor() 
:   camera(pixymosi, pixymiso, pixysclk),
    jy(jysda, jyscl),
    limit(limitPin)
{
    _line[0] = new Line(line1,1500);
    _line[1] = new Line(line2,1500);
    _line[2] = new Line(line3,1500);
    _line[3] = new Line(line4,1500);
    _line[4] = new Line(line5,1500);
    _line[5] = new Line(line6,1500);
    angleCount = 0;
    ballcount = 0;
    bluecount = 0;
    yellowcount = 0;
    jy.calibrateAll(50);
    limit.mode(PullUp);
    thread.start(callback(this,&sensor::sensorloop));
}

void sensor::sensorloop()
{
    while(1)
    {
        rawAngle = jy.getZaxisAngle();
        if(rawAngle > 180.0) rawAngle -= 360;

        angleLimit = rawAngle;
        if((angleLimit - tempAngle) > 300) angleCount -= 1;
        if((tempAngle - angleLimit) > 300) angleCount += 1;
        angle = angleLimit + angleCount * 360.0;         
        tempAngle = angleLimit;
        
        if(camera.ballSignal()) ballcount+=2;
        else ballcount--;
        if(camera.blueSignal()) bluecount+=2;
        else bluecount--;
        if(camera.yellowSignal()) yellowcount+=2;
        else yellowcount--;
        if(ballcount >= 200) ballcount = 200;
        else if(ballcount <= 0) ballcount = 0;
        if(yellowcount >= 200) yellowcount = 200;
        else if(yellowcount <= 0) yellowcount = 0;
        if(bluecount >= 200) bluecount = 200;
        else if(bluecount <= 0) bluecount = 0;
        if((ballcount > 20 )&& ballTimeoutState==0)
        {
            ballTimeout.stop();
            ballTimeout.reset();
            ballx = 168 - camera.ballX();
            bally = 128 - camera.ballY();
        }
        else if(ballTimeoutState == 0)
        {
            ballTimeoutState = 1;
            ballTimeout.start();
        }
        else if(ballTimeout > 0.5)
        {
            ballx = 0;
            bally = 0;
        }
        if(bluecount > 20 && blueTimeoutState==0)
        {
            blueTimeout.stop();
            blueTimeout.reset();
            blueTimeoutState = 0;
            bluex = 168 - camera.blueX();
            bluey = 128 - camera.blueY();
        }
        else if(blueTimeoutState == 0)
        {
            blueTimeoutState = 1;
            blueTimeout.start();
        }
        else if(blueTimeout > 0.5)
        {
            bluex = 0;
            bluey = 0;
        }
        if(yellowcount > 20 && yellowTimeoutState == 0)
        {
            yellowTimeout.stop();
            yellowTimeout.reset();
            yellowTimeoutState = 0;
            yellowx = 168 - camera.yellowX();
            yellowy = 128 - camera.yellowY();
        }
        else if(yellowTimeoutState == 0)
        {
            yellowTimeoutState = 1;
            yellowTimeout.start();
        }
        else if(yellowTimeout > 0.5)
        {
            yellowx = 0;
            yellowy = 0;
        }
        
        ballAngle = atan2(ballx,bally) * -180/PI;
        ballRange = hypot(ballx,bally);
        blueAngle = atan2(bluex,bluey) * -180/PI;
        blueRange = hypot(bluex,bluey);
        yellowAngle = atan2(yellowx,yellowy) * -180/PI;
        yellowRange = hypot(yellowx,yellowy);
        
        for(int i=0; i < 6; i++) line[i] = _line[i]->judg();
        ballKeep = !limit;
        if (ballKeep && (ballkeepcount < 200)) {
            ballkeepcount++;
        } else if(ballkeepcount > 0) {
            ballkeepcount--;
        }
        ThisThread::sleep_for(1000.0/300.0);
    }

}