main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2018 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 //#include "stats_report.h"
00008 #include "PCA9685.h"
00009 #include "QEI.h"
00010 #include "WS2812.h"
00011 #include "PixelArray.h"
00012 #include "DebounceIn.h"
00013 #include "DFPlayerMini.h"
00014 #include "MODSERIAL.h"
00015 
00016 #include "rgbhsv.h"
00017 
00018 #define SERVOSTART 544
00019 #define SERVOEND 2400
00020 #define MESSAGE_BUFFER_SIZE 32
00021 #define DELTA 6
00022 Semaphore messageReceived(0);
00023 
00024 DigitalOut led1(LED1);                        
00025 I2C i2c (PB_9,PB_8);
00026 PCA9685 pwmouts(0x41<<1,&i2c,60);
00027 WS2812 ws(PB_15,160, 0, 18, 10, 15);
00028 DFPlayerMini mp3 (PA_9, PA_10);
00029 MODSERIAL pc (USBTX, USBRX);
00030 
00031 PixelArray px(160);
00032 char messageBufferIncoming[MESSAGE_BUFFER_SIZE];
00033 
00034 
00035 enum Action
00036 {
00037     NONE,
00038     MUS,
00039     MLIT,
00040     SLOW,
00041     OPEN,
00042     KACH    
00043 };
00044 
00045 const uint8_t sensormap[16] = {0,1,2,3,4,5,10,7,8,9,6,11};
00046 //const float actionstime[16]={5,5,5,5,5,5,5,5,2,5,5,5,5,5,5,5};
00047 const Action actions[16]=         {MLIT,MUS,OPEN,OPEN,KACH,OPEN,OPEN,OPEN,SLOW,OPEN,KACH,OPEN};
00048 const float   actionstime[16] =      {5,   5,  5,    5,   7,   3,   5,   5,   2,   5,  7,   6,   5,   5,   5,   5};
00049 const uint8_t servoLimitsStart[16]= {20,  85, 27,   35,   0,  23, 155, 106, 150,  45, 80, 136,   0,   0,   0,   0};
00050 const uint8_t servoLimitsCenter[16]={31, 108, 27,   35,   0,  23, 155, 106, 150,  45, 85, 136,   0,   0,   0,   0};
00051 const uint8_t servoLimitsEnd[16]=   {40, 135, 45,   90, 180,  90,  70,  75,  40, 120, 90,  55, 180, 180, 180, 180};
00052 
00053 uint8_t sensorsZeros[16];
00054 
00055 
00056 volatile int lastpos=0;
00057 volatile int activated=0;
00058 volatile int idle=0;
00059 
00060 char dist[12];
00061 
00062 
00063 void setservo(int num, float ang);
00064 
00065 
00066 Timer at;
00067 void slow_move(int id,float time)
00068 {
00069     //time =1.8;
00070     at.reset();
00071     at.start();
00072     mp3.mp3_play(id+1);
00073     while(at.read()<time)
00074     {
00075         setservo(id,servoLimitsStart[id]+(servoLimitsEnd[id]-servoLimitsStart[id])*(at.read()/time));
00076         float rw=rand()%20;
00077         wait(0.03+rw/100);
00078     }
00079     setservo(id,servoLimitsCenter[id]);
00080 }
00081 
00082 void kachelle(int id,float time)
00083 {
00084     at.reset();
00085     at.start();
00086     mp3.mp3_play(id+1);
00087     while(at.read()<time)
00088     {
00089         setservo(id,servoLimitsCenter[id]+sin(at.read()*2)*(servoLimitsEnd[id]-servoLimitsStart[id]));
00090         wait(0.03);
00091     }
00092     setservo(id,servoLimitsCenter[id]);
00093 
00094 }
00095 
00096 void kachelle_double(int id1,int id2,int idm, float time)
00097 {
00098     at.reset();
00099     at.start();
00100     mp3.mp3_play(idm);
00101     while(at.read()<time)
00102     {
00103         
00104         //setservo(id2,servoLimitsCenter[id2]+(1-sin(at.read()*3))*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
00105         setservo(id2,servoLimitsCenter[id2]+sin(at.read()*3)*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
00106         
00107         setservo(id1,servoLimitsCenter[id1]+cos((at.read()*3)+1)*(servoLimitsEnd[id1]-servoLimitsStart[id1])/2);
00108         wait(0.03);
00109         
00110         
00111     }
00112     setservo(id1,servoLimitsCenter[id1]+1);
00113     setservo(id2,servoLimitsCenter[id2]-1);
00114     wait(1);
00115     setservo(id1,servoLimitsCenter[id1]);
00116     setservo(id2,servoLimitsCenter[id2]);
00117 }
00118 
00119 void open(int id,float time)
00120 {
00121     at.reset();
00122     at.start();
00123     mp3.mp3_play(id+1);
00124     setservo(id,servoLimitsEnd[id]);
00125     while(at.read()<time)
00126     {
00127         wait(0.02);
00128     }
00129     setservo(id,servoLimitsCenter[id]);
00130 
00131 }
00132 
00133 void music(int id,float time)
00134 {
00135     at.reset();
00136     at.start();
00137     mp3.mp3_play(id+1);
00138     while(at.read()<time)
00139     {
00140         wait(0.02);
00141     }
00142 }
00143 
00144 void musiclight(int id,float time)
00145 {
00146     at.reset();
00147     at.start();
00148     mp3.mp3_play(id+1);
00149     Timer t;
00150     t.start();
00151     rgb RGB;
00152     hsv HSV;
00153     int color;
00154     while(at.read()<time)
00155     {  
00156             HSV.s=1;
00157             HSV.v=1;
00158             //HSV.v=abs(sin(t.read()*1.11));
00159             HSV.h=abs(sin(t.read()))*360;
00160             RGB = hsv2rgb(HSV);
00161             int r =RGB.r*255.0;
00162             int g =RGB.g*255.0;
00163             int b =RGB.b*255.0;
00164             
00165             color=r<<16 | g<<8 | b; 
00166         
00167         px.SetAll(color);
00168         
00169         //ws.write(px.getBuf());
00170         wait(0.02);
00171     
00172     }
00173 }
00174 
00175 
00176 void ledstripworker()
00177 {
00178     printf("hello leds\n");
00179     ws.useII(WS2812::OFF); // use no intensity scaling
00180     Timer t;
00181     t.start();
00182     
00183     px.SetAll(0x800000);
00184     ws.write(px.getBuf());
00185     wait(10);
00186     rgb RGB;
00187     hsv HSV;
00188     
00189     int color=0;
00190     while(1)
00191     {
00192         
00193         //if (activated) 
00194         {   
00195             HSV.s=1;//0.1;
00196             HSV.v=0.1;//abs(sin(t.read()*1.11));
00197             HSV.h=abs(sin(t.read()/30))*360;
00198             RGB = hsv2rgb(HSV);
00199             int r =RGB.r*255.0;
00200             int g =RGB.g*255.0;
00201             int b =RGB.b*255.0;
00202             
00203             color=r<<16 | g<<8 | b; 
00204         }
00205         //else color=0;
00206         
00207         px.SetAll(color);
00208         wait(0.025);
00209         ws.write(px.getBuf());
00210         
00211     }
00212 }
00213 
00214 float servoang(float ang)
00215 {
00216     if (ang<0) ang=0;
00217     if (ang>180) ang=180;
00218     ang=ang/180;
00219     float ret = SERVOSTART + ang*(SERVOEND-SERVOSTART); 
00220     return ret;
00221 }
00222 //Mutex m;
00223 void setservo(int num, float ang)
00224 {
00225     num++;
00226     //pc.printf("servoset %d %f\n",num,ang);
00227     
00228     if (num>=0 && num <16 && ang>0 && ang<181) 
00229     {
00230         if (ang>servoLimitsEnd[num] && ang>servoLimitsStart[num]) 
00231         {
00232             //ang = servoLimitsEnd[num];
00233          //   pc.printf("out of range %d %3.2f\n",num,ang);
00234          }
00235         if (ang<servoLimitsStart[num] && ang<servoLimitsEnd[num]) 
00236         {
00237             //ang = servoLimitsStart[num];
00238          //   pc.printf("out of range %d %3.2f\n",num,ang);
00239          }
00240         //m.lock();
00241         pwmouts.set_pwm_pw(num,servoang(ang));
00242         //m.unlock();
00243         
00244     } else pc.printf("wrong values %d %f\n",num,ang);
00245     
00246 }
00247 void servoworker()
00248 {
00249     Timer t;
00250     t.start();
00251     pwmouts.init();
00252     for (uint8_t i=0; i<16; i++)
00253     {
00254         setservo(i,servoLimitsCenter[i]);
00255         //wait(0.2);    
00256     }
00257     while(1)
00258     {
00259         float an;
00260         for (uint8_t i=0; i<16; i++)
00261         {
00262         //    an=servoang(90.0f+sin(t.read())*90.0f);
00263         //    pwmouts.set_pwm_pw(i,1440+sin(t.read())*550.0f);
00264         }
00265         //printf("ang %f",an);
00266         wait(0.3);
00267     }
00268         
00269 }
00270 void messageReceive(MODSERIAL_IRQ_INFO *q) {
00271     MODSERIAL *sys = q->serial;
00272     memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
00273     sys->move(messageBufferIncoming, MESSAGE_BUFFER_SIZE);
00274     pc.rxBufferFlush();
00275         
00276         
00277     messageReceived.release();
00278     //return 0;
00279 }
00280 void serialworker (void)
00281 {
00282     char data[MESSAGE_BUFFER_SIZE];
00283     while(1)
00284     {
00285         messageReceived.wait();
00286         
00287         pc.printf("got '%s",messageBufferIncoming);
00288         pc.printf("'\n");
00289         
00290         //memset(data,0,MESSAGE_BUFFER_SIZE);
00291         memcpy(data,messageBufferIncoming,MESSAGE_BUFFER_SIZE);
00292         //memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
00293         
00294         //pc.printf("got %s",data);
00295         int ang;
00296         int num;
00297         if (sscanf(data,"%d %d\n",&num,&ang)==2)
00298         {
00299             if (num>0)
00300             {
00301                 pc.printf("servo %d ang %d\n",num, ang);
00302                 setservo(num,ang);
00303             }
00304             else {
00305                 if (ang) mp3.mp3_play(ang); else mp3.mp3_stop();
00306                 }
00307         };// else {}
00308     }
00309 }
00310 
00311 void setSensorsZeros (void){
00312     memset(dist,0,12);
00313     i2c.read(0x20,dist,12);
00314     
00315     pc.printf("SensorsZeros \n");
00316     for(uint8_t i=0; i<12; i++)
00317     {   
00318         sensorsZeros[i]=dist[i];
00319                 
00320         pc.printf("%d\t ",sensorsZeros[i]);
00321     }
00322     pc.printf("SensorsZeros \n");
00323     }
00324     
00325 
00326 int main()
00327 {
00328     
00329     
00330     pc.baud(115200);
00331     pc.attach(&messageReceive, MODSERIAL::RxAutoDetect);
00332     pc.autoDetectChar('\n');
00333     
00334     pc.printf("hi\n");
00335     //wait(1.0f);
00336     mp3.mp3_play_physical(2);
00337     
00338     Thread ledstripthread;
00339     Thread servothread;
00340     Thread serialthread;
00341     
00342     ledstripthread.start(ledstripworker);
00343     servothread.start(servoworker);
00344     serialthread.start(serialworker);
00345     Action act = NONE;
00346     
00347     int activated=-1;
00348     int activated_t=0;
00349     wait(2.5);
00350     mp3.mp3_stop();
00351     int dist_tmp[16][4];
00352     
00353     setSensorsZeros();
00354 
00355     
00356     while(1)
00357     {
00358         memset(dist,0,12);
00359         i2c.read(0x20,dist,12);
00360         activated=-1;
00361         for(uint8_t j=3; j>0; j--)
00362         {   
00363             pc.printf("\n**");
00364             for(uint8_t i=0; i<12; i++)
00365             {   
00366                 dist_tmp[i][j]=dist_tmp[i][j-1];
00367               //  pc.printf("%d\t ",dist_tmp[i][j]);
00368             }
00369             
00370         }
00371         //pc.printf("\n");
00372         for(uint8_t i=0; i<12; i++)
00373         {   
00374             pc.printf("\t %d",dist[i]);
00375             dist_tmp[i][0]=dist[i];
00376              if ((sensorsZeros[i]-dist_tmp[i][0])>DELTA && 
00377                  (sensorsZeros[i]-dist_tmp[i][1])>DELTA && 
00378                  (sensorsZeros[i]-dist_tmp[i][2])>DELTA &&
00379                  (sensorsZeros[i]-dist_tmp[i][3])>DELTA ) {
00380                  activated=sensormap[i];
00381                 
00382                 }
00383                 //else activated=-1;
00384         }
00385         
00386         
00387         pc.printf("\n");
00388         if (activated<0) activated_t=0;
00389         
00390         if (activated>=0 && activated_t==0)
00391         {
00392          activated_t=1;
00393          act = actions[activated];
00394          pc.printf("activated %d action %d\n",activated,act);
00395          float time=actionstime[activated];
00396          switch(act)
00397             {
00398             
00399             case NONE:
00400             break;
00401             
00402             case MUS:
00403             music(activated,time);
00404             break;
00405             
00406             case MLIT:
00407             musiclight(activated,time);
00408             break;
00409             
00410             case SLOW:
00411             slow_move(activated,time);
00412             break;
00413             
00414             case OPEN:
00415             open(activated,time);
00416             break;
00417             
00418             case KACH:
00419             if (activated==4) kachelle_double(0,1,5,time);
00420             else kachelle(activated,time);  
00421             break;
00422             
00423             default:
00424             pc.printf("Action does not exist - MEGABUG\n");
00425             wait(1);    
00426             }
00427             activated=-1;
00428         }
00429         wait(0.3f);
00430     }
00431     
00432     
00433 }