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

25326

Dependencies:   QEI WS2812 PixelArray DFPlayerMini MODSERIAL PCA9685_ pca9685

main.cpp@9:4bc77e4c090e, 2019-08-23 (annotated)

Committer:
turumputum
Date:
Fri Aug 23 16:58:47 2019 +0000
Revision:
9:4bc77e4c090e
Parent:
8:8f09c13d5138 
123

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