Александр Селифонов
2532
Dependencies: QEI WS2812 PixelArray DFPlayerMini MODSERIAL PCA9685_ pca9685
Diff: main.cpp
- Revision:
- 2:ed1f45f9b06a
- Parent:
- 0:474fb16588bf
- Child:
- 4:2f4d485ebe58
--- 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);
}
+
+
}