hirromu takaki / _sensorGen

re:pub

Dependents:   200_yotsuba_21 200_yotuba_21_uiChange

sensorGen.cpp@0:03658c55ab2e, 2020-01-31 (annotated)

Committer:
piroro4560
Date:
Fri Jan 31 09:20:51 2020 +0000
Revision:
0:03658c55ab2e
Child:
1:c65c5282c05b
sensorGen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 0:03658c55ab2e 1 #include "sensorGen.h"
piroro4560 0:03658c55ab2e 2
piroro4560 0:03658c55ab2e 3 sensor::sensor()
piroro4560 0:03658c55ab2e 4 : camera(pixymosi, pixymiso, pixysclk),
piroro4560 0:03658c55ab2e 5 jy(jysda, jyscl),
piroro4560 0:03658c55ab2e 6 limit(limitPin)
piroro4560 0:03658c55ab2e 7 {
piroro4560 0:03658c55ab2e 8 _line[0] = new Line(line1,2000);
piroro4560 0:03658c55ab2e 9 _line[1] = new Line(line2,2000);
piroro4560 0:03658c55ab2e 10 _line[2] = new Line(line3,2000);
piroro4560 0:03658c55ab2e 11 _line[3] = new Line(line4,2000);
piroro4560 0:03658c55ab2e 12 _line[4] = new Line(line5,2000);
piroro4560 0:03658c55ab2e 13 _line[5] = new Line(line6,2000);
piroro4560 0:03658c55ab2e 14 angleCount = 0;
piroro4560 0:03658c55ab2e 15 jy.calibrateAll(50);
piroro4560 0:03658c55ab2e 16 limit.mode(PullUp);
piroro4560 0:03658c55ab2e 17 thread.start(callback(this,&sensor::sensorloop));
piroro4560 0:03658c55ab2e 18 }
piroro4560 0:03658c55ab2e 19
piroro4560 0:03658c55ab2e 20 void sensor::sensorloop()
piroro4560 0:03658c55ab2e 21 {
piroro4560 0:03658c55ab2e 22 while(1)
piroro4560 0:03658c55ab2e 23 {
piroro4560 0:03658c55ab2e 24 rawAngle = jy.getZaxisAngle();
piroro4560 0:03658c55ab2e 25 if(rawAngle > 180.0) rawAngle -= 360;
piroro4560 0:03658c55ab2e 26
piroro4560 0:03658c55ab2e 27 angleLimit = rawAngle;
piroro4560 0:03658c55ab2e 28 if((angleLimit - tempAngle) > 300) angleCount -= 1;
piroro4560 0:03658c55ab2e 29 if((tempAngle - angleLimit) > 300) angleCount += 1;
piroro4560 0:03658c55ab2e 30 angle = angleLimit + angleCount * 360.0;
piroro4560 0:03658c55ab2e 31 tempAngle = angleLimit;
piroro4560 0:03658c55ab2e 32 if(camera.ballSignal())
piroro4560 0:03658c55ab2e 33 {
piroro4560 0:03658c55ab2e 34 ballTimeout.stop();
piroro4560 0:03658c55ab2e 35 ballTimeout.reset();
piroro4560 0:03658c55ab2e 36 ballTimeoutState = 0;
piroro4560 0:03658c55ab2e 37 ballx = 168 - camera.ballX();
piroro4560 0:03658c55ab2e 38 bally = 128 - camera.ballY();
piroro4560 0:03658c55ab2e 39 }
piroro4560 0:03658c55ab2e 40 else if(ballTimeoutState == 0)
piroro4560 0:03658c55ab2e 41 {
piroro4560 0:03658c55ab2e 42 ballTimeoutState = 1;
piroro4560 0:03658c55ab2e 43 ballTimeout.start();
piroro4560 0:03658c55ab2e 44 }
piroro4560 0:03658c55ab2e 45 else if(ballTimeout > 0.5)
piroro4560 0:03658c55ab2e 46 {
piroro4560 0:03658c55ab2e 47 ballx = 0;
piroro4560 0:03658c55ab2e 48 bally = 0;
piroro4560 0:03658c55ab2e 49 }
piroro4560 0:03658c55ab2e 50 if(camera.blueSignal())
piroro4560 0:03658c55ab2e 51 {
piroro4560 0:03658c55ab2e 52 blueTimeout.stop();
piroro4560 0:03658c55ab2e 53 blueTimeout.reset();
piroro4560 0:03658c55ab2e 54 blueTimeoutState = 0;
piroro4560 0:03658c55ab2e 55 bluex = 168 - camera.blueX();
piroro4560 0:03658c55ab2e 56 bluey = 128 - camera.blueY();
piroro4560 0:03658c55ab2e 57 }
piroro4560 0:03658c55ab2e 58 else if(blueTimeoutState == 0)
piroro4560 0:03658c55ab2e 59 {
piroro4560 0:03658c55ab2e 60 blueTimeoutState = 1;
piroro4560 0:03658c55ab2e 61 blueTimeout.start();
piroro4560 0:03658c55ab2e 62 }
piroro4560 0:03658c55ab2e 63 else if(ballTimeout > 0.5)
piroro4560 0:03658c55ab2e 64 {
piroro4560 0:03658c55ab2e 65 bluex = 0;
piroro4560 0:03658c55ab2e 66 bluey = 0;
piroro4560 0:03658c55ab2e 67 }
piroro4560 0:03658c55ab2e 68 if(camera.yellowSignal())
piroro4560 0:03658c55ab2e 69 {
piroro4560 0:03658c55ab2e 70 yellowTimeout.stop();
piroro4560 0:03658c55ab2e 71 yellowTimeout.reset();
piroro4560 0:03658c55ab2e 72 yellowTimeoutState = 0;
piroro4560 0:03658c55ab2e 73 yellowx = 168 - camera.yellowX();
piroro4560 0:03658c55ab2e 74 yellowy = 128 - camera.yellowY();
piroro4560 0:03658c55ab2e 75 }
piroro4560 0:03658c55ab2e 76 else if(yellowTimeoutState == 0)
piroro4560 0:03658c55ab2e 77 {
piroro4560 0:03658c55ab2e 78 yellowTimeoutState = 1;
piroro4560 0:03658c55ab2e 79 yellowTimeout.start();
piroro4560 0:03658c55ab2e 80 }
piroro4560 0:03658c55ab2e 81 else if(ballTimeout > 0.5)
piroro4560 0:03658c55ab2e 82 {
piroro4560 0:03658c55ab2e 83 yellowx = 0;
piroro4560 0:03658c55ab2e 84 yellowy = 0;
piroro4560 0:03658c55ab2e 85 }
piroro4560 0:03658c55ab2e 86
piroro4560 0:03658c55ab2e 87 ballAngle = atan2(ballx,bally) * -180/PI;
piroro4560 0:03658c55ab2e 88 ballRange = hypot(ballx,bally);
piroro4560 0:03658c55ab2e 89 blueAngle = atan2(bluex,bluey) * -180/PI;
piroro4560 0:03658c55ab2e 90 blueRange = hypot(bluex,bluey);
piroro4560 0:03658c55ab2e 91 yellowAngle = atan2(yellowx,yellowy) * -180/PI;
piroro4560 0:03658c55ab2e 92 yellowRange = hypot(yellowx,yellowy);
piroro4560 0:03658c55ab2e 93
piroro4560 0:03658c55ab2e 94 for(int i=0; i < 6; i++) line[i] = _line[i]->judg();
piroro4560 0:03658c55ab2e 95 ballKeep = !limit;
piroro4560 0:03658c55ab2e 96 ThisThread::sleep_for(1000.0/300.0);
piroro4560 0:03658c55ab2e 97 }
piroro4560 0:03658c55ab2e 98
piroro4560 0:03658c55ab2e 99 }