hirromu takaki / _sensorGen

re:pub

Dependents:   200_yotsuba_21 200_yotuba_21_uiChange

sensorGen.cpp@9:9b1381c4ad90, 2021-03-07 (annotated)

Committer:
THtakahiro702286
Date:
Sun Mar 07 12:24:01 2021 +0000
Revision:
9:9b1381c4ad90
Parent:
8:e11be46f5480 
victory!!!!

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