File content as of revision 2:e3f387b9d050:

#include "sensorGen.h"

sensor::sensor() 
:   camera(pixymosi, pixymiso, pixysclk),
    jy(jysda, jyscl),
    limit(limitPin)
{
    _line[0] = new Line(line1,1800/*-500*/);
    _line[1] = new Line(line2,1800/*-500*/);
    _line[2] = new Line(line3,1800/*-500*/);
    _line[3] = new Line(line4,1800/*-500*/);
    _line[4] = new Line(line5,1800)/*-500*/;
    _line[5] = new Line(line6,1800/*-500*/);
    angleCount = 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())
        {
            ballTimeout.stop();
            ballTimeout.reset();
            ballTimeoutState = 0;
            ballx = 168 - camera.ballX();
            bally = 128 - camera.ballY();
        }
        else if(ballTimeoutState == 0)
        {
            ballTimeoutState = 1;
            ballTimeout.start();
        }
        else if(ballTimeout > 2.5)
        {
            ballx = 0;
            bally = 0;
        }
        if(camera.blueSignal())
        {
            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 > 3.5)
        {
            bluex = 0;
            bluey = 0;
        }
        if(camera.yellowSignal())
        {
            yellowTimeout.stop();
            yellowTimeout.reset();
            yellowTimeoutState = 0;
            yellowx = 168 - camera.yellowX();
            yellowy = 128 - camera.yellowY();
        }
        else if(yellowTimeoutState == 0)
        {
            yellowTimeoutState = 1;
            yellowTimeout.start();
        }
        else if(ballTimeout > 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;
        ThisThread::sleep_for(1000.0/300.0);
    }

}