hirromu takaki / _sensorGen

re:pub

Dependents:   200_yotsuba_21 200_yotuba_21_uiChange

sensorGen.cpp@2:10fa708a59e1, 2020-02-01 (annotated)

Committer:
piroro4560
Date:
Sat Feb 01 15:12:37 2020 +0000
Revision:
2:10fa708a59e1
Parent:
1:c65c5282c05b
Child:
3:91064a3feb31
add line count

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 2:10fa708a59e1 36 if(camera.ballSignal()) ballcount+=2;
piroro4560 2:10fa708a59e1 37 else ballcount--;
piroro4560 2:10fa708a59e1 38 if(camera.blueSignal()) bluecount+=2;
piroro4560 2:10fa708a59e1 39 else bluecount--;
piroro4560 2:10fa708a59e1 40 if(camera.yellowSignal()) yellowcount+=2;
piroro4560 2:10fa708a59e1 41 else yellowcount--;
piroro4560 2:10fa708a59e1 42 if(ballcount >= 200) ballcount = 200;
piroro4560 2:10fa708a59e1 43 else if(ballcount <= 0) ballcount = 0;
piroro4560 2:10fa708a59e1 44 if(yellowcount >= 200) yellowcount = 200;
piroro4560 2:10fa708a59e1 45 else if(yellowcount <= 0) yellowcount = 0;
piroro4560 2:10fa708a59e1 46 if(bluecount >= 200) bluecount = 200;
piroro4560 2:10fa708a59e1 47 else if(bluecount <= 0) bluecount = 0;
piroro4560 2:10fa708a59e1 48 if((ballcount > 20 )&& ballTimeoutState==0)
piroro4560 0:03658c55ab2e 49 {
piroro4560 0:03658c55ab2e 50 ballTimeout.stop();
piroro4560 0:03658c55ab2e 51 ballTimeout.reset();
piroro4560 0:03658c55ab2e 52 ballx = 168 - camera.ballX();
piroro4560 0:03658c55ab2e 53 bally = 128 - camera.ballY();
piroro4560 0:03658c55ab2e 54 }
piroro4560 0:03658c55ab2e 55 else if(ballTimeoutState == 0)
piroro4560 0:03658c55ab2e 56 {
piroro4560 0:03658c55ab2e 57 ballTimeoutState = 1;
piroro4560 0:03658c55ab2e 58 ballTimeout.start();
piroro4560 0:03658c55ab2e 59 }
piroro4560 0:03658c55ab2e 60 else if(ballTimeout > 0.5)
piroro4560 0:03658c55ab2e 61 {
piroro4560 0:03658c55ab2e 62 ballx = 0;
piroro4560 0:03658c55ab2e 63 bally = 0;
piroro4560 0:03658c55ab2e 64 }
piroro4560 2:10fa708a59e1 65 if(bluecount > 20 && blueTimeoutState==0)
piroro4560 0:03658c55ab2e 66 {
piroro4560 0:03658c55ab2e 67 blueTimeout.stop();
piroro4560 0:03658c55ab2e 68 blueTimeout.reset();
piroro4560 0:03658c55ab2e 69 blueTimeoutState = 0;
piroro4560 0:03658c55ab2e 70 bluex = 168 - camera.blueX();
piroro4560 0:03658c55ab2e 71 bluey = 128 - camera.blueY();
piroro4560 0:03658c55ab2e 72 }
piroro4560 0:03658c55ab2e 73 else if(blueTimeoutState == 0)
piroro4560 0:03658c55ab2e 74 {
piroro4560 0:03658c55ab2e 75 blueTimeoutState = 1;
piroro4560 0:03658c55ab2e 76 blueTimeout.start();
piroro4560 0:03658c55ab2e 77 }
piroro4560 2:10fa708a59e1 78 else if(blueTimeout > 0.5)
piroro4560 0:03658c55ab2e 79 {
piroro4560 0:03658c55ab2e 80 bluex = 0;
piroro4560 0:03658c55ab2e 81 bluey = 0;
piroro4560 0:03658c55ab2e 82 }
piroro4560 2:10fa708a59e1 83 if(yellowcount > 20 && yellowTimeoutState == 0)
piroro4560 0:03658c55ab2e 84 {
piroro4560 0:03658c55ab2e 85 yellowTimeout.stop();
piroro4560 0:03658c55ab2e 86 yellowTimeout.reset();
piroro4560 0:03658c55ab2e 87 yellowTimeoutState = 0;
piroro4560 0:03658c55ab2e 88 yellowx = 168 - camera.yellowX();
piroro4560 0:03658c55ab2e 89 yellowy = 128 - camera.yellowY();
piroro4560 0:03658c55ab2e 90 }
piroro4560 0:03658c55ab2e 91 else if(yellowTimeoutState == 0)
piroro4560 0:03658c55ab2e 92 {
piroro4560 0:03658c55ab2e 93 yellowTimeoutState = 1;
piroro4560 0:03658c55ab2e 94 yellowTimeout.start();
piroro4560 0:03658c55ab2e 95 }
piroro4560 2:10fa708a59e1 96 else if(yellowTimeout > 0.5)
piroro4560 0:03658c55ab2e 97 {
piroro4560 0:03658c55ab2e 98 yellowx = 0;
piroro4560 0:03658c55ab2e 99 yellowy = 0;
piroro4560 0:03658c55ab2e 100 }
piroro4560 0:03658c55ab2e 101
piroro4560 0:03658c55ab2e 102 ballAngle = atan2(ballx,bally) * -180/PI;
piroro4560 0:03658c55ab2e 103 ballRange = hypot(ballx,bally);
piroro4560 0:03658c55ab2e 104 blueAngle = atan2(bluex,bluey) * -180/PI;
piroro4560 0:03658c55ab2e 105 blueRange = hypot(bluex,bluey);
piroro4560 0:03658c55ab2e 106 yellowAngle = atan2(yellowx,yellowy) * -180/PI;
piroro4560 0:03658c55ab2e 107 yellowRange = hypot(yellowx,yellowy);
piroro4560 0:03658c55ab2e 108
piroro4560 0:03658c55ab2e 109 for(int i=0; i < 6; i++) line[i] = _line[i]->judg();
piroro4560 0:03658c55ab2e 110 ballKeep = !limit;
piroro4560 2:10fa708a59e1 111 if (ballKeep && (ballkeepcount < 200)) {
piroro4560 2:10fa708a59e1 112 ballkeepcount++;
piroro4560 2:10fa708a59e1 113 } else if(ballkeepcount > 0) {
piroro4560 2:10fa708a59e1 114 ballkeepcount--;
piroro4560 2:10fa708a59e1 115 }
piroro4560 0:03658c55ab2e 116 ThisThread::sleep_for(1000.0/300.0);
piroro4560 0:03658c55ab2e 117 }
piroro4560 0:03658c55ab2e 118
piroro4560 0:03658c55ab2e 119 }