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 MIN_DIST 35
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 
00054 volatile int lastpos=0;
00055 volatile int activated=0;
00056 volatile int idle=0;
00057 
00058 
00059 void setservo(int num, float ang);
00060 
00061 
00062 Timer at;
00063 void slow_move(int id,float time)
00064 {
00065     //time =1.8;
00066     at.reset();
00067     at.start();
00068     mp3.mp3_play(id+1);
00069     while(at.read()<time)
00070     {
00071         setservo(id,servoLimitsStart[id]+(servoLimitsEnd[id]-servoLimitsStart[id])*(at.read()/time));
00072         float rw=rand()%20;
00073         wait(0.03+rw/100);
00074     }
00075     setservo(id,servoLimitsCenter[id]);
00076 }
00077 
00078 void kachelle(int id,float time)
00079 {
00080     at.reset();
00081     at.start();
00082     mp3.mp3_play(id+1);
00083     while(at.read()<time)
00084     {
00085         setservo(id,servoLimitsCenter[id]+sin(at.read()*2)*(servoLimitsEnd[id]-servoLimitsStart[id]));
00086         wait(0.03);
00087     }
00088     setservo(id,servoLimitsCenter[id]);
00089 
00090 }
00091 
00092 void kachelle_double(int id1,int id2,int idm, float time)
00093 {
00094     at.reset();
00095     at.start();
00096     mp3.mp3_play(idm);
00097     while(at.read()<time)
00098     {
00099         
00100         //setservo(id2,servoLimitsCenter[id2]+(1-sin(at.read()*3))*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
00101         setservo(id2,servoLimitsCenter[id2]+sin(at.read()*3)*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
00102         
00103         setservo(id1,servoLimitsCenter[id1]+cos((at.read()*3)+1)*(servoLimitsEnd[id1]-servoLimitsStart[id1])/2);
00104         wait(0.03);
00105         
00106         
00107     }
00108     setservo(id1,servoLimitsCenter[id1]+1);
00109     setservo(id2,servoLimitsCenter[id2]-1);
00110     wait(1);
00111     setservo(id1,servoLimitsCenter[id1]);
00112     setservo(id2,servoLimitsCenter[id2]);
00113 }
00114 
00115 void open(int id,float time)
00116 {
00117     at.reset();
00118     at.start();
00119     mp3.mp3_play(id+1);
00120     setservo(id,servoLimitsEnd[id]);
00121     while(at.read()<time)
00122     {
00123         wait(0.02);
00124     }
00125     setservo(id,servoLimitsCenter[id]);
00126 
00127 }
00128 
00129 void music(int id,float time)
00130 {
00131     at.reset();
00132     at.start();
00133     mp3.mp3_play(id+1);
00134     while(at.read()<time)
00135     {
00136         wait(0.02);
00137     }
00138 }
00139 
00140 void musiclight(int id,float time)
00141 {
00142     at.reset();
00143     at.start();
00144     mp3.mp3_play(id+1);
00145     Timer t;
00146     t.start();
00147     rgb RGB;
00148     hsv HSV;
00149     int color;
00150     while(at.read()<time)
00151     {  
00152             HSV.s=1;
00153             HSV.v=1;
00154             //HSV.v=abs(sin(t.read()*1.11));
00155             HSV.h=abs(sin(t.read()))*360;
00156             RGB = hsv2rgb(HSV);
00157             int r =RGB.r*255.0;
00158             int g =RGB.g*255.0;
00159             int b =RGB.b*255.0;
00160             
00161             color=r<<16 | g<<8 | b; 
00162         
00163         px.SetAll(color);
00164         
00165         //ws.write(px.getBuf());
00166         wait(0.02);
00167     
00168     }
00169 }
00170 
00171 
00172 void ledstripworker()
00173 {
00174     printf("hello leds\n");
00175     ws.useII(WS2812::OFF); // use no intensity scaling
00176     Timer t;
00177     t.start();
00178     
00179     px.SetAll(0x800000);
00180     ws.write(px.getBuf());
00181     wait(10);
00182     rgb RGB;
00183     hsv HSV;
00184     
00185     int color=0;
00186     while(1)
00187     {
00188         
00189         //if (activated) 
00190         {   
00191             HSV.s=1;//0.1;
00192             HSV.v=0.1;//abs(sin(t.read()*1.11));
00193             HSV.h=abs(sin(t.read()/30))*360;
00194             RGB = hsv2rgb(HSV);
00195             int r =RGB.r*255.0;
00196             int g =RGB.g*255.0;
00197             int b =RGB.b*255.0;
00198             
00199             color=r<<16 | g<<8 | b; 
00200         }
00201         //else color=0;
00202         
00203         px.SetAll(color);
00204         wait(0.025);
00205         ws.write(px.getBuf());
00206         
00207     }
00208 }
00209 
00210 float servoang(float ang)
00211 {
00212     if (ang<0) ang=0;
00213     if (ang>180) ang=180;
00214     ang=ang/180;
00215     float ret = SERVOSTART + ang*(SERVOEND-SERVOSTART); 
00216     return ret;
00217 }
00218 //Mutex m;
00219 void setservo(int num, float ang)
00220 {
00221     num++;
00222     //pc.printf("servoset %d %f\n",num,ang);
00223     
00224     if (num>=0 && num <16 && ang>0 && ang<181) 
00225     {
00226         if (ang>servoLimitsEnd[num] && ang>servoLimitsStart[num]) 
00227         {
00228             //ang = servoLimitsEnd[num];
00229          //   pc.printf("out of range %d %3.2f\n",num,ang);
00230          }
00231         if (ang<servoLimitsStart[num] && ang<servoLimitsEnd[num]) 
00232         {
00233             //ang = servoLimitsStart[num];
00234          //   pc.printf("out of range %d %3.2f\n",num,ang);
00235          }
00236         //m.lock();
00237         pwmouts.set_pwm_pw(num,servoang(ang));
00238         //m.unlock();
00239         
00240     } else pc.printf("wrong values %d %f\n",num,ang);
00241     
00242 }
00243 void servoworker()
00244 {
00245     Timer t;
00246     t.start();
00247     pwmouts.init();
00248     for (uint8_t i=0; i<16; i++)
00249     {
00250         setservo(i,servoLimitsCenter[i]);
00251         //wait(0.2);    
00252     }
00253     while(1)
00254     {
00255         float an;
00256         for (uint8_t i=0; i<16; i++)
00257         {
00258         //    an=servoang(90.0f+sin(t.read())*90.0f);
00259         //    pwmouts.set_pwm_pw(i,1440+sin(t.read())*550.0f);
00260         }
00261         //printf("ang %f",an);
00262         wait(0.3);
00263     }
00264         
00265 }
00266 void messageReceive(MODSERIAL_IRQ_INFO *q) {
00267     MODSERIAL *sys = q->serial;
00268     memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
00269     sys->move(messageBufferIncoming, MESSAGE_BUFFER_SIZE);
00270     pc.rxBufferFlush();
00271         
00272         
00273     messageReceived.release();
00274     //return 0;
00275 }
00276 void serialworker (void)
00277 {
00278     char data[MESSAGE_BUFFER_SIZE];
00279     while(1)
00280     {
00281         messageReceived.wait();
00282         
00283         pc.printf("got '%s",messageBufferIncoming);
00284         pc.printf("'\n");
00285         
00286         //memset(data,0,MESSAGE_BUFFER_SIZE);
00287         memcpy(data,messageBufferIncoming,MESSAGE_BUFFER_SIZE);
00288         //memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
00289         
00290         //pc.printf("got %s",data);
00291         int ang;
00292         int num;
00293         if (sscanf(data,"%d %d\n",&num,&ang)==2)
00294         {
00295             if (num>0)
00296             {
00297                 pc.printf("servo %d ang %d\n",num, ang);
00298                 setservo(num,ang);
00299             }
00300             else {
00301                 if (ang) mp3.mp3_play(ang); else mp3.mp3_stop();
00302                 }
00303         };// else {}
00304     }
00305 }
00306 
00307 int main()
00308 {
00309     
00310     
00311     pc.baud(115200);
00312     pc.attach(&messageReceive, MODSERIAL::RxAutoDetect);
00313     pc.autoDetectChar('\n');
00314     
00315     pc.printf("hi\n");
00316     //wait(1.0f);
00317     mp3.mp3_play_physical(2);
00318     
00319     Thread ledstripthread;
00320     Thread servothread;
00321     Thread serialthread;
00322     
00323     ledstripthread.start(ledstripworker);
00324     servothread.start(servoworker);
00325     serialthread.start(serialworker);
00326     Action act = NONE;
00327     char dist[12];
00328     int activated=-1;
00329     int activated_t=0;
00330     wait(2.5);
00331     mp3.mp3_stop();
00332     int dist_tmp[16][4];
00333     while(1)
00334     {
00335         memset(dist,0,12);
00336         i2c.read(0x20,dist,12);
00337         activated=-1;
00338         for(uint8_t j=3; j>0; j--)
00339         {   
00340             pc.printf("\n**");
00341             for(uint8_t i=0; i<12; i++)
00342             {   
00343                 dist_tmp[i][j]=dist_tmp[i][j-1];
00344               //  pc.printf("%d\t ",dist_tmp[i][j]);
00345             }
00346             
00347         }
00348         //pc.printf("\n");
00349         for(uint8_t i=0; i<12; i++)
00350         {   
00351             pc.printf("\t %d",dist[i]);
00352             dist_tmp[i][0]=dist[i];
00353              if (dist_tmp[i][0]<MIN_DIST && 
00354                  dist_tmp[i][1]<MIN_DIST && 
00355                  dist_tmp[i][2]<MIN_DIST &&
00356                  dist_tmp[i][3]<MIN_DIST ) {
00357                  activated=sensormap[i];
00358                 
00359                 }
00360                 //else activated=-1;
00361         }
00362         
00363         
00364         pc.printf("\n");
00365         if (activated<0) activated_t=0;
00366         
00367         if (activated>=0 && activated_t==0)
00368         {
00369          activated_t=1;
00370          act = actions[activated];
00371          pc.printf("activated %d action %d\n",activated,act);
00372          float time=actionstime[activated];
00373          switch(act)
00374             {
00375             
00376             case NONE:
00377             break;
00378             
00379             case MUS:
00380             music(activated,time);
00381             break;
00382             
00383             case MLIT:
00384             musiclight(activated,time);
00385             break;
00386             
00387             case SLOW:
00388             slow_move(activated,time);
00389             break;
00390             
00391             case OPEN:
00392             open(activated,time);
00393             break;
00394             
00395             case KACH:
00396             if (activated==4) kachelle_double(0,1,5,time);
00397             else kachelle(activated,time);  
00398             break;
00399             
00400             default:
00401             pc.printf("Action does not exist - MEGABUG\n");
00402             wait(1);    
00403             }
00404             activated=-1;
00405         }
00406         wait(0.3f);
00407     }
00408     
00409     
00410 }