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

2532

Dependencies:   QEI WS2812 PixelArray DFPlayerMini MODSERIAL PCA9685_ pca9685

main.cpp@4:2f4d485ebe58, 2019-06-22 (annotated)

Committer:
dimavb
Date:
Sat Jun 22 17:57:41 2019 +0000
Revision:
4:2f4d485ebe58
Parent:
2:ed1f45f9b06a
Child:
5:cec7f85ce6c4
+delay

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};
dimavb 2:ed1f45f9b06a 48 const float actionstime[16] = {5, 5, 5, 5, 5, 5, 5, 5, 2, 5, 5, 5, 5, 5, 5, 5};
dimavb 2:ed1f45f9b06a 49 const uint8_t servoLimitsStart[16]= {20, 85, 30, 40, 0, 20, 155, 105, 150, 45, 75, 130, 0, 0, 0, 0};
dimavb 2:ed1f45f9b06a 50 const uint8_t servoLimitsCenter[16]={40, 110, 30, 40, 0, 20, 155, 105, 150, 45, 85, 130, 0, 0, 0, 0};
dimavb 2:ed1f45f9b06a 51 const uint8_t servoLimitsEnd[16]= {60, 135, 50, 130, 180, 140, 70, 75, 40, 130, 95, 50, 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 {
dimavb 2:ed1f45f9b06a 85 setservo(id,servoLimitsCenter[id]+sin(at.read())*(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
dimavb 2:ed1f45f9b06a 100 setservo(id2,servoLimitsCenter[id2]+(1-sin(at.read()*3))*(servoLimitsEnd[id2]-servoLimitsStart[id2])/2);
dimavb 2:ed1f45f9b06a 101 setservo(id1,servoLimitsCenter[id1]+sin(at.read()*3)*(servoLimitsEnd[id1]-servoLimitsStart[id1])/2);
dimavb 2:ed1f45f9b06a 102 wait(0.03);
dimavb 2:ed1f45f9b06a 103 }
dimavb 2:ed1f45f9b06a 104 setservo(id1,servoLimitsCenter[id1]);
dimavb 2:ed1f45f9b06a 105 setservo(id2,servoLimitsCenter[id2]);
dimavb 2:ed1f45f9b06a 106
dimavb 2:ed1f45f9b06a 107 }
dimavb 2:ed1f45f9b06a 108
dimavb 2:ed1f45f9b06a 109 void open(int id,float time)
dimavb 2:ed1f45f9b06a 110 {
dimavb 2:ed1f45f9b06a 111 at.reset();
dimavb 2:ed1f45f9b06a 112 at.start();
dimavb 2:ed1f45f9b06a 113 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 114 setservo(id,servoLimitsEnd[id]);
dimavb 2:ed1f45f9b06a 115 while(at.read()<time)
dimavb 2:ed1f45f9b06a 116 {
dimavb 2:ed1f45f9b06a 117 wait(0.02);
dimavb 2:ed1f45f9b06a 118 }
dimavb 2:ed1f45f9b06a 119 setservo(id,servoLimitsCenter[id]);
dimavb 2:ed1f45f9b06a 120
dimavb 2:ed1f45f9b06a 121 }
dimavb 2:ed1f45f9b06a 122
dimavb 2:ed1f45f9b06a 123 void music(int id,float time)
dimavb 2:ed1f45f9b06a 124 {
dimavb 2:ed1f45f9b06a 125 at.reset();
dimavb 2:ed1f45f9b06a 126 at.start();
dimavb 2:ed1f45f9b06a 127 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 128 while(at.read()<time)
dimavb 2:ed1f45f9b06a 129 {
dimavb 2:ed1f45f9b06a 130 wait(0.02);
dimavb 0:474fb16588bf 131 }
dimavb 0:474fb16588bf 132 }
dimavb 2:ed1f45f9b06a 133
dimavb 2:ed1f45f9b06a 134 void musiclight(int id,float time)
dimavb 2:ed1f45f9b06a 135 {
dimavb 2:ed1f45f9b06a 136 at.reset();
dimavb 2:ed1f45f9b06a 137 at.start();
dimavb 2:ed1f45f9b06a 138 mp3.mp3_play(id+1);
dimavb 2:ed1f45f9b06a 139 Timer t;
dimavb 2:ed1f45f9b06a 140 t.start();
dimavb 2:ed1f45f9b06a 141 rgb RGB;
dimavb 2:ed1f45f9b06a 142 hsv HSV;
dimavb 2:ed1f45f9b06a 143 int color;
dimavb 2:ed1f45f9b06a 144 while(at.read()<time)
dimavb 2:ed1f45f9b06a 145 {
dimavb 2:ed1f45f9b06a 146 HSV.s=1;
dimavb 2:ed1f45f9b06a 147 HSV.v=1;
dimavb 2:ed1f45f9b06a 148 //HSV.v=abs(sin(t.read()*1.11));
dimavb 2:ed1f45f9b06a 149 HSV.h=abs(sin(t.read()))*360;
dimavb 2:ed1f45f9b06a 150 RGB = hsv2rgb(HSV);
dimavb 2:ed1f45f9b06a 151 int r =RGB.r*255.0;
dimavb 2:ed1f45f9b06a 152 int g =RGB.g*255.0;
dimavb 2:ed1f45f9b06a 153 int b =RGB.b*255.0;
dimavb 2:ed1f45f9b06a 154
dimavb 2:ed1f45f9b06a 155 color=r<<16 | g<<8 | b;
dimavb 2:ed1f45f9b06a 156
dimavb 2:ed1f45f9b06a 157 px.SetAll(color);
dimavb 2:ed1f45f9b06a 158
dimavb 2:ed1f45f9b06a 159 //ws.write(px.getBuf());
dimavb 2:ed1f45f9b06a 160 wait(0.02);
dimavb 2:ed1f45f9b06a 161
dimavb 2:ed1f45f9b06a 162 }
dimavb 2:ed1f45f9b06a 163 }
dimavb 2:ed1f45f9b06a 164
dimavb 2:ed1f45f9b06a 165
dimavb 2:ed1f45f9b06a 166 void ledstripworker()
dimavb 0:474fb16588bf 167 {
dimavb 0:474fb16588bf 168 printf("hello leds\n");
dimavb 0:474fb16588bf 169 ws.useII(WS2812::OFF); // use no intensity scaling
dimavb 0:474fb16588bf 170 Timer t;
dimavb 0:474fb16588bf 171 t.start();
dimavb 2:ed1f45f9b06a 172
dimavb 2:ed1f45f9b06a 173 px.SetAll(0x800000);
dimavb 2:ed1f45f9b06a 174 ws.write(px.getBuf());
dimavb 2:ed1f45f9b06a 175 wait(10);
dimavb 2:ed1f45f9b06a 176 rgb RGB;
dimavb 2:ed1f45f9b06a 177 hsv HSV;
dimavb 2:ed1f45f9b06a 178
dimavb 2:ed1f45f9b06a 179 int color=0;
dimavb 0:474fb16588bf 180 while(1)
dimavb 0:474fb16588bf 181 {
dimavb 2:ed1f45f9b06a 182
dimavb 2:ed1f45f9b06a 183 //if (activated)
dimavb 2:ed1f45f9b06a 184 {
dimavb 4:2f4d485ebe58 185 HSV.s=1;//0.1;
dimavb 4:2f4d485ebe58 186 HSV.v=0.1;//abs(sin(t.read()*1.11));
dimavb 2:ed1f45f9b06a 187 HSV.h=abs(sin(t.read()/30))*360;
dimavb 2:ed1f45f9b06a 188 RGB = hsv2rgb(HSV);
dimavb 2:ed1f45f9b06a 189 int r =RGB.r*255.0;
dimavb 2:ed1f45f9b06a 190 int g =RGB.g*255.0;
dimavb 2:ed1f45f9b06a 191 int b =RGB.b*255.0;
dimavb 2:ed1f45f9b06a 192
dimavb 2:ed1f45f9b06a 193 color=r<<16 | g<<8 | b;
dimavb 0:474fb16588bf 194 }
dimavb 2:ed1f45f9b06a 195 //else color=0;
dimavb 0:474fb16588bf 196
dimavb 2:ed1f45f9b06a 197 px.SetAll(color);
dimavb 2:ed1f45f9b06a 198 wait(0.025);
dimavb 2:ed1f45f9b06a 199 ws.write(px.getBuf());
dimavb 2:ed1f45f9b06a 200
dimavb 0:474fb16588bf 201 }
dimavb 0:474fb16588bf 202 }
dimavb 2:ed1f45f9b06a 203
dimavb 2:ed1f45f9b06a 204 float servoang(float ang)
dimavb 2:ed1f45f9b06a 205 {
dimavb 2:ed1f45f9b06a 206 if (ang<0) ang=0;
dimavb 2:ed1f45f9b06a 207 if (ang>180) ang=180;
dimavb 2:ed1f45f9b06a 208 ang=ang/180;
dimavb 2:ed1f45f9b06a 209 float ret = SERVOSTART + ang*(SERVOEND-SERVOSTART);
dimavb 2:ed1f45f9b06a 210 return ret;
dimavb 2:ed1f45f9b06a 211 }
dimavb 2:ed1f45f9b06a 212 //Mutex m;
dimavb 2:ed1f45f9b06a 213 void setservo(int num, float ang)
dimavb 2:ed1f45f9b06a 214 {
dimavb 2:ed1f45f9b06a 215 num++;
dimavb 2:ed1f45f9b06a 216 //pc.printf("servoset %d %f\n",num,ang);
dimavb 2:ed1f45f9b06a 217
dimavb 2:ed1f45f9b06a 218 if (num>=0 && num <16 && ang>0 && ang<181)
dimavb 2:ed1f45f9b06a 219 {
dimavb 2:ed1f45f9b06a 220 if (ang>servoLimitsEnd[num] && ang>servoLimitsStart[num])
dimavb 2:ed1f45f9b06a 221 {
dimavb 2:ed1f45f9b06a 222 //ang = servoLimitsEnd[num];
dimavb 4:2f4d485ebe58 223 // pc.printf("out of range %d %3.2f\n",num,ang);
dimavb 4:2f4d485ebe58 224 }
dimavb 2:ed1f45f9b06a 225 if (ang<servoLimitsStart[num] && ang<servoLimitsEnd[num])
dimavb 2:ed1f45f9b06a 226 {
dimavb 2:ed1f45f9b06a 227 //ang = servoLimitsStart[num];
dimavb 4:2f4d485ebe58 228 // pc.printf("out of range %d %3.2f\n",num,ang);
dimavb 4:2f4d485ebe58 229 }
dimavb 2:ed1f45f9b06a 230 //m.lock();
dimavb 2:ed1f45f9b06a 231 pwmouts.set_pwm_pw(num,servoang(ang));
dimavb 2:ed1f45f9b06a 232 //m.unlock();
dimavb 2:ed1f45f9b06a 233
dimavb 2:ed1f45f9b06a 234 } else pc.printf("wrong values %d %f\n",num,ang);
dimavb 2:ed1f45f9b06a 235
dimavb 2:ed1f45f9b06a 236 }
dimavb 2:ed1f45f9b06a 237 void servoworker()
dimavb 2:ed1f45f9b06a 238 {
dimavb 2:ed1f45f9b06a 239 Timer t;
dimavb 2:ed1f45f9b06a 240 t.start();
dimavb 2:ed1f45f9b06a 241 pwmouts.init();
dimavb 2:ed1f45f9b06a 242 for (uint8_t i=0; i<16; i++)
dimavb 2:ed1f45f9b06a 243 {
dimavb 2:ed1f45f9b06a 244 setservo(i,servoLimitsCenter[i]);
dimavb 2:ed1f45f9b06a 245 //wait(0.2);
dimavb 2:ed1f45f9b06a 246 }
dimavb 2:ed1f45f9b06a 247 while(1)
dimavb 2:ed1f45f9b06a 248 {
dimavb 2:ed1f45f9b06a 249 float an;
dimavb 2:ed1f45f9b06a 250 for (uint8_t i=0; i<16; i++)
dimavb 2:ed1f45f9b06a 251 {
dimavb 2:ed1f45f9b06a 252 // an=servoang(90.0f+sin(t.read())*90.0f);
dimavb 2:ed1f45f9b06a 253 // pwmouts.set_pwm_pw(i,1440+sin(t.read())*550.0f);
dimavb 2:ed1f45f9b06a 254 }
dimavb 2:ed1f45f9b06a 255 //printf("ang %f",an);
dimavb 2:ed1f45f9b06a 256 wait(0.3);
dimavb 2:ed1f45f9b06a 257 }
dimavb 2:ed1f45f9b06a 258
dimavb 2:ed1f45f9b06a 259 }
dimavb 2:ed1f45f9b06a 260 void messageReceive(MODSERIAL_IRQ_INFO *q) {
dimavb 2:ed1f45f9b06a 261 MODSERIAL *sys = q->serial;
dimavb 2:ed1f45f9b06a 262 memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 263 sys->move(messageBufferIncoming, MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 264 pc.rxBufferFlush();
dimavb 2:ed1f45f9b06a 265
dimavb 2:ed1f45f9b06a 266
dimavb 2:ed1f45f9b06a 267 messageReceived.release();
dimavb 2:ed1f45f9b06a 268 //return 0;
dimavb 2:ed1f45f9b06a 269 }
dimavb 2:ed1f45f9b06a 270 void serialworker (void)
dimavb 2:ed1f45f9b06a 271 {
dimavb 2:ed1f45f9b06a 272 char data[MESSAGE_BUFFER_SIZE];
dimavb 2:ed1f45f9b06a 273 while(1)
dimavb 2:ed1f45f9b06a 274 {
dimavb 2:ed1f45f9b06a 275 messageReceived.wait();
dimavb 2:ed1f45f9b06a 276
dimavb 2:ed1f45f9b06a 277 pc.printf("got '%s",messageBufferIncoming);
dimavb 2:ed1f45f9b06a 278 pc.printf("'\n");
dimavb 2:ed1f45f9b06a 279
dimavb 2:ed1f45f9b06a 280 //memset(data,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 281 memcpy(data,messageBufferIncoming,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 282 //memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 283
dimavb 2:ed1f45f9b06a 284 //pc.printf("got %s",data);
dimavb 2:ed1f45f9b06a 285 int ang;
dimavb 2:ed1f45f9b06a 286 int num;
dimavb 2:ed1f45f9b06a 287 if (sscanf(data,"%d %d\n",&num,&ang)==2)
dimavb 2:ed1f45f9b06a 288 {
dimavb 2:ed1f45f9b06a 289 if (num>0)
dimavb 2:ed1f45f9b06a 290 {
dimavb 2:ed1f45f9b06a 291 pc.printf("servo %d ang %d\n",num, ang);
dimavb 2:ed1f45f9b06a 292 setservo(num,ang);
dimavb 2:ed1f45f9b06a 293 }
dimavb 2:ed1f45f9b06a 294 else {
dimavb 2:ed1f45f9b06a 295 if (ang) mp3.mp3_play(ang); else mp3.mp3_stop();
dimavb 2:ed1f45f9b06a 296 }
dimavb 2:ed1f45f9b06a 297 };// else {}
dimavb 2:ed1f45f9b06a 298 }
dimavb 2:ed1f45f9b06a 299 }
dimavb 2:ed1f45f9b06a 300
dimavb 0:474fb16588bf 301 int main()
dimavb 0:474fb16588bf 302 {
dimavb 2:ed1f45f9b06a 303
dimavb 2:ed1f45f9b06a 304
dimavb 2:ed1f45f9b06a 305 pc.baud(115200);
dimavb 2:ed1f45f9b06a 306 pc.attach(&messageReceive, MODSERIAL::RxAutoDetect);
dimavb 2:ed1f45f9b06a 307 pc.autoDetectChar('\n');
dimavb 2:ed1f45f9b06a 308
dimavb 2:ed1f45f9b06a 309 pc.printf("hi\n");
dimavb 2:ed1f45f9b06a 310 //wait(1.0f);
dimavb 2:ed1f45f9b06a 311 mp3.mp3_play_physical(2);
dimavb 2:ed1f45f9b06a 312
dimavb 2:ed1f45f9b06a 313 Thread ledstripthread;
dimavb 2:ed1f45f9b06a 314 Thread servothread;
dimavb 2:ed1f45f9b06a 315 Thread serialthread;
dimavb 0:474fb16588bf 316
dimavb 2:ed1f45f9b06a 317 ledstripthread.start(ledstripworker);
dimavb 2:ed1f45f9b06a 318 servothread.start(servoworker);
dimavb 2:ed1f45f9b06a 319 serialthread.start(serialworker);
dimavb 2:ed1f45f9b06a 320 Action act = NONE;
dimavb 2:ed1f45f9b06a 321 char dist[12];
dimavb 2:ed1f45f9b06a 322 int activated=-1;
dimavb 4:2f4d485ebe58 323 wait(2.5);
dimavb 2:ed1f45f9b06a 324 mp3.mp3_stop();
dimavb 4:2f4d485ebe58 325 int dist_tmp[16][4];
dimavb 2:ed1f45f9b06a 326 while(1)
dimavb 0:474fb16588bf 327 {
dimavb 2:ed1f45f9b06a 328 memset(dist,0,12);
dimavb 2:ed1f45f9b06a 329 i2c.read(0x20,dist,12);
dimavb 2:ed1f45f9b06a 330 activated=-1;
dimavb 4:2f4d485ebe58 331 for(uint8_t j=3; j>0; j--)
dimavb 4:2f4d485ebe58 332 {
dimavb 4:2f4d485ebe58 333 pc.printf("\n**");
dimavb 4:2f4d485ebe58 334 for(uint8_t i=0; i<12; i++)
dimavb 4:2f4d485ebe58 335 {
dimavb 4:2f4d485ebe58 336 dist_tmp[i][j]=dist_tmp[i][j-1];
dimavb 4:2f4d485ebe58 337 // pc.printf("%d\t ",dist_tmp[i][j]);
dimavb 4:2f4d485ebe58 338 }
dimavb 4:2f4d485ebe58 339
dimavb 4:2f4d485ebe58 340 }
dimavb 4:2f4d485ebe58 341 //pc.printf("\n");
dimavb 2:ed1f45f9b06a 342 for(uint8_t i=0; i<12; i++)
dimavb 2:ed1f45f9b06a 343 {
dimavb 2:ed1f45f9b06a 344 pc.printf("\t %d",dist[i]);
dimavb 4:2f4d485ebe58 345 dist_tmp[i][0]=dist[i];
dimavb 4:2f4d485ebe58 346 if (dist_tmp[i][0]<MIN_DIST &&
dimavb 4:2f4d485ebe58 347 dist_tmp[i][1]<MIN_DIST &&
dimavb 4:2f4d485ebe58 348 dist_tmp[i][2]<MIN_DIST &&
dimavb 4:2f4d485ebe58 349 dist_tmp[i][3]<MIN_DIST ) {
dimavb 4:2f4d485ebe58 350 activated=sensormap[i];
dimavb 2:ed1f45f9b06a 351
dimavb 4:2f4d485ebe58 352 }
dimavb 2:ed1f45f9b06a 353 //else activated=-1;
dimavb 2:ed1f45f9b06a 354 }
dimavb 4:2f4d485ebe58 355
dimavb 4:2f4d485ebe58 356
dimavb 2:ed1f45f9b06a 357 pc.printf("\n");
dimavb 2:ed1f45f9b06a 358
dimavb 2:ed1f45f9b06a 359 if (activated>=0)
dimavb 2:ed1f45f9b06a 360 {
dimavb 2:ed1f45f9b06a 361 act = actions[activated];
dimavb 2:ed1f45f9b06a 362 pc.printf("activated %d action %d\n",activated,act);
dimavb 2:ed1f45f9b06a 363 float time=actionstime[activated];
dimavb 2:ed1f45f9b06a 364 switch(act)
dimavb 2:ed1f45f9b06a 365 {
dimavb 2:ed1f45f9b06a 366
dimavb 2:ed1f45f9b06a 367 case NONE:
dimavb 2:ed1f45f9b06a 368 break;
dimavb 2:ed1f45f9b06a 369
dimavb 2:ed1f45f9b06a 370 case MUS:
dimavb 2:ed1f45f9b06a 371 music(activated,time);
dimavb 2:ed1f45f9b06a 372 break;
dimavb 2:ed1f45f9b06a 373
dimavb 2:ed1f45f9b06a 374 case MLIT:
dimavb 2:ed1f45f9b06a 375 musiclight(activated,time);
dimavb 2:ed1f45f9b06a 376 break;
dimavb 2:ed1f45f9b06a 377
dimavb 2:ed1f45f9b06a 378 case SLOW:
dimavb 2:ed1f45f9b06a 379 slow_move(activated,time);
dimavb 2:ed1f45f9b06a 380 break;
dimavb 2:ed1f45f9b06a 381
dimavb 2:ed1f45f9b06a 382 case OPEN:
dimavb 2:ed1f45f9b06a 383 open(activated,time);
dimavb 2:ed1f45f9b06a 384 break;
dimavb 2:ed1f45f9b06a 385
dimavb 2:ed1f45f9b06a 386 case KACH:
dimavb 2:ed1f45f9b06a 387 if (activated==4) kachelle_double(0,1,5,time);
dimavb 2:ed1f45f9b06a 388 else kachelle(activated,time);
dimavb 2:ed1f45f9b06a 389 break;
dimavb 2:ed1f45f9b06a 390
dimavb 2:ed1f45f9b06a 391 default:
dimavb 2:ed1f45f9b06a 392 pc.printf("Action does not exist - MEGABUG\n");
dimavb 2:ed1f45f9b06a 393 wait(1);
dimavb 2:ed1f45f9b06a 394 }
dimavb 2:ed1f45f9b06a 395 activated=-1;
dimavb 2:ed1f45f9b06a 396 }
dimavb 2:ed1f45f9b06a 397 wait(0.3f);
dimavb 0:474fb16588bf 398 }
dimavb 2:ed1f45f9b06a 399
dimavb 2:ed1f45f9b06a 400
dimavb 0:474fb16588bf 401 }