2532
Dependencies: QEI WS2812 PixelArray DFPlayerMini MODSERIAL PCA9685_ pca9685
Diff: main.cpp
- Revision:
- 4:2f4d485ebe58
- Parent:
- 2:ed1f45f9b06a
- Child:
- 5:cec7f85ce6c4
diff -r da339cc84c9f -r 2f4d485ebe58 main.cpp --- a/main.cpp Sat Jun 22 17:25:59 2019 +0000 +++ b/main.cpp Sat Jun 22 17:57:41 2019 +0000 @@ -18,7 +18,7 @@ #define SERVOSTART 544 #define SERVOEND 2400 #define MESSAGE_BUFFER_SIZE 32 - +#define MIN_DIST 35 Semaphore messageReceived(0); DigitalOut led1(LED1); @@ -182,8 +182,8 @@ //if (activated) { - HSV.s=0.1; - HSV.v=abs(sin(t.read()*1.11)); + HSV.s=1;//0.1; + HSV.v=0.1;//abs(sin(t.read()*1.11)); HSV.h=abs(sin(t.read()/30))*360; RGB = hsv2rgb(HSV); int r =RGB.r*255.0; @@ -220,11 +220,13 @@ if (ang>servoLimitsEnd[num] && ang>servoLimitsStart[num]) { //ang = servoLimitsEnd[num]; - pc.printf("out of range %d %3.2f\n",num,ang);} + // pc.printf("out of range %d %3.2f\n",num,ang); + } if (ang<servoLimitsStart[num] && ang<servoLimitsEnd[num]) { //ang = servoLimitsStart[num]; - pc.printf("out of range %d %3.2f\n",num,ang);} + // pc.printf("out of range %d %3.2f\n",num,ang); + } //m.lock(); pwmouts.set_pwm_pw(num,servoang(ang)); //m.unlock(); @@ -318,22 +320,40 @@ Action act = NONE; char dist[12]; int activated=-1; - wait(2); + wait(2.5); mp3.mp3_stop(); + int dist_tmp[16][4]; while(1) { memset(dist,0,12); i2c.read(0x20,dist,12); activated=-1; + for(uint8_t j=3; j>0; j--) + { + pc.printf("\n**"); + for(uint8_t i=0; i<12; i++) + { + dist_tmp[i][j]=dist_tmp[i][j-1]; + // pc.printf("%d\t ",dist_tmp[i][j]); + } + + } + //pc.printf("\n"); for(uint8_t i=0; i<12; i++) { pc.printf("\t %d",dist[i]); - if (dist[i]<35) { - activated=sensormap[i]; + dist_tmp[i][0]=dist[i]; + if (dist_tmp[i][0]<MIN_DIST && + dist_tmp[i][1]<MIN_DIST && + dist_tmp[i][2]<MIN_DIST && + dist_tmp[i][3]<MIN_DIST ) { + activated=sensormap[i]; - } + } //else activated=-1; } + + pc.printf("\n"); if (activated>=0)