hirromu takaki / _sensorGen

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?

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 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 }