2532
Dependencies: QEI WS2812 PixelArray DFPlayerMini MODSERIAL PCA9685_ pca9685
main.cpp
- Committer:
- dimavb
- Date:
- 2019-06-26
- Revision:
- 6:4b007c7f0b7a
- Parent:
- 5:cec7f85ce6c4
File content as of revision 6:4b007c7f0b7a:
/* mbed Microcontroller Library * Copyright (c) 2018 ARM Limited * SPDX-License-Identifier: Apache-2.0 */ #include "mbed.h" //#include "stats_report.h" #include "PCA9685.h" #include "QEI.h" #include "WS2812.h" #include "PixelArray.h" #include "DebounceIn.h" #include "DFPlayerMini.h" #include "MODSERIAL.h" #include "rgbhsv.h" #define SERVOSTART 544 #define SERVOEND 2400 #define MESSAGE_BUFFER_SIZE 32 #define MIN_DIST 35 Semaphore messageReceived(0); DigitalOut led1(LED1); I2C i2c (PB_9,PB_8); PCA9685 pwmouts(0x41<<1,&i2c,60); WS2812 ws(PB_15,160, 0, 18, 10, 15); DFPlayerMini mp3 (PA_9, PA_10); MODSERIAL pc (USBTX, USBRX); PixelArray px(160); char messageBufferIncoming[MESSAGE_BUFFER_SIZE]; enum Action { NONE, MUS, MLIT, SLOW, OPEN, KACH }; const uint8_t sensormap[16] = {0,1,2,3,4,5,10,7,8,9,6,11}; //const float actionstime[16]={5,5,5,5,5,5,5,5,2,5,5,5,5,5,5,5}; const Action actions[16]= {MLIT,MUS,OPEN,OPEN,KACH,OPEN,OPEN,OPEN,SLOW,OPEN,KACH,OPEN}; const float actionstime[16] = {5, 5, 5, 5, 7, 3, 5, 5, 2, 5, 7, 6, 5, 5, 5, 5}; const uint8_t servoLimitsStart[16]= {20, 85, 27, 35, 0, 23, 155, 106, 150, 45, 80, 136, 0, 0, 0, 0}; const uint8_t servoLimitsCenter[16]={31, 108, 27, 35, 0, 23, 155, 106, 150, 45, 85, 136, 0, 0, 0, 0}; const uint8_t servoLimitsEnd[16]= {40, 135, 45, 90, 180, 90, 70, 75, 40, 120, 90, 55, 180, 180, 180, 180}; volatile int lastpos=0; volatile int activated=0; volatile int idle=0; void setservo(int num, float ang); Timer at; void slow_move(int id,float time) { //time =1.8; at.reset(); at.start(); mp3.mp3_play(id+1); while(at.read()<time) { setservo(id,servoLimitsStart[id]+(servoLimitsEnd[id]-servoLimitsStart[id])*(at.read()/time)); float rw=rand()%20; wait(0.03+rw/100); } setservo(id,servoLimitsCenter[id]); } void kachelle(int id,float time) { at.reset(); at.start(); mp3.mp3_play(id+1); while(at.read()<time) { setservo(id,servoLimitsCenter[id]+sin(at.read()*2)*(servoLimitsEnd[id]-servoLimitsStart[id])); wait(0.03); } setservo(id,servoLimitsCenter[id]); } void kachelle_double(int id1,int id2,int idm, float time) { at.reset(); at.start(); mp3.mp3_play(idm); while(at.read()<time) { //setservo(id2,servoLimitsCenter[id2]+(1-sin(at.read()*3))*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2); setservo(id2,servoLimitsCenter[id2]+sin(at.read()*3)*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2); setservo(id1,servoLimitsCenter[id1]+cos((at.read()*3)+1)*(servoLimitsEnd[id1]-servoLimitsStart[id1])/2); wait(0.03); } setservo(id1,servoLimitsCenter[id1]+1); setservo(id2,servoLimitsCenter[id2]-1); wait(1); setservo(id1,servoLimitsCenter[id1]); setservo(id2,servoLimitsCenter[id2]); } void open(int id,float time) { at.reset(); at.start(); mp3.mp3_play(id+1); setservo(id,servoLimitsEnd[id]); while(at.read()<time) { wait(0.02); } setservo(id,servoLimitsCenter[id]); } void music(int id,float time) { at.reset(); at.start(); mp3.mp3_play(id+1); while(at.read()<time) { wait(0.02); } } void musiclight(int id,float time) { at.reset(); at.start(); mp3.mp3_play(id+1); Timer t; t.start(); rgb RGB; hsv HSV; int color; while(at.read()<time) { HSV.s=1; HSV.v=1; //HSV.v=abs(sin(t.read()*1.11)); HSV.h=abs(sin(t.read()))*360; RGB = hsv2rgb(HSV); int r =RGB.r*255.0; int g =RGB.g*255.0; int b =RGB.b*255.0; color=r<<16 | g<<8 | b; px.SetAll(color); //ws.write(px.getBuf()); wait(0.02); } } void ledstripworker() { printf("hello leds\n"); ws.useII(WS2812::OFF); // use no intensity scaling Timer t; t.start(); px.SetAll(0x800000); ws.write(px.getBuf()); wait(10); rgb RGB; hsv HSV; int color=0; while(1) { //if (activated) { 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; int g =RGB.g*255.0; int b =RGB.b*255.0; color=r<<16 | g<<8 | b; } //else color=0; px.SetAll(color); wait(0.025); ws.write(px.getBuf()); } } float servoang(float ang) { if (ang<0) ang=0; if (ang>180) ang=180; ang=ang/180; float ret = SERVOSTART + ang*(SERVOEND-SERVOSTART); return ret; } //Mutex m; void setservo(int num, float ang) { num++; //pc.printf("servoset %d %f\n",num,ang); if (num>=0 && num <16 && ang>0 && ang<181) { if (ang>servoLimitsEnd[num] && ang>servoLimitsStart[num]) { //ang = servoLimitsEnd[num]; // 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); } //m.lock(); pwmouts.set_pwm_pw(num,servoang(ang)); //m.unlock(); } else pc.printf("wrong values %d %f\n",num,ang); } void servoworker() { Timer t; t.start(); pwmouts.init(); for (uint8_t i=0; i<16; i++) { setservo(i,servoLimitsCenter[i]); //wait(0.2); } while(1) { float an; for (uint8_t i=0; i<16; i++) { // an=servoang(90.0f+sin(t.read())*90.0f); // pwmouts.set_pwm_pw(i,1440+sin(t.read())*550.0f); } //printf("ang %f",an); wait(0.3); } } void messageReceive(MODSERIAL_IRQ_INFO *q) { MODSERIAL *sys = q->serial; memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE); sys->move(messageBufferIncoming, MESSAGE_BUFFER_SIZE); pc.rxBufferFlush(); messageReceived.release(); //return 0; } void serialworker (void) { char data[MESSAGE_BUFFER_SIZE]; while(1) { messageReceived.wait(); pc.printf("got '%s",messageBufferIncoming); pc.printf("'\n"); //memset(data,0,MESSAGE_BUFFER_SIZE); memcpy(data,messageBufferIncoming,MESSAGE_BUFFER_SIZE); //memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE); //pc.printf("got %s",data); int ang; int num; if (sscanf(data,"%d %d\n",&num,&ang)==2) { if (num>0) { pc.printf("servo %d ang %d\n",num, ang); setservo(num,ang); } else { if (ang) mp3.mp3_play(ang); else mp3.mp3_stop(); } };// else {} } } int main() { pc.baud(115200); pc.attach(&messageReceive, MODSERIAL::RxAutoDetect); pc.autoDetectChar('\n'); pc.printf("hi\n"); //wait(1.0f); mp3.mp3_play_physical(2); Thread ledstripthread; Thread servothread; Thread serialthread; ledstripthread.start(ledstripworker); servothread.start(servoworker); serialthread.start(serialworker); Action act = NONE; char dist[12]; int activated=-1; int activated_t=0; 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]); 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) activated_t=0; if (activated>=0 && activated_t==0) { activated_t=1; act = actions[activated]; pc.printf("activated %d action %d\n",activated,act); float time=actionstime[activated]; switch(act) { case NONE: break; case MUS: music(activated,time); break; case MLIT: musiclight(activated,time); break; case SLOW: slow_move(activated,time); break; case OPEN: open(activated,time); break; case KACH: if (activated==4) kachelle_double(0,1,5,time); else kachelle(activated,time); break; default: pc.printf("Action does not exist - MEGABUG\n"); wait(1); } activated=-1; } wait(0.3f); } }