File content as of revision 9:4bc77e4c090e:

/* 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 DELTA 6
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};

uint8_t sensorsZeros[16];


volatile int lastpos=0;
volatile int activated=0;
volatile int idle=0;

char dist[12];


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 {}
    }
}

void setSensorsZeros (void){
    memset(dist,0,12);
    i2c.read(0x20,dist,12);
    
    pc.printf("SensorsZeros \n");
    for(uint8_t i=0; i<12; i++)
    {   
        sensorsZeros[i]=dist[i];
                
        pc.printf("%d\t ",sensorsZeros[i]);
    }
    pc.printf("SensorsZeros \n");
    }
    

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;
    
    int activated=-1;
    int activated_t=0;
    wait(2.5);
    mp3.mp3_stop();
    int dist_tmp[16][4];
    
    setSensorsZeros();

    
    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 ((sensorsZeros[i]-dist_tmp[i][0])>DELTA && 
                 (sensorsZeros[i]-dist_tmp[i][1])>DELTA && 
                 (sensorsZeros[i]-dist_tmp[i][2])>DELTA &&
                 (sensorsZeros[i]-dist_tmp[i][3])>DELTA ) {
                 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);
    }
    
    
}