re:pub
Dependents: 200_yotsuba_21 200_yotuba_21_uiChange
sensorGen.cpp@3:91064a3feb31, 2020-12-12 (annotated)
- Committer:
- piroro4560
- Date:
- Sat Dec 12 05:07:48 2020 +0000
- Revision:
- 3:91064a3feb31
- Parent:
- 2:10fa708a59e1
- Child:
- 4:f651e59a9805
mbed-os 6
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:10fa708a59e1 | 8 | _line[0] = new Line(line1,1500); |
piroro4560 | 2:10fa708a59e1 | 9 | _line[1] = new Line(line2,1500); |
piroro4560 | 2:10fa708a59e1 | 10 | _line[2] = new Line(line3,1500); |
piroro4560 | 2:10fa708a59e1 | 11 | _line[3] = new Line(line4,1500); |
piroro4560 | 2:10fa708a59e1 | 12 | _line[4] = new Line(line5,1500); |
piroro4560 | 2:10fa708a59e1 | 13 | _line[5] = new Line(line6,1500); |
piroro4560 | 0:03658c55ab2e | 14 | angleCount = 0; |
piroro4560 | 2:10fa708a59e1 | 15 | ballcount = 0; |
piroro4560 | 2:10fa708a59e1 | 16 | bluecount = 0; |
piroro4560 | 2:10fa708a59e1 | 17 | yellowcount = 0; |
piroro4560 | 0:03658c55ab2e | 18 | jy.calibrateAll(50); |
piroro4560 | 0:03658c55ab2e | 19 | limit.mode(PullUp); |
piroro4560 | 0:03658c55ab2e | 20 | thread.start(callback(this,&sensor::sensorloop)); |
piroro4560 | 0:03658c55ab2e | 21 | } |
piroro4560 | 0:03658c55ab2e | 22 | |
piroro4560 | 0:03658c55ab2e | 23 | void sensor::sensorloop() |
piroro4560 | 0:03658c55ab2e | 24 | { |
piroro4560 | 0:03658c55ab2e | 25 | while(1) |
piroro4560 | 0:03658c55ab2e | 26 | { |
piroro4560 | 0:03658c55ab2e | 27 | rawAngle = jy.getZaxisAngle(); |
piroro4560 | 0:03658c55ab2e | 28 | if(rawAngle > 180.0) rawAngle -= 360; |
piroro4560 | 0:03658c55ab2e | 29 | |
piroro4560 | 0:03658c55ab2e | 30 | angleLimit = rawAngle; |
piroro4560 | 0:03658c55ab2e | 31 | if((angleLimit - tempAngle) > 300) angleCount -= 1; |
piroro4560 | 0:03658c55ab2e | 32 | if((tempAngle - angleLimit) > 300) angleCount += 1; |
piroro4560 | 0:03658c55ab2e | 33 | angle = angleLimit + angleCount * 360.0; |
piroro4560 | 0:03658c55ab2e | 34 | tempAngle = angleLimit; |
piroro4560 | 2:10fa708a59e1 | 35 | |
piroro4560 | 3:91064a3feb31 | 36 | ballSig = camera.ballSignal(); |
piroro4560 | 3:91064a3feb31 | 37 | blueSig = camera.blueSignal(); |
piroro4560 | 3:91064a3feb31 | 38 | yellowSig = camera.yellowSignal(); |
piroro4560 | 3:91064a3feb31 | 39 | // if(camera.ballSignal()) ballcount=100; |
piroro4560 | 3:91064a3feb31 | 40 | // else ballcount--; |
piroro4560 | 3:91064a3feb31 | 41 | // if(camera.blueSignal()) bluecount=100; |
piroro4560 | 3:91064a3feb31 | 42 | // else bluecount--; |
piroro4560 | 3:91064a3feb31 | 43 | // if(camera.yellowSignal()) yellowcount=100; |
piroro4560 | 3:91064a3feb31 | 44 | // else yellowcount--; |
piroro4560 | 3:91064a3feb31 | 45 | // if(ballcount >= 200) ballcount = 200; |
piroro4560 | 3:91064a3feb31 | 46 | // else if(ballcount <= 0) ballcount = 0; |
piroro4560 | 3:91064a3feb31 | 47 | // if(yellowcount >= 200) yellowcount = 200; |
piroro4560 | 3:91064a3feb31 | 48 | // else if(yellowcount <= 0) yellowcount = 0; |
piroro4560 | 3:91064a3feb31 | 49 | // if(bluecount >= 200) bluecount = 200; |
piroro4560 | 3:91064a3feb31 | 50 | // else if(bluecount <= 0) bluecount = 0; |
piroro4560 | 3:91064a3feb31 | 51 | if(camera.ballSignal()) |
piroro4560 | 0:03658c55ab2e | 52 | { |
piroro4560 | 0:03658c55ab2e | 53 | ballTimeout.stop(); |
piroro4560 | 0:03658c55ab2e | 54 | ballTimeout.reset(); |
piroro4560 | 3:91064a3feb31 | 55 | ballTimeoutState = 0; |
piroro4560 | 0:03658c55ab2e | 56 | ballx = 168 - camera.ballX(); |
piroro4560 | 0:03658c55ab2e | 57 | bally = 128 - camera.ballY(); |
piroro4560 | 0:03658c55ab2e | 58 | } |
piroro4560 | 0:03658c55ab2e | 59 | else if(ballTimeoutState == 0) |
piroro4560 | 0:03658c55ab2e | 60 | { |
piroro4560 | 0:03658c55ab2e | 61 | ballTimeoutState = 1; |
piroro4560 | 0:03658c55ab2e | 62 | ballTimeout.start(); |
piroro4560 | 0:03658c55ab2e | 63 | } |
piroro4560 | 3:91064a3feb31 | 64 | else if(ballTimeout > 1.0) |
piroro4560 | 0:03658c55ab2e | 65 | { |
piroro4560 | 0:03658c55ab2e | 66 | ballx = 0; |
piroro4560 | 0:03658c55ab2e | 67 | bally = 0; |
piroro4560 | 0:03658c55ab2e | 68 | } |
piroro4560 | 3:91064a3feb31 | 69 | if(camera.blueSignal()) |
piroro4560 | 0:03658c55ab2e | 70 | { |
piroro4560 | 0:03658c55ab2e | 71 | blueTimeout.stop(); |
piroro4560 | 0:03658c55ab2e | 72 | blueTimeout.reset(); |
piroro4560 | 0:03658c55ab2e | 73 | blueTimeoutState = 0; |
piroro4560 | 0:03658c55ab2e | 74 | bluex = 168 - camera.blueX(); |
piroro4560 | 0:03658c55ab2e | 75 | bluey = 128 - camera.blueY(); |
piroro4560 | 0:03658c55ab2e | 76 | } |
piroro4560 | 0:03658c55ab2e | 77 | else if(blueTimeoutState == 0) |
piroro4560 | 0:03658c55ab2e | 78 | { |
piroro4560 | 0:03658c55ab2e | 79 | blueTimeoutState = 1; |
piroro4560 | 0:03658c55ab2e | 80 | blueTimeout.start(); |
piroro4560 | 0:03658c55ab2e | 81 | } |
piroro4560 | 2:10fa708a59e1 | 82 | else if(blueTimeout > 0.5) |
piroro4560 | 0:03658c55ab2e | 83 | { |
piroro4560 | 0:03658c55ab2e | 84 | bluex = 0; |
piroro4560 | 0:03658c55ab2e | 85 | bluey = 0; |
piroro4560 | 0:03658c55ab2e | 86 | } |
piroro4560 | 3:91064a3feb31 | 87 | if(camera.yellowSignal() ) |
piroro4560 | 0:03658c55ab2e | 88 | { |
piroro4560 | 0:03658c55ab2e | 89 | yellowTimeout.stop(); |
piroro4560 | 0:03658c55ab2e | 90 | yellowTimeout.reset(); |
piroro4560 | 0:03658c55ab2e | 91 | yellowTimeoutState = 0; |
piroro4560 | 0:03658c55ab2e | 92 | yellowx = 168 - camera.yellowX(); |
piroro4560 | 0:03658c55ab2e | 93 | yellowy = 128 - camera.yellowY(); |
piroro4560 | 0:03658c55ab2e | 94 | } |
piroro4560 | 0:03658c55ab2e | 95 | else if(yellowTimeoutState == 0) |
piroro4560 | 0:03658c55ab2e | 96 | { |
piroro4560 | 0:03658c55ab2e | 97 | yellowTimeoutState = 1; |
piroro4560 | 0:03658c55ab2e | 98 | yellowTimeout.start(); |
piroro4560 | 0:03658c55ab2e | 99 | } |
piroro4560 | 2:10fa708a59e1 | 100 | else if(yellowTimeout > 0.5) |
piroro4560 | 0:03658c55ab2e | 101 | { |
piroro4560 | 0:03658c55ab2e | 102 | yellowx = 0; |
piroro4560 | 0:03658c55ab2e | 103 | yellowy = 0; |
piroro4560 | 0:03658c55ab2e | 104 | } |
piroro4560 | 0:03658c55ab2e | 105 | |
piroro4560 | 0:03658c55ab2e | 106 | ballAngle = atan2(ballx,bally) * -180/PI; |
piroro4560 | 0:03658c55ab2e | 107 | ballRange = hypot(ballx,bally); |
piroro4560 | 0:03658c55ab2e | 108 | blueAngle = atan2(bluex,bluey) * -180/PI; |
piroro4560 | 0:03658c55ab2e | 109 | blueRange = hypot(bluex,bluey); |
piroro4560 | 0:03658c55ab2e | 110 | yellowAngle = atan2(yellowx,yellowy) * -180/PI; |
piroro4560 | 0:03658c55ab2e | 111 | yellowRange = hypot(yellowx,yellowy); |
piroro4560 | 0:03658c55ab2e | 112 | |
piroro4560 | 0:03658c55ab2e | 113 | for(int i=0; i < 6; i++) line[i] = _line[i]->judg(); |
piroro4560 | 0:03658c55ab2e | 114 | ballKeep = !limit; |
piroro4560 | 2:10fa708a59e1 | 115 | if (ballKeep && (ballkeepcount < 200)) { |
piroro4560 | 2:10fa708a59e1 | 116 | ballkeepcount++; |
piroro4560 | 2:10fa708a59e1 | 117 | } else if(ballkeepcount > 0) { |
piroro4560 | 2:10fa708a59e1 | 118 | ballkeepcount--; |
piroro4560 | 2:10fa708a59e1 | 119 | } |
piroro4560 | 0:03658c55ab2e | 120 | ThisThread::sleep_for(1000.0/300.0); |
piroro4560 | 0:03658c55ab2e | 121 | } |
piroro4560 | 0:03658c55ab2e | 122 | |
piroro4560 | 0:03658c55ab2e | 123 | } |