Александр Селифонов / Mbed OS pinball

2532

Dependencies:   QEI WS2812 PixelArray DFPlayerMini MODSERIAL PCA9685_ pca9685

main.cpp@6:4b007c7f0b7a, 2019-06-26 (annotated)

Committer:
dimavb
Date:
Wed Jun 26 18:06:30 2019 +0000
Revision:
6:4b007c7f0b7a
Parent:
5:cec7f85ce6c4 
activation trigger

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dimavb 0:474fb16588bf 1 /* mbed Microcontroller Library
dimavb 0:474fb16588bf 2 * Copyright (c) 2018 ARM Limited
dimavb 0:474fb16588bf 3 * SPDX-License-Identifier: Apache-2.0
dimavb 0:474fb16588bf 4 */
dimavb 0:474fb16588bf 5
dimavb 0:474fb16588bf 6 #include "mbed.h"
dimavb 2:ed1f45f9b06a 7 //#include "stats_report.h"
dimavb 0:474fb16588bf 8 #include "PCA9685.h"
dimavb 0:474fb16588bf 9 #include "QEI.h"
dimavb 0:474fb16588bf 10 #include "WS2812.h"
dimavb 0:474fb16588bf 11 #include "PixelArray.h"
dimavb 0:474fb16588bf 12 #include "DebounceIn.h"
dimavb 2:ed1f45f9b06a 13 #include "DFPlayerMini.h"
dimavb 2:ed1f45f9b06a 14 #include "MODSERIAL.h"
dimavb 2:ed1f45f9b06a 15
dimavb 2:ed1f45f9b06a 16 #include "rgbhsv.h"
dimavb 2:ed1f45f9b06a 17
dimavb 2:ed1f45f9b06a 18 #define SERVOSTART 544
dimavb 2:ed1f45f9b06a 19 #define SERVOEND 2400
dimavb 2:ed1f45f9b06a 20 #define MESSAGE_BUFFER_SIZE 32
dimavb 4:2f4d485ebe58 21 #define MIN_DIST 35
dimavb 2:ed1f45f9b06a 22 Semaphore messageReceived(0);
dimavb 2:ed1f45f9b06a 23
dimavb 2:ed1f45f9b06a 24 DigitalOut led1(LED1);
dimavb 2:ed1f45f9b06a 25 I2C i2c (PB_9,PB_8);
dimavb 2:ed1f45f9b06a 26 PCA9685 pwmouts(0x41<<1,&i2c,60);
dimavb 2:ed1f45f9b06a 27 WS2812 ws(PB_15,160, 0, 18, 10, 15);
dimavb 2:ed1f45f9b06a 28 DFPlayerMini mp3 (PA_9, PA_10);
dimavb 2:ed1f45f9b06a 29 MODSERIAL pc (USBTX, USBRX);
dimavb 2:ed1f45f9b06a 30
dimavb 2:ed1f45f9b06a 31 PixelArray px(160);
dimavb 2:ed1f45f9b06a 32 char messageBufferIncoming[MESSAGE_BUFFER_SIZE];
dimavb 2:ed1f45f9b06a 33
dimavb 2:ed1f45f9b06a 34
dimavb 2:ed1f45f9b06a 35 enum Action
dimavb 2:ed1f45f9b06a 36 {
dimavb 2:ed1f45f9b06a 37 NONE,
dimavb 2:ed1f45f9b06a 38 MUS,
dimavb 2:ed1f45f9b06a 39 MLIT,
dimavb 2:ed1f45f9b06a 40 SLOW,
dimavb 2:ed1f45f9b06a 41 OPEN,
dimavb 2:ed1f45f9b06a 42 KACH
dimavb 2:ed1f45f9b06a 43 };
dimavb 2:ed1f45f9b06a 44
dimavb 2:ed1f45f9b06a 45 const uint8_t sensormap[16] = {0,1,2,3,4,5,10,7,8,9,6,11};
dimavb 2:ed1f45f9b06a 46 //const float actionstime[16]={5,5,5,5,5,5,5,5,2,5,5,5,5,5,5,5};
dimavb 2:ed1f45f9b06a 47 const Action actions[16]= {MLIT,MUS,OPEN,OPEN,KACH,OPEN,OPEN,OPEN,SLOW,OPEN,KACH,OPEN};
turumputum 5:cec7f85ce6c4 48 const float actionstime[16] = {5, 5, 5, 5, 7, 3, 5, 5, 2, 5, 7, 6, 5, 5, 5, 5};
turumputum 5:cec7f85ce6c4 49 const uint8_t servoLimitsStart[16]= {20, 85, 27, 35, 0, 23, 155, 106, 150, 45, 80, 136, 0, 0, 0, 0};
turumputum 5:cec7f85ce6c4 50 const uint8_t servoLimitsCenter[16]={31, 108, 27, 35, 0, 23, 155, 106, 150, 45, 85, 136, 0, 0, 0, 0};
turumputum 5:cec7f85ce6c4 51 const uint8_t servoLimitsEnd[16]= {40, 135, 45, 90, 180, 90, 70, 75, 40, 120, 90, 55, 180, 180, 180, 180};
dimavb 2:ed1f45f9b06a 52
dimavb 2:ed1f45f9b06a 53
dimavb 2:ed1f45f9b06a 54 volatile int lastpos=0;
dimavb 2:ed1f45f9b06a 55 volatile int activated=0;
dimavb 2:ed1f45f9b06a 56 volatile int idle=0;
dimavb 2:ed1f45f9b06a 57
dimavb 2:ed1f45f9b06a 58
dimavb 2:ed1f45f9b06a 59 void setservo(int num, float ang);
dimavb 0:474fb16588bf 60
dimavb 0:474fb16588bf 61
dimavb 2:ed1f45f9b06a 62 Timer at;
dimavb 2:ed1f45f9b06a 63 void slow_move(int id,float time)
dimavb 2:ed1f45f9b06a 64 {
dimavb 2:ed1f45f9b06a 65 //time =1.8;
dimavb 2:ed1f45f9b06a 66 at.reset();
dimavb 2:ed1f45f9b06a 67 at.start();
dimavb 2:ed1f45f9b06a 68 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 69 while(at.read()<time)
dimavb 2:ed1f45f9b06a 70 {
dimavb 2:ed1f45f9b06a 71 setservo(id,servoLimitsStart[id]+(servoLimitsEnd[id]-servoLimitsStart[id])*(at.read()/time));
dimavb 2:ed1f45f9b06a 72 float rw=rand()%20;
dimavb 2:ed1f45f9b06a 73 wait(0.03+rw/100);
dimavb 2:ed1f45f9b06a 74 }
dimavb 2:ed1f45f9b06a 75 setservo(id,servoLimitsCenter[id]);
dimavb 2:ed1f45f9b06a 76 }
dimavb 0:474fb16588bf 77
dimavb 2:ed1f45f9b06a 78 void kachelle(int id,float time)
dimavb 2:ed1f45f9b06a 79 {
dimavb 2:ed1f45f9b06a 80 at.reset();
dimavb 2:ed1f45f9b06a 81 at.start();
dimavb 2:ed1f45f9b06a 82 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 83 while(at.read()<time)
dimavb 2:ed1f45f9b06a 84 {
turumputum 5:cec7f85ce6c4 85 setservo(id,servoLimitsCenter[id]+sin(at.read()*2)*(servoLimitsEnd[id]-servoLimitsStart[id]));
dimavb 2:ed1f45f9b06a 86 wait(0.03);
dimavb 2:ed1f45f9b06a 87 }
dimavb 2:ed1f45f9b06a 88 setservo(id,servoLimitsCenter[id]);
dimavb 0:474fb16588bf 89
dimavb 0:474fb16588bf 90 }
dimavb 2:ed1f45f9b06a 91
dimavb 2:ed1f45f9b06a 92 void kachelle_double(int id1,int id2,int idm, float time)
dimavb 0:474fb16588bf 93 {
dimavb 2:ed1f45f9b06a 94 at.reset();
dimavb 2:ed1f45f9b06a 95 at.start();
dimavb 2:ed1f45f9b06a 96 mp3.mp3_play(idm);
dimavb 2:ed1f45f9b06a 97 while(at.read()<time)
dimavb 0:474fb16588bf 98 {
dimavb 0:474fb16588bf 99
turumputum 5:cec7f85ce6c4 100 //setservo(id2,servoLimitsCenter[id2]+(1-sin(at.read()*3))*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
turumputum 5:cec7f85ce6c4 101 setservo(id2,servoLimitsCenter[id2]+sin(at.read()*3)*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
turumputum 5:cec7f85ce6c4 102
turumputum 5:cec7f85ce6c4 103 setservo(id1,servoLimitsCenter[id1]+cos((at.read()*3)+1)*(servoLimitsEnd[id1]-servoLimitsStart[id1])/2);
dimavb 2:ed1f45f9b06a 104 wait(0.03);
turumputum 5:cec7f85ce6c4 105
turumputum 5:cec7f85ce6c4 106
dimavb 2:ed1f45f9b06a 107 }
turumputum 5:cec7f85ce6c4 108 setservo(id1,servoLimitsCenter[id1]+1);
turumputum 5:cec7f85ce6c4 109 setservo(id2,servoLimitsCenter[id2]-1);
turumputum 5:cec7f85ce6c4 110 wait(1);
dimavb 2:ed1f45f9b06a 111 setservo(id1,servoLimitsCenter[id1]);
dimavb 2:ed1f45f9b06a 112 setservo(id2,servoLimitsCenter[id2]);
dimavb 2:ed1f45f9b06a 113 }
dimavb 2:ed1f45f9b06a 114
dimavb 2:ed1f45f9b06a 115 void open(int id,float time)
dimavb 2:ed1f45f9b06a 116 {
dimavb 2:ed1f45f9b06a 117 at.reset();
dimavb 2:ed1f45f9b06a 118 at.start();
dimavb 2:ed1f45f9b06a 119 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 120 setservo(id,servoLimitsEnd[id]);
dimavb 2:ed1f45f9b06a 121 while(at.read()<time)
dimavb 2:ed1f45f9b06a 122 {
dimavb 2:ed1f45f9b06a 123 wait(0.02);
dimavb 2:ed1f45f9b06a 124 }
dimavb 2:ed1f45f9b06a 125 setservo(id,servoLimitsCenter[id]);
dimavb 2:ed1f45f9b06a 126
dimavb 2:ed1f45f9b06a 127 }
dimavb 2:ed1f45f9b06a 128
dimavb 2:ed1f45f9b06a 129 void music(int id,float time)
dimavb 2:ed1f45f9b06a 130 {
dimavb 2:ed1f45f9b06a 131 at.reset();
dimavb 2:ed1f45f9b06a 132 at.start();
dimavb 2:ed1f45f9b06a 133 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 134 while(at.read()<time)
dimavb 2:ed1f45f9b06a 135 {
dimavb 2:ed1f45f9b06a 136 wait(0.02);
dimavb 0:474fb16588bf 137 }
dimavb 0:474fb16588bf 138 }
dimavb 2:ed1f45f9b06a 139
dimavb 2:ed1f45f9b06a 140 void musiclight(int id,float time)
dimavb 2:ed1f45f9b06a 141 {
dimavb 2:ed1f45f9b06a 142 at.reset();
dimavb 2:ed1f45f9b06a 143 at.start();
dimavb 2:ed1f45f9b06a 144 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 145 Timer t;
dimavb 2:ed1f45f9b06a 146 t.start();
dimavb 2:ed1f45f9b06a 147 rgb RGB;
dimavb 2:ed1f45f9b06a 148 hsv HSV;
dimavb 2:ed1f45f9b06a 149 int color;
dimavb 2:ed1f45f9b06a 150 while(at.read()<time)
dimavb 2:ed1f45f9b06a 151 {
dimavb 2:ed1f45f9b06a 152 HSV.s=1;
dimavb 2:ed1f45f9b06a 153 HSV.v=1;
dimavb 2:ed1f45f9b06a 154 //HSV.v=abs(sin(t.read()*1.11));
dimavb 2:ed1f45f9b06a 155 HSV.h=abs(sin(t.read()))*360;
dimavb 2:ed1f45f9b06a 156 RGB = hsv2rgb(HSV);
dimavb 2:ed1f45f9b06a 157 int r =RGB.r*255.0;
dimavb 2:ed1f45f9b06a 158 int g =RGB.g*255.0;
dimavb 2:ed1f45f9b06a 159 int b =RGB.b*255.0;
dimavb 2:ed1f45f9b06a 160
dimavb 2:ed1f45f9b06a 161 color=r<<16 | g<<8 | b;
dimavb 2:ed1f45f9b06a 162
dimavb 2:ed1f45f9b06a 163 px.SetAll(color);
dimavb 2:ed1f45f9b06a 164
dimavb 2:ed1f45f9b06a 165 //ws.write(px.getBuf());
dimavb 2:ed1f45f9b06a 166 wait(0.02);
dimavb 2:ed1f45f9b06a 167
dimavb 2:ed1f45f9b06a 168 }
dimavb 2:ed1f45f9b06a 169 }
dimavb 2:ed1f45f9b06a 170
dimavb 2:ed1f45f9b06a 171
dimavb 2:ed1f45f9b06a 172 void ledstripworker()
dimavb 0:474fb16588bf 173 {
dimavb 0:474fb16588bf 174 printf("hello leds\n");
dimavb 0:474fb16588bf 175 ws.useII(WS2812::OFF); // use no intensity scaling
dimavb 0:474fb16588bf 176 Timer t;
dimavb 0:474fb16588bf 177 t.start();
dimavb 2:ed1f45f9b06a 178
dimavb 2:ed1f45f9b06a 179 px.SetAll(0x800000);
dimavb 2:ed1f45f9b06a 180 ws.write(px.getBuf());
dimavb 2:ed1f45f9b06a 181 wait(10);
dimavb 2:ed1f45f9b06a 182 rgb RGB;
dimavb 2:ed1f45f9b06a 183 hsv HSV;
dimavb 2:ed1f45f9b06a 184
dimavb 2:ed1f45f9b06a 185 int color=0;
dimavb 0:474fb16588bf 186 while(1)
dimavb 0:474fb16588bf 187 {
dimavb 2:ed1f45f9b06a 188
dimavb 2:ed1f45f9b06a 189 //if (activated)
dimavb 2:ed1f45f9b06a 190 {
dimavb 4:2f4d485ebe58 191 HSV.s=1;//0.1;
dimavb 4:2f4d485ebe58 192 HSV.v=0.1;//abs(sin(t.read()*1.11));
dimavb 2:ed1f45f9b06a 193 HSV.h=abs(sin(t.read()/30))*360;
dimavb 2:ed1f45f9b06a 194 RGB = hsv2rgb(HSV);
dimavb 2:ed1f45f9b06a 195 int r =RGB.r*255.0;
dimavb 2:ed1f45f9b06a 196 int g =RGB.g*255.0;
dimavb 2:ed1f45f9b06a 197 int b =RGB.b*255.0;
dimavb 2:ed1f45f9b06a 198
dimavb 2:ed1f45f9b06a 199 color=r<<16 | g<<8 | b;
dimavb 0:474fb16588bf 200 }
dimavb 2:ed1f45f9b06a 201 //else color=0;
dimavb 0:474fb16588bf 202
dimavb 2:ed1f45f9b06a 203 px.SetAll(color);
dimavb 2:ed1f45f9b06a 204 wait(0.025);
dimavb 2:ed1f45f9b06a 205 ws.write(px.getBuf());
dimavb 2:ed1f45f9b06a 206
dimavb 0:474fb16588bf 207 }
dimavb 0:474fb16588bf 208 }
dimavb 2:ed1f45f9b06a 209
dimavb 2:ed1f45f9b06a 210 float servoang(float ang)
dimavb 2:ed1f45f9b06a 211 {
dimavb 2:ed1f45f9b06a 212 if (ang<0) ang=0;
dimavb 2:ed1f45f9b06a 213 if (ang>180) ang=180;
dimavb 2:ed1f45f9b06a 214 ang=ang/180;
dimavb 2:ed1f45f9b06a 215 float ret = SERVOSTART + ang*(SERVOEND-SERVOSTART);
dimavb 2:ed1f45f9b06a 216 return ret;
dimavb 2:ed1f45f9b06a 217 }
dimavb 2:ed1f45f9b06a 218 //Mutex m;
dimavb 2:ed1f45f9b06a 219 void setservo(int num, float ang)
dimavb 2:ed1f45f9b06a 220 {
dimavb 2:ed1f45f9b06a 221 num++;
dimavb 2:ed1f45f9b06a 222 //pc.printf("servoset %d %f\n",num,ang);
dimavb 2:ed1f45f9b06a 223
dimavb 2:ed1f45f9b06a 224 if (num>=0 && num <16 && ang>0 && ang<181)
dimavb 2:ed1f45f9b06a 225 {
dimavb 2:ed1f45f9b06a 226 if (ang>servoLimitsEnd[num] && ang>servoLimitsStart[num])
dimavb 2:ed1f45f9b06a 227 {
dimavb 2:ed1f45f9b06a 228 //ang = servoLimitsEnd[num];
dimavb 4:2f4d485ebe58 229 // pc.printf("out of range %d %3.2f\n",num,ang);
dimavb 4:2f4d485ebe58 230 }
dimavb 2:ed1f45f9b06a 231 if (ang<servoLimitsStart[num] && ang<servoLimitsEnd[num])
dimavb 2:ed1f45f9b06a 232 {
dimavb 2:ed1f45f9b06a 233 //ang = servoLimitsStart[num];
dimavb 4:2f4d485ebe58 234 // pc.printf("out of range %d %3.2f\n",num,ang);
dimavb 4:2f4d485ebe58 235 }
dimavb 2:ed1f45f9b06a 236 //m.lock();
dimavb 2:ed1f45f9b06a 237 pwmouts.set_pwm_pw(num,servoang(ang));
dimavb 2:ed1f45f9b06a 238 //m.unlock();
dimavb 2:ed1f45f9b06a 239
dimavb 2:ed1f45f9b06a 240 } else pc.printf("wrong values %d %f\n",num,ang);
dimavb 2:ed1f45f9b06a 241
dimavb 2:ed1f45f9b06a 242 }
dimavb 2:ed1f45f9b06a 243 void servoworker()
dimavb 2:ed1f45f9b06a 244 {
dimavb 2:ed1f45f9b06a 245 Timer t;
dimavb 2:ed1f45f9b06a 246 t.start();
dimavb 2:ed1f45f9b06a 247 pwmouts.init();
dimavb 2:ed1f45f9b06a 248 for (uint8_t i=0; i<16; i++)
dimavb 2:ed1f45f9b06a 249 {
dimavb 2:ed1f45f9b06a 250 setservo(i,servoLimitsCenter[i]);
dimavb 2:ed1f45f9b06a 251 //wait(0.2);
dimavb 2:ed1f45f9b06a 252 }
dimavb 2:ed1f45f9b06a 253 while(1)
dimavb 2:ed1f45f9b06a 254 {
dimavb 2:ed1f45f9b06a 255 float an;
dimavb 2:ed1f45f9b06a 256 for (uint8_t i=0; i<16; i++)
dimavb 2:ed1f45f9b06a 257 {
dimavb 2:ed1f45f9b06a 258 // an=servoang(90.0f+sin(t.read())*90.0f);
dimavb 2:ed1f45f9b06a 259 // pwmouts.set_pwm_pw(i,1440+sin(t.read())*550.0f);
dimavb 2:ed1f45f9b06a 260 }
dimavb 2:ed1f45f9b06a 261 //printf("ang %f",an);
dimavb 2:ed1f45f9b06a 262 wait(0.3);
dimavb 2:ed1f45f9b06a 263 }
dimavb 2:ed1f45f9b06a 264
dimavb 2:ed1f45f9b06a 265 }
dimavb 2:ed1f45f9b06a 266 void messageReceive(MODSERIAL_IRQ_INFO *q) {
dimavb 2:ed1f45f9b06a 267 MODSERIAL *sys = q->serial;
dimavb 2:ed1f45f9b06a 268 memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 269 sys->move(messageBufferIncoming, MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 270 pc.rxBufferFlush();
dimavb 2:ed1f45f9b06a 271
dimavb 2:ed1f45f9b06a 272
dimavb 2:ed1f45f9b06a 273 messageReceived.release();
dimavb 2:ed1f45f9b06a 274 //return 0;
dimavb 2:ed1f45f9b06a 275 }
dimavb 2:ed1f45f9b06a 276 void serialworker (void)
dimavb 2:ed1f45f9b06a 277 {
dimavb 2:ed1f45f9b06a 278 char data[MESSAGE_BUFFER_SIZE];
dimavb 2:ed1f45f9b06a 279 while(1)
dimavb 2:ed1f45f9b06a 280 {
dimavb 2:ed1f45f9b06a 281 messageReceived.wait();
dimavb 2:ed1f45f9b06a 282
dimavb 2:ed1f45f9b06a 283 pc.printf("got '%s",messageBufferIncoming);
dimavb 2:ed1f45f9b06a 284 pc.printf("'\n");
dimavb 2:ed1f45f9b06a 285
dimavb 2:ed1f45f9b06a 286 //memset(data,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 287 memcpy(data,messageBufferIncoming,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 288 //memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 289
dimavb 2:ed1f45f9b06a 290 //pc.printf("got %s",data);
dimavb 2:ed1f45f9b06a 291 int ang;
dimavb 2:ed1f45f9b06a 292 int num;
dimavb 2:ed1f45f9b06a 293 if (sscanf(data,"%d %d\n",&num,&ang)==2)
dimavb 2:ed1f45f9b06a 294 {
dimavb 2:ed1f45f9b06a 295 if (num>0)
dimavb 2:ed1f45f9b06a 296 {
dimavb 2:ed1f45f9b06a 297 pc.printf("servo %d ang %d\n",num, ang);
dimavb 2:ed1f45f9b06a 298 setservo(num,ang);
dimavb 2:ed1f45f9b06a 299 }
dimavb 2:ed1f45f9b06a 300 else {
dimavb 2:ed1f45f9b06a 301 if (ang) mp3.mp3_play(ang); else mp3.mp3_stop();
dimavb 2:ed1f45f9b06a 302 }
dimavb 2:ed1f45f9b06a 303 };// else {}
dimavb 2:ed1f45f9b06a 304 }
dimavb 2:ed1f45f9b06a 305 }
dimavb 2:ed1f45f9b06a 306
dimavb 0:474fb16588bf 307 int main()
dimavb 0:474fb16588bf 308 {
dimavb 2:ed1f45f9b06a 309
dimavb 2:ed1f45f9b06a 310
dimavb 2:ed1f45f9b06a 311 pc.baud(115200);
dimavb 2:ed1f45f9b06a 312 pc.attach(&messageReceive, MODSERIAL::RxAutoDetect);
dimavb 2:ed1f45f9b06a 313 pc.autoDetectChar('\n');
dimavb 2:ed1f45f9b06a 314
dimavb 2:ed1f45f9b06a 315 pc.printf("hi\n");
dimavb 2:ed1f45f9b06a 316 //wait(1.0f);
dimavb 2:ed1f45f9b06a 317 mp3.mp3_play_physical(2);
dimavb 2:ed1f45f9b06a 318
dimavb 2:ed1f45f9b06a 319 Thread ledstripthread;
dimavb 2:ed1f45f9b06a 320 Thread servothread;
dimavb 2:ed1f45f9b06a 321 Thread serialthread;
dimavb 0:474fb16588bf 322
dimavb 2:ed1f45f9b06a 323 ledstripthread.start(ledstripworker);
dimavb 2:ed1f45f9b06a 324 servothread.start(servoworker);
dimavb 2:ed1f45f9b06a 325 serialthread.start(serialworker);
dimavb 2:ed1f45f9b06a 326 Action act = NONE;
dimavb 2:ed1f45f9b06a 327 char dist[12];
dimavb 2:ed1f45f9b06a 328 int activated=-1;
dimavb 6:4b007c7f0b7a 329 int activated_t=0;
dimavb 4:2f4d485ebe58 330 wait(2.5);
dimavb 2:ed1f45f9b06a 331 mp3.mp3_stop();
dimavb 4:2f4d485ebe58 332 int dist_tmp[16][4];
dimavb 2:ed1f45f9b06a 333 while(1)
dimavb 0:474fb16588bf 334 {
dimavb 2:ed1f45f9b06a 335 memset(dist,0,12);
dimavb 2:ed1f45f9b06a 336 i2c.read(0x20,dist,12);
dimavb 2:ed1f45f9b06a 337 activated=-1;
dimavb 4:2f4d485ebe58 338 for(uint8_t j=3; j>0; j--)
dimavb 4:2f4d485ebe58 339 {
dimavb 4:2f4d485ebe58 340 pc.printf("\n**");
dimavb 4:2f4d485ebe58 341 for(uint8_t i=0; i<12; i++)
dimavb 4:2f4d485ebe58 342 {
dimavb 4:2f4d485ebe58 343 dist_tmp[i][j]=dist_tmp[i][j-1];
dimavb 4:2f4d485ebe58 344 // pc.printf("%d\t ",dist_tmp[i][j]);
dimavb 4:2f4d485ebe58 345 }
dimavb 4:2f4d485ebe58 346
dimavb 4:2f4d485ebe58 347 }
dimavb 4:2f4d485ebe58 348 //pc.printf("\n");
dimavb 2:ed1f45f9b06a 349 for(uint8_t i=0; i<12; i++)
dimavb 2:ed1f45f9b06a 350 {
dimavb 2:ed1f45f9b06a 351 pc.printf("\t %d",dist[i]);
dimavb 4:2f4d485ebe58 352 dist_tmp[i][0]=dist[i];
dimavb 4:2f4d485ebe58 353 if (dist_tmp[i][0]<MIN_DIST &&
dimavb 4:2f4d485ebe58 354 dist_tmp[i][1]<MIN_DIST &&
dimavb 4:2f4d485ebe58 355 dist_tmp[i][2]<MIN_DIST &&
dimavb 4:2f4d485ebe58 356 dist_tmp[i][3]<MIN_DIST ) {
dimavb 4:2f4d485ebe58 357 activated=sensormap[i];
dimavb 2:ed1f45f9b06a 358
dimavb 4:2f4d485ebe58 359 }
dimavb 2:ed1f45f9b06a 360 //else activated=-1;
dimavb 2:ed1f45f9b06a 361 }
dimavb 4:2f4d485ebe58 362
dimavb 4:2f4d485ebe58 363
dimavb 2:ed1f45f9b06a 364 pc.printf("\n");
dimavb 6:4b007c7f0b7a 365 if (activated<0) activated_t=0;
dimavb 2:ed1f45f9b06a 366
dimavb 6:4b007c7f0b7a 367 if (activated>=0 && activated_t==0)
dimavb 2:ed1f45f9b06a 368 {
dimavb 6:4b007c7f0b7a 369 activated_t=1;
dimavb 2:ed1f45f9b06a 370 act = actions[activated];
dimavb 2:ed1f45f9b06a 371 pc.printf("activated %d action %d\n",activated,act);
dimavb 2:ed1f45f9b06a 372 float time=actionstime[activated];
dimavb 2:ed1f45f9b06a 373 switch(act)
dimavb 2:ed1f45f9b06a 374 {
dimavb 2:ed1f45f9b06a 375
dimavb 2:ed1f45f9b06a 376 case NONE:
dimavb 2:ed1f45f9b06a 377 break;
dimavb 2:ed1f45f9b06a 378
dimavb 2:ed1f45f9b06a 379 case MUS:
dimavb 2:ed1f45f9b06a 380 music(activated,time);
dimavb 2:ed1f45f9b06a 381 break;
dimavb 2:ed1f45f9b06a 382
dimavb 2:ed1f45f9b06a 383 case MLIT:
dimavb 2:ed1f45f9b06a 384 musiclight(activated,time);
dimavb 2:ed1f45f9b06a 385 break;
dimavb 2:ed1f45f9b06a 386
dimavb 2:ed1f45f9b06a 387 case SLOW:
dimavb 2:ed1f45f9b06a 388 slow_move(activated,time);
dimavb 2:ed1f45f9b06a 389 break;
dimavb 2:ed1f45f9b06a 390
dimavb 2:ed1f45f9b06a 391 case OPEN:
dimavb 2:ed1f45f9b06a 392 open(activated,time);
dimavb 2:ed1f45f9b06a 393 break;
dimavb 2:ed1f45f9b06a 394
dimavb 2:ed1f45f9b06a 395 case KACH:
dimavb 2:ed1f45f9b06a 396 if (activated==4) kachelle_double(0,1,5,time);
dimavb 2:ed1f45f9b06a 397 else kachelle(activated,time);
dimavb 2:ed1f45f9b06a 398 break;
dimavb 2:ed1f45f9b06a 399
dimavb 2:ed1f45f9b06a 400 default:
dimavb 2:ed1f45f9b06a 401 pc.printf("Action does not exist - MEGABUG\n");
dimavb 2:ed1f45f9b06a 402 wait(1);
dimavb 2:ed1f45f9b06a 403 }
dimavb 2:ed1f45f9b06a 404 activated=-1;
dimavb 2:ed1f45f9b06a 405 }
dimavb 2:ed1f45f9b06a 406 wait(0.3f);
dimavb 0:474fb16588bf 407 }
dimavb 2:ed1f45f9b06a 408
dimavb 2:ed1f45f9b06a 409
dimavb 0:474fb16588bf 410 }