Revision 2:ed1f45f9b06a, committed 2019-06-22

Comitter:

dimavb

Date:

Sat Jun 22 17:24:44 2019 +0000

Parent:

1:caf626dbb517

Child:

3:da339cc84c9f

Commit message:

init

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DFPlayerMini.lib	Sat Jun 22 17:24:44 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/dimavb/code/DFPlayerMini/#a637be95c1ea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Sat Jun 22 17:24:44 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/MODSERIAL/#d2a5e26fd658

--- a/PCA9685.lib	Sun Jun 02 14:46:47 2019 +0000
+++ b/PCA9685.lib	Sat Jun 22 17:24:44 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/el13cj/code/PCA9685/#3bcda7deb098
+https://os.mbed.com/users/el13cj/code/PCA9685/#d44f88fa27a1

--- a/main.cpp	Sun Jun 02 14:46:47 2019 +0000
+++ b/main.cpp	Sat Jun 22 17:24:44 2019 +0000
@@ -4,89 +4,378 @@
  */
 
 #include "mbed.h"
-#include "stats_report.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
+
+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,   5,   5,   5,   5,   2,   5,  5,   5,   5,   5,   5,   5};
+const uint8_t servoLimitsStart[16]= {20,  85, 30,   40,   0,  20, 155, 105, 150,  45, 75, 130,   0,   0,   0,   0};
+const uint8_t servoLimitsCenter[16]={40, 110, 30,   40,   0,  20, 155, 105, 150,  45, 85, 130,   0,   0,   0,   0};
+const uint8_t servoLimitsEnd[16]=   {60, 135, 50,  130, 180, 140,  70,  75,  40, 130, 95,  50, 180, 180, 180, 180};
+
+
+volatile int lastpos=0;
+volatile int activated=0;
+volatile int idle=0;
+
+
+void setservo(int num, float ang);
 
 
-//InterruptIn int0 (PB_0);
-//enc pb2 pb4
-//zero pb5
-DebounceIn zerosens(PB_6);
-QEI enc(PB_2,PB_4,NC,1);
-I2C i2c (PB_9,PB_8);
-PCA9685 pwmouts(0x40<<1,i2c,500);
-WS2812 ws(PB_15,500, 0, 18, 10, 15);
-PixelArray px(500);
-DigitalOut led1(LED1);
+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]);
+}
 
-#define SLEEP_TIME                  200 // (msec)
-#define PRINT_AFTER_N_LOOPS         20
-#define REVCNT 500
+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())*(servoLimitsEnd[id]-servoLimitsStart[id]));
+        wait(0.03);
+    }
+    setservo(id,servoLimitsCenter[id]);
 
-void int0f (void)
-{
-    led1=!led1;
 }
-void encworker()
+
+void kachelle_double(int id1,int id2,int idm, float time)
 {
-    printf("hello encoder\n");
-    int pos=0;
-    int pos_=0;
-    int npos=0;
-    int lastdirection=0;
-    while(1)
+    at.reset();
+    at.start();
+    mp3.mp3_play(idm);
+    while(at.read()<time)
     {
-        if (zerosens.ntrig()) 
-        {
-            printf("zero\n");
-            npos=enc.getPulses();
-            enc.reset();
-        }
-        pos=enc.getPulses();
         
-        printf("pos: %d \t%d \t%d\n",pos,npos,zerosens.read());
-        wait(0.1);    
+        setservo(id2,servoLimitsCenter[id2]+(1-sin(at.read()*3))*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
+        setservo(id1,servoLimitsCenter[id1]+sin(at.read()*3)*(servoLimitsEnd[id1]-servoLimitsStart[id1])/2);
+        wait(0.03);
+    }
+    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 ledworker()
+
+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)
     {
-        int p=(enc.getPulses()%160)/10;
-        for (uint8_t i=0; i<15; i++)
-        {
-            if (i==p) pwmouts.set_pwm_duty(i,1); else pwmouts.set_pwm_duty(i,0);
+        
+        //if (activated) 
+        {   
+            HSV.s=0.1;
+            HSV.v=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; 
         }
-        px.SetAll(3000000+t.read_ms());
+        //else color=0;
         
-        //ws.write(px.getBuf());
-        wait(0.02);
+        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()
 {
-    Thread ledsthread;
-    Thread encthread;
+    
+    
+    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;
     
-    ledsthread.start(ledworker);
-    encthread.start(encworker);
-    printf("hello\n");
-    pwmouts.init();
-    float v=0;
-    Timer t;
-    t.start();
-    while (true) 
+    ledstripthread.start(ledstripworker);
+    servothread.start(servoworker);
+    serialthread.start(serialworker);
+    Action act = NONE;
+    char dist[12];
+    int activated=-1;
+    wait(2);
+    mp3.mp3_stop();
+    while(1)
     {
-        wait(10);
-        printf("TITS!\n (.)(.)\n");
+        memset(dist,0,12);
+        i2c.read(0x20,dist,12);
+        activated=-1;
+        for(uint8_t i=0; i<12; i++)
+        {   
+            pc.printf("\t %d",dist[i]);
+            if (dist[i]<35) {
+                activated=sensormap[i];
+                
+                } 
+                //else activated=-1;
+        }
+        pc.printf("\n");
+        
+        if (activated>=0)
+        {
+         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);
     }
+    
+    
 }

--- a/mbed-os.lib	Sun Jun 02 14:46:47 2019 +0000
+++ b/mbed-os.lib	Sat Jun 22 17:24:44 2019 +0000
@@ -1,1 +1,1 @@
-https://github.com/ARMmbed/mbed-os/#0063e5de32fc575f061244c96ac60c41c07bd2e6
+https://github.com/ARMmbed/mbed-os/#2fd0c5cfbd83fce62da6308f9d64c0ab64e1f0d6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rgbhsv.h	Sat Jun 22 17:24:44 2019 +0000
@@ -0,0 +1,117 @@
+typedef struct {
+    double r;       // a fraction between 0 and 1
+    double g;       // a fraction between 0 and 1
+    double b;       // a fraction between 0 and 1
+} rgb;
+
+typedef struct {
+    double h;       // angle in degrees
+    double s;       // a fraction between 0 and 1
+    double v;       // a fraction between 0 and 1
+} hsv;
+
+hsv   rgb2hsv(rgb in);
+rgb   hsv2rgb(hsv in);
+
+hsv rgb2hsv(rgb in)
+{
+    hsv         out;
+    double      min, max, delta;
+
+    min = in.r < in.g ? in.r : in.g;
+    min = min  < in.b ? min  : in.b;
+
+    max = in.r > in.g ? in.r : in.g;
+    max = max  > in.b ? max  : in.b;
+
+    out.v = max;                                // v
+    delta = max - min;
+    if (delta < 0.00001)
+    {
+        out.s = 0;
+        out.h = 0; // undefined, maybe nan?
+        return out;
+    }
+    if( max > 0.0 ) { // NOTE: if Max is == 0, this divide would cause a crash
+        out.s = (delta / max);                  // s
+    } else {
+        // if max is 0, then r = g = b = 0              
+        // s = 0, h is undefined
+        out.s = 0.0;
+        out.h = NAN;                            // its now undefined
+        return out;
+    }
+    if( in.r >= max )                           // > is bogus, just keeps compilor happy
+        out.h = ( in.g - in.b ) / delta;        // between yellow & magenta
+    else
+    if( in.g >= max )
+        out.h = 2.0 + ( in.b - in.r ) / delta;  // between cyan & yellow
+    else
+        out.h = 4.0 + ( in.r - in.g ) / delta;  // between magenta & cyan
+
+    out.h *= 60.0;                              // degrees
+
+    if( out.h < 0.0 )
+        out.h += 360.0;
+
+    return out;
+}
+
+
+rgb hsv2rgb(hsv in)
+{
+    double      hh, p, q, t, ff;
+    long        i;
+    rgb         out;
+
+    if(in.s <= 0.0) {       // < is bogus, just shuts up warnings
+        out.r = in.v;
+        out.g = in.v;
+        out.b = in.v;
+        return out;
+    }
+    hh = in.h;
+    if(hh >= 360.0) hh = 0.0;
+    hh /= 60.0;
+    i = (long)hh;
+    ff = hh - i;
+    p = in.v * (1.0 - in.s);
+    q = in.v * (1.0 - (in.s * ff));
+    t = in.v * (1.0 - (in.s * (1.0 - ff)));
+
+    switch(i) {
+    case 0:
+        out.r = in.v;
+        out.g = t;
+        out.b = p;
+        break;
+    case 1:
+        out.r = q;
+        out.g = in.v;
+        out.b = p;
+        break;
+    case 2:
+        out.r = p;
+        out.g = in.v;
+        out.b = t;
+        break;
+
+    case 3:
+        out.r = p;
+        out.g = q;
+        out.b = in.v;
+        break;
+    case 4:
+        out.r = t;
+        out.g = p;
+        out.b = in.v;
+        break;
+    case 5:
+    default:
+        out.r = in.v;
+        out.g = p;
+        out.b = q;
+        break;
+    }
+    return out;     
+}