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

2532

Dependencies:   QEI WS2812 PixelArray DFPlayerMini MODSERIAL PCA9685_ pca9685

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

Committer:
dimavb
Date:
Sat Jun 22 17:24:44 2019 +0000
Revision:
2:ed1f45f9b06a
Parent:
0:474fb16588bf
Child:
4:2f4d485ebe58
init

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 2:ed1f45f9b06a 21
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 2:ed1f45f9b06a 185 HSV.s=0.1;
dimavb 2:ed1f45f9b06a 186 HSV.v=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 2:ed1f45f9b06a 223 pc.printf("out of range %d %3.2f\n",num,ang);}
dimavb 2:ed1f45f9b06a 224 if (ang<servoLimitsStart[num] && ang<servoLimitsEnd[num])
dimavb 2:ed1f45f9b06a 225 {
dimavb 2:ed1f45f9b06a 226 //ang = servoLimitsStart[num];
dimavb 2:ed1f45f9b06a 227 pc.printf("out of range %d %3.2f\n",num,ang);}
dimavb 2:ed1f45f9b06a 228 //m.lock();
dimavb 2:ed1f45f9b06a 229 pwmouts.set_pwm_pw(num,servoang(ang));
dimavb 2:ed1f45f9b06a 230 //m.unlock();
dimavb 2:ed1f45f9b06a 231
dimavb 2:ed1f45f9b06a 232 } else pc.printf("wrong values %d %f\n",num,ang);
dimavb 2:ed1f45f9b06a 233
dimavb 2:ed1f45f9b06a 234 }
dimavb 2:ed1f45f9b06a 235 void servoworker()
dimavb 2:ed1f45f9b06a 236 {
dimavb 2:ed1f45f9b06a 237 Timer t;
dimavb 2:ed1f45f9b06a 238 t.start();
dimavb 2:ed1f45f9b06a 239 pwmouts.init();
dimavb 2:ed1f45f9b06a 240 for (uint8_t i=0; i<16; i++)
dimavb 2:ed1f45f9b06a 241 {
dimavb 2:ed1f45f9b06a 242 setservo(i,servoLimitsCenter[i]);
dimavb 2:ed1f45f9b06a 243 //wait(0.2);
dimavb 2:ed1f45f9b06a 244 }
dimavb 2:ed1f45f9b06a 245 while(1)
dimavb 2:ed1f45f9b06a 246 {
dimavb 2:ed1f45f9b06a 247 float an;
dimavb 2:ed1f45f9b06a 248 for (uint8_t i=0; i<16; i++)
dimavb 2:ed1f45f9b06a 249 {
dimavb 2:ed1f45f9b06a 250 // an=servoang(90.0f+sin(t.read())*90.0f);
dimavb 2:ed1f45f9b06a 251 // pwmouts.set_pwm_pw(i,1440+sin(t.read())*550.0f);
dimavb 2:ed1f45f9b06a 252 }
dimavb 2:ed1f45f9b06a 253 //printf("ang %f",an);
dimavb 2:ed1f45f9b06a 254 wait(0.3);
dimavb 2:ed1f45f9b06a 255 }
dimavb 2:ed1f45f9b06a 256
dimavb 2:ed1f45f9b06a 257 }
dimavb 2:ed1f45f9b06a 258 void messageReceive(MODSERIAL_IRQ_INFO *q) {
dimavb 2:ed1f45f9b06a 259 MODSERIAL *sys = q->serial;
dimavb 2:ed1f45f9b06a 260 memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 261 sys->move(messageBufferIncoming, MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 262 pc.rxBufferFlush();
dimavb 2:ed1f45f9b06a 263
dimavb 2:ed1f45f9b06a 264
dimavb 2:ed1f45f9b06a 265 messageReceived.release();
dimavb 2:ed1f45f9b06a 266 //return 0;
dimavb 2:ed1f45f9b06a 267 }
dimavb 2:ed1f45f9b06a 268 void serialworker (void)
dimavb 2:ed1f45f9b06a 269 {
dimavb 2:ed1f45f9b06a 270 char data[MESSAGE_BUFFER_SIZE];
dimavb 2:ed1f45f9b06a 271 while(1)
dimavb 2:ed1f45f9b06a 272 {
dimavb 2:ed1f45f9b06a 273 messageReceived.wait();
dimavb 2:ed1f45f9b06a 274
dimavb 2:ed1f45f9b06a 275 pc.printf("got '%s",messageBufferIncoming);
dimavb 2:ed1f45f9b06a 276 pc.printf("'\n");
dimavb 2:ed1f45f9b06a 277
dimavb 2:ed1f45f9b06a 278 //memset(data,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 279 memcpy(data,messageBufferIncoming,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 280 //memset(messageBufferIncoming,0,MESSAGE_BUFFER_SIZE);
dimavb 2:ed1f45f9b06a 281
dimavb 2:ed1f45f9b06a 282 //pc.printf("got %s",data);
dimavb 2:ed1f45f9b06a 283 int ang;
dimavb 2:ed1f45f9b06a 284 int num;
dimavb 2:ed1f45f9b06a 285 if (sscanf(data,"%d %d\n",&num,&ang)==2)
dimavb 2:ed1f45f9b06a 286 {
dimavb 2:ed1f45f9b06a 287 if (num>0)
dimavb 2:ed1f45f9b06a 288 {
dimavb 2:ed1f45f9b06a 289 pc.printf("servo %d ang %d\n",num, ang);
dimavb 2:ed1f45f9b06a 290 setservo(num,ang);
dimavb 2:ed1f45f9b06a 291 }
dimavb 2:ed1f45f9b06a 292 else {
dimavb 2:ed1f45f9b06a 293 if (ang) mp3.mp3_play(ang); else mp3.mp3_stop();
dimavb 2:ed1f45f9b06a 294 }
dimavb 2:ed1f45f9b06a 295 };// else {}
dimavb 2:ed1f45f9b06a 296 }
dimavb 2:ed1f45f9b06a 297 }
dimavb 2:ed1f45f9b06a 298
dimavb 0:474fb16588bf 299 int main()
dimavb 0:474fb16588bf 300 {
dimavb 2:ed1f45f9b06a 301
dimavb 2:ed1f45f9b06a 302
dimavb 2:ed1f45f9b06a 303 pc.baud(115200);
dimavb 2:ed1f45f9b06a 304 pc.attach(&messageReceive, MODSERIAL::RxAutoDetect);
dimavb 2:ed1f45f9b06a 305 pc.autoDetectChar('\n');
dimavb 2:ed1f45f9b06a 306
dimavb 2:ed1f45f9b06a 307 pc.printf("hi\n");
dimavb 2:ed1f45f9b06a 308 //wait(1.0f);
dimavb 2:ed1f45f9b06a 309 mp3.mp3_play_physical(2);
dimavb 2:ed1f45f9b06a 310
dimavb 2:ed1f45f9b06a 311 Thread ledstripthread;
dimavb 2:ed1f45f9b06a 312 Thread servothread;
dimavb 2:ed1f45f9b06a 313 Thread serialthread;
dimavb 0:474fb16588bf 314
dimavb 2:ed1f45f9b06a 315 ledstripthread.start(ledstripworker);
dimavb 2:ed1f45f9b06a 316 servothread.start(servoworker);
dimavb 2:ed1f45f9b06a 317 serialthread.start(serialworker);
dimavb 2:ed1f45f9b06a 318 Action act = NONE;
dimavb 2:ed1f45f9b06a 319 char dist[12];
dimavb 2:ed1f45f9b06a 320 int activated=-1;
dimavb 2:ed1f45f9b06a 321 wait(2);
dimavb 2:ed1f45f9b06a 322 mp3.mp3_stop();
dimavb 2:ed1f45f9b06a 323 while(1)
dimavb 0:474fb16588bf 324 {
dimavb 2:ed1f45f9b06a 325 memset(dist,0,12);
dimavb 2:ed1f45f9b06a 326 i2c.read(0x20,dist,12);
dimavb 2:ed1f45f9b06a 327 activated=-1;
dimavb 2:ed1f45f9b06a 328 for(uint8_t i=0; i<12; i++)
dimavb 2:ed1f45f9b06a 329 {
dimavb 2:ed1f45f9b06a 330 pc.printf("\t %d",dist[i]);
dimavb 2:ed1f45f9b06a 331 if (dist[i]<35) {
dimavb 2:ed1f45f9b06a 332 activated=sensormap[i];
dimavb 2:ed1f45f9b06a 333
dimavb 2:ed1f45f9b06a 334 }
dimavb 2:ed1f45f9b06a 335 //else activated=-1;
dimavb 2:ed1f45f9b06a 336 }
dimavb 2:ed1f45f9b06a 337 pc.printf("\n");
dimavb 2:ed1f45f9b06a 338
dimavb 2:ed1f45f9b06a 339 if (activated>=0)
dimavb 2:ed1f45f9b06a 340 {
dimavb 2:ed1f45f9b06a 341 act = actions[activated];
dimavb 2:ed1f45f9b06a 342 pc.printf("activated %d action %d\n",activated,act);
dimavb 2:ed1f45f9b06a 343 float time=actionstime[activated];
dimavb 2:ed1f45f9b06a 344 switch(act)
dimavb 2:ed1f45f9b06a 345 {
dimavb 2:ed1f45f9b06a 346
dimavb 2:ed1f45f9b06a 347 case NONE:
dimavb 2:ed1f45f9b06a 348 break;
dimavb 2:ed1f45f9b06a 349
dimavb 2:ed1f45f9b06a 350 case MUS:
dimavb 2:ed1f45f9b06a 351 music(activated,time);
dimavb 2:ed1f45f9b06a 352 break;
dimavb 2:ed1f45f9b06a 353
dimavb 2:ed1f45f9b06a 354 case MLIT:
dimavb 2:ed1f45f9b06a 355 musiclight(activated,time);
dimavb 2:ed1f45f9b06a 356 break;
dimavb 2:ed1f45f9b06a 357
dimavb 2:ed1f45f9b06a 358 case SLOW:
dimavb 2:ed1f45f9b06a 359 slow_move(activated,time);
dimavb 2:ed1f45f9b06a 360 break;
dimavb 2:ed1f45f9b06a 361
dimavb 2:ed1f45f9b06a 362 case OPEN:
dimavb 2:ed1f45f9b06a 363 open(activated,time);
dimavb 2:ed1f45f9b06a 364 break;
dimavb 2:ed1f45f9b06a 365
dimavb 2:ed1f45f9b06a 366 case KACH:
dimavb 2:ed1f45f9b06a 367 if (activated==4) kachelle_double(0,1,5,time);
dimavb 2:ed1f45f9b06a 368 else kachelle(activated,time);
dimavb 2:ed1f45f9b06a 369 break;
dimavb 2:ed1f45f9b06a 370
dimavb 2:ed1f45f9b06a 371 default:
dimavb 2:ed1f45f9b06a 372 pc.printf("Action does not exist - MEGABUG\n");
dimavb 2:ed1f45f9b06a 373 wait(1);
dimavb 2:ed1f45f9b06a 374 }
dimavb 2:ed1f45f9b06a 375 activated=-1;
dimavb 2:ed1f45f9b06a 376 }
dimavb 2:ed1f45f9b06a 377 wait(0.3f);
dimavb 0:474fb16588bf 378 }
dimavb 2:ed1f45f9b06a 379
dimavb 2:ed1f45f9b06a 380
dimavb 0:474fb16588bf 381 }