hirromu takaki / _sensorGen

re:pub

Dependents:   200_yotsuba_21 200_yotuba_21_uiChange

sensorGen.cpp@5:8ed76df0196b, 2021-03-04 (annotated)

Committer:
THtakahiro702286
Date:
Thu Mar 04 14:20:35 2021 +0000
Revision:
5:8ed76df0196b
Parent:
4:f651e59a9805
Child:
6:fc6e4c814d3f
3/4 change;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 0:03658c55ab2e 1 #include "sensorGen.h"
piroro4560 0:03658c55ab2e 2
piroro4560 4:f651e59a9805 3 sensorgen::sensorgen()
piroro4560 0:03658c55ab2e 4 : camera(pixymosi, pixymiso, pixysclk),
piroro4560 4:f651e59a9805 5 jy(jysda, jyscl)
piroro4560 0:03658c55ab2e 6 {
piroro4560 4:f651e59a9805 7 _line[0] = new Line(line1,1000);
piroro4560 4:f651e59a9805 8 _line[1] = new Line(line2,1000);
piroro4560 4:f651e59a9805 9 _line[2] = new Line(line3,1000);
piroro4560 4:f651e59a9805 10 _line[3] = new Line(line4,1000);
piroro4560 4:f651e59a9805 11 _line[4] = new Line(line5,1000);
piroro4560 4:f651e59a9805 12 _line[5] = new Line(line6,1000);
piroro4560 0:03658c55ab2e 13 angleCount = 0;
piroro4560 2:10fa708a59e1 14 ballcount = 0;
piroro4560 2:10fa708a59e1 15 bluecount = 0;
piroro4560 2:10fa708a59e1 16 yellowcount = 0;
piroro4560 0:03658c55ab2e 17 jy.calibrateAll(50);
THtakahiro702286 5:8ed76df0196b 18 thread.start(callback(this,&sensorgen::sensorloop));
piroro4560 0:03658c55ab2e 19 }
piroro4560 0:03658c55ab2e 20
piroro4560 4:f651e59a9805 21 void sensorgen::sensorloop()
piroro4560 0:03658c55ab2e 22 {
piroro4560 0:03658c55ab2e 23 while(1)
piroro4560 0:03658c55ab2e 24 {
piroro4560 0:03658c55ab2e 25 rawAngle = jy.getZaxisAngle();
piroro4560 0:03658c55ab2e 26 if(rawAngle > 180.0) rawAngle -= 360;
piroro4560 0:03658c55ab2e 27
piroro4560 0:03658c55ab2e 28 angleLimit = rawAngle;
piroro4560 0:03658c55ab2e 29 if((angleLimit - tempAngle) > 300) angleCount -= 1;
piroro4560 0:03658c55ab2e 30 if((tempAngle - angleLimit) > 300) angleCount += 1;
piroro4560 0:03658c55ab2e 31 angle = angleLimit + angleCount * 360.0;
piroro4560 0:03658c55ab2e 32 tempAngle = angleLimit;
piroro4560 2:10fa708a59e1 33
piroro4560 3:91064a3feb31 34 ballSig = camera.ballSignal();
piroro4560 3:91064a3feb31 35 blueSig = camera.blueSignal();
piroro4560 3:91064a3feb31 36 yellowSig = camera.yellowSignal();
piroro4560 4:f651e59a9805 37 /*見失ったらカウントが減っていくよ*/
piroro4560 3:91064a3feb31 38 // if(camera.ballSignal()) ballcount=100;
piroro4560 3:91064a3feb31 39 // else ballcount--;
piroro4560 3:91064a3feb31 40 // if(camera.blueSignal()) bluecount=100;
piroro4560 3:91064a3feb31 41 // else bluecount--;
piroro4560 3:91064a3feb31 42 // if(camera.yellowSignal()) yellowcount=100;
piroro4560 3:91064a3feb31 43 // else yellowcount--;
piroro4560 3:91064a3feb31 44 // if(ballcount >= 200) ballcount = 200;
piroro4560 3:91064a3feb31 45 // else if(ballcount <= 0) ballcount = 0;
piroro4560 3:91064a3feb31 46 // if(yellowcount >= 200) yellowcount = 200;
piroro4560 3:91064a3feb31 47 // else if(yellowcount <= 0) yellowcount = 0;
piroro4560 3:91064a3feb31 48 // if(bluecount >= 200) bluecount = 200;
piroro4560 3:91064a3feb31 49 // else if(bluecount <= 0) bluecount = 0;
piroro4560 3:91064a3feb31 50 if(camera.ballSignal())
piroro4560 0:03658c55ab2e 51 {
piroro4560 0:03658c55ab2e 52 ballTimeout.stop();
piroro4560 0:03658c55ab2e 53 ballTimeout.reset();
piroro4560 3:91064a3feb31 54 ballTimeoutState = 0;
THtakahiro702286 5:8ed76df0196b 55 ballx = CAMCENTERX - camera.ballX();
THtakahiro702286 5:8ed76df0196b 56 bally = CAMCENTERY - camera.ballY();
piroro4560 0:03658c55ab2e 57 }
piroro4560 0:03658c55ab2e 58 else if(ballTimeoutState == 0)
piroro4560 0:03658c55ab2e 59 {
piroro4560 0:03658c55ab2e 60 ballTimeoutState = 1;
piroro4560 0:03658c55ab2e 61 ballTimeout.start();
piroro4560 0:03658c55ab2e 62 }
piroro4560 3:91064a3feb31 63 else if(ballTimeout > 1.0)
piroro4560 0:03658c55ab2e 64 {
piroro4560 0:03658c55ab2e 65 ballx = 0;
piroro4560 0:03658c55ab2e 66 bally = 0;
piroro4560 0:03658c55ab2e 67 }
piroro4560 3:91064a3feb31 68 if(camera.blueSignal())
piroro4560 0:03658c55ab2e 69 {
piroro4560 0:03658c55ab2e 70 blueTimeout.stop();
piroro4560 0:03658c55ab2e 71 blueTimeout.reset();
piroro4560 0:03658c55ab2e 72 blueTimeoutState = 0;
THtakahiro702286 5:8ed76df0196b 73 bluex = CAMCENTERX - camera.blueX();
THtakahiro702286 5:8ed76df0196b 74 bluey = CAMCENTERY - camera.blueY();
piroro4560 0:03658c55ab2e 75 }
piroro4560 0:03658c55ab2e 76 else if(blueTimeoutState == 0)
piroro4560 0:03658c55ab2e 77 {
piroro4560 0:03658c55ab2e 78 blueTimeoutState = 1;
piroro4560 0:03658c55ab2e 79 blueTimeout.start();
piroro4560 0:03658c55ab2e 80 }
piroro4560 2:10fa708a59e1 81 else if(blueTimeout > 0.5)
piroro4560 0:03658c55ab2e 82 {
piroro4560 0:03658c55ab2e 83 bluex = 0;
piroro4560 0:03658c55ab2e 84 bluey = 0;
piroro4560 0:03658c55ab2e 85 }
piroro4560 3:91064a3feb31 86 if(camera.yellowSignal() )
piroro4560 0:03658c55ab2e 87 {
piroro4560 0:03658c55ab2e 88 yellowTimeout.stop();
piroro4560 0:03658c55ab2e 89 yellowTimeout.reset();
piroro4560 0:03658c55ab2e 90 yellowTimeoutState = 0;
THtakahiro702286 5:8ed76df0196b 91 yellowx = CAMCENTERX - camera.yellowX();
THtakahiro702286 5:8ed76df0196b 92 yellowy = CAMCENTERY - camera.yellowY();
piroro4560 0:03658c55ab2e 93 }
piroro4560 0:03658c55ab2e 94 else if(yellowTimeoutState == 0)
piroro4560 0:03658c55ab2e 95 {
piroro4560 0:03658c55ab2e 96 yellowTimeoutState = 1;
piroro4560 0:03658c55ab2e 97 yellowTimeout.start();
piroro4560 0:03658c55ab2e 98 }
piroro4560 2:10fa708a59e1 99 else if(yellowTimeout > 0.5)
piroro4560 0:03658c55ab2e 100 {
piroro4560 0:03658c55ab2e 101 yellowx = 0;
piroro4560 0:03658c55ab2e 102 yellowy = 0;
piroro4560 0:03658c55ab2e 103 }
piroro4560 0:03658c55ab2e 104
piroro4560 0:03658c55ab2e 105 ballAngle = atan2(ballx,bally) * -180/PI;
THtakahiro702286 5:8ed76df0196b 106 ballRange = hypot(ballx,bally) / 150.0;
piroro4560 0:03658c55ab2e 107 blueAngle = atan2(bluex,bluey) * -180/PI;
THtakahiro702286 5:8ed76df0196b 108 blueRange = hypot(bluex,bluey) / 150.0;
piroro4560 0:03658c55ab2e 109 yellowAngle = atan2(yellowx,yellowy) * -180/PI;
THtakahiro702286 5:8ed76df0196b 110 yellowRange = hypot(yellowx,yellowy) / 150.0;
piroro4560 0:03658c55ab2e 111
piroro4560 0:03658c55ab2e 112 for(int i=0; i < 6; i++) line[i] = _line[i]->judg();
piroro4560 4:f651e59a9805 113 ballKeep = 0;
piroro4560 2:10fa708a59e1 114 if (ballKeep && (ballkeepcount < 200)) {
piroro4560 2:10fa708a59e1 115 ballkeepcount++;
piroro4560 2:10fa708a59e1 116 } else if(ballkeepcount > 0) {
piroro4560 2:10fa708a59e1 117 ballkeepcount--;
piroro4560 2:10fa708a59e1 118 }
piroro4560 4:f651e59a9805 119 ThisThread::sleep_for(1000/300);
piroro4560 0:03658c55ab2e 120 }
piroro4560 0:03658c55ab2e 121
piroro4560 0:03658c55ab2e 122 }