semestralka
Dependencies: FRDM_MMA8451Q mbed
Diff: Vytah.cpp
- Revision:
- 0:6f3a386422fa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vytah.cpp Mon Feb 06 16:21:00 2017 +0000
@@ -0,0 +1,731 @@
+ #include "mbed.h"
+ #include "Header.h"
+ #include "adresarik.h"
+
+
+
+int main()
+{
+ term.baud(57600);
+ term.attach(&prerusVysielanie, Serial::RxIrq);
+
+ inicializuj();
+ sleduj_cas.attach(&watchdog, 1.4);
+ sleduj_lcd.attach(&lcd_display,0.5);
+ acc.MotionDetection(&nudzovaBrzda);
+
+ while(1) {
+ tlacitkoStena();
+ tlacitkoVytah();
+ }
+ }
+
+ /*****KOmunikacia******************************************************/
+ /*sekcia ktora riadi komunikaciu*/
+
+ void posliSpravu(int addrRx, int addrTx,int dataSize, int aData[])
+ {
+
+ int aPacket[5+dataSize];
+ aPacket[0] = 0xA0;
+ aPacket[1] = addrRx;
+ aPacket[2] = addrTx;
+ aPacket[3] = dataSize;
+
+ for(int i = 0; i < dataSize; i++){
+ aPacket[4+i] = aData[i];
+ }
+
+ int crc = getCrc(addrRx, addrTx, dataSize, aData);
+ aPacket[4+dataSize] = crc;
+
+
+ prijmi = 0;
+
+ for(int i = 0; i < ARRAY_SIZE(aPacket); i++){
+ term.putc(aPacket[i]);
+ }
+}
+
+int getCrc(int addrRx, int addrTx, int dataSize, int aData[])
+{
+ int aCRC[] = {0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
+ 157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
+ 35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
+ 190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
+ 70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
+ 219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
+ 101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
+ 248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
+ 140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
+ 17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
+ 175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
+ 50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
+ 202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
+ 87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
+ 233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
+ 116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
+ };
+ int crc = 0;
+ crc = aCRC[crc ^ addrRx];
+ crc = aCRC[crc ^ addrTx];
+ for(int i = 0; i < dataSize; i++){
+ crc = aCRC[crc ^ aData[i]];
+ }
+ return crc;
+}
+
+void inicializuj()
+{
+ inicializacia = 1;
+ resetuj_watchdog();
+ uvolniNB();
+ dole();
+}
+
+void prerusVysielanie()
+{
+
+
+ prijmi = 1;
+ for(int i = 0; i < 4; i++) {
+ adresa[i] = term.getc();
+ }
+
+
+ if(adresa[3] == 0x00) {
+ sprava[0] = term.getc();
+ }else{
+ for(int i = 0; i < adresa[3]; i++){
+ sprava[i]=term.getc();
+ }
+ adresa[4]=term.getc();
+ if(adresa[4] == getCrc(adresa[1], adresa[2],adresa[3],sprava)){
+
+ }
+ }
+
+ riadeniePohybu();
+
+
+}
+/****Pohy**************************************************************************/
+/***sekcia Pohybu******************************************************************/
+
+
+
+int dajPoziciu()
+{
+ int pozicia;
+ switch(adresa[2]){
+ case SWITCH_P:{
+ pozicia = SWITCH_P;
+ cisloPochodia_dislpej = 0;
+ break;
+ }
+ case SWITCH_1:{
+ pozicia = SWITCH_1;
+ cisloPochodia_dislpej = 1;
+ break;
+ }
+ case SWITCH_2:{
+ pozicia = SWITCH_2;
+ cisloPochodia_dislpej = 2;
+ break;
+ }
+ case SWITCH_3:{
+ pozicia = SWITCH_3;
+ cisloPochodia_dislpej = 3;
+ break;
+ }
+ case SWITCH_4:{
+ pozicia = SWITCH_4;
+ cisloPochodia_dislpej = 4;
+ break;
+ }
+ }
+ return pozicia;
+}
+
+void tlacitkoVytah()
+{
+ if(prijmi == 1){
+ switch(adresa[2]){
+ case C_P_BUTTON:{
+
+ p_0 = 1;
+ init_kabina++;
+ if(kere_poschodie > 0){
+ dole();
+ }else if(kere_poschodie < 0){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_0 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case C_1_BUTTON:{
+
+ p_1 = 1;
+ init_kabina++;
+ if(kere_poschodie > 1){
+ dole();
+ }else if(kere_poschodie < 1){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_1 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case C_2_BUTTON:{
+
+ p_2 = 1;
+ init_kabina++;
+ if(kere_poschodie > 2){
+ dole();
+ }else if(kere_poschodie < 2){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_2 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case C_3_BUTTON:{
+
+ p_3 = 1;
+ init_kabina++;
+ if(kere_poschodie > 3){
+ dole();
+ }else if(kere_poschodie < 3){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_3 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case C_4_BUTTON:{
+
+ p_4 = 1;
+ init_kabina++;
+ if(kere_poschodie > 4){
+ dole();
+ }else if(kere_poschodie < 4){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_4 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ }
+ }
+}
+
+void tlacitkoStena()
+{
+ if(prijmi == 1){
+ switch(adresa[2]){
+ case W_P_BUTTON:{
+
+ p_0 = 1;
+ stena++;
+ if(kere_poschodie > 0){
+ dole();
+ }else if(kere_poschodie < 0){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_0 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case W_1_BUTTON:{
+
+ p_1 = 1;
+ stena++;
+ if(kere_poschodie > 1){
+ dole();
+ }else if(kere_poschodie < 1){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_1 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case W_2_BUTTON:{
+
+ p_2 = 1;
+ stena++;
+ if(kere_poschodie > 2){
+ dole();
+ }else if(kere_poschodie < 2){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_2 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case W_3_BUTTON:{
+
+ p_3 = 1;
+ stena++;
+ if(kere_poschodie > 3){
+ dole();
+ }else if(kere_poschodie < 3){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_3 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ case W_4_BUTTON:{
+
+ p_4 = 1;
+ stena++;
+ if(kere_poschodie > 4){
+ dole();
+ }else if(kere_poschodie < 4){
+ hore();
+ }else{
+ if(init_pohyb == 0){
+ p_4 = 0;
+
+ }
+ }
+ prijmi = 0;
+ break;
+ }
+ }
+ }
+}
+
+void dockajNaPoschodi(){
+ dockaj++;
+ if(dockaj > 5){
+ dockaj = 0;
+ cakajNaPoschodi.detach();
+ pohniSa();
+ }
+}
+
+void zamkni()
+{
+ int aDataIn[] = {0x01};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(C_LOCK,0x00,dataSize, aDataIn);
+}
+void unzamkni()
+{
+ int aDataIn[] = {0x00};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(C_LOCK,0x00,dataSize, aDataIn);
+}
+
+void hore()
+{
+ if(dockaj == 0){
+ pohyb++;
+ if(init_pohyb == 0){
+ init_pohyb = 1;
+ zamkni();
+ int aDataIn[] = {0x2, 0x64, 0x00, 0x00, 0x00};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(MOTOR,0x00,dataSize, aDataIn);
+ smer = 1;
+ }
+ if(pohyb > 2){
+ pohyb = 0;
+ init_pohyb = 1;
+ zamkni();
+ int aDataIn[] = {0x2, 0x64, 0x00, 0x00, 0x00};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(MOTOR,0x00,dataSize, aDataIn);
+ smer = 1;
+ }
+ }
+}
+
+void dole()
+{
+ if(dockaj == 0){
+ pohyb++;
+ if(init_pohyb == 0){
+ init_pohyb = -1;
+ zamkni();
+ int aDataIn[] = {0x2, 0x9C, 0xff, 0xff, 0xff};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(MOTOR,0x00,dataSize, aDataIn);
+ smer = -1;
+ }
+ if(pohyb > 2){
+ pohyb = 0;
+ init_pohyb = -1;
+ zamkni();
+ int aDataIn[] = {0x2, 0x9C, 0xff, 0xff, 0xff};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(MOTOR,0x00,dataSize, aDataIn);
+ smer = -1;
+ }
+ }
+}
+
+void zastav()
+{
+ if(dockaj == 0){
+ init_pohyb = 0;
+ int aDataIn[] = {0x1};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(MOTOR,0x00,dataSize, aDataIn);
+ delayer.attach(&zamkni, 0.4);
+ }
+}
+
+
+void pohniSa()
+{
+ switch(kere_poschodie){
+ case 0:{
+ if (p_1 == 1 || p_2 == 1 || p_3 == 1 || p_4 == 1){
+ hore();
+ }
+ break;
+ }
+ case 1:{
+ if(smer == -1){
+ if(p_0 == 1){
+ dole();
+ }else if (p_2 == 1 || p_3 == 1 || p_4 == 1){
+ hore();
+ }
+ }else if(smer == 1){
+ if (p_2 == 1 || p_3 == 1 || p_4 == 1){
+ hore();
+ }else if(p_0 == 1){
+ dole();
+ }
+ }
+ break;
+ }
+ case 2:{
+ if(smer == -1){
+ if(p_0 == 1 || p_1 == 1){
+ dole();
+ }else if(p_3 == 1 || p_4 == 1){
+ hore();
+ }
+ }else if(smer == 1){
+ if(p_3 == 1 || p_4 == 1){
+ hore();
+ }else if(p_0 == 1 || p_1 == 1){
+ dole();
+ }
+ }
+ break;
+ }
+ case 3:{
+ if(smer == -1){
+ if (p_0 == 1 || p_1 == 1 || p_2 == 1){
+ dole();
+ }else if(p_4 == 1){
+ hore();
+ }
+ }else if(smer == 1){
+ if(p_4 == 1){
+ hore();
+ }else if(p_0 == 1 || p_1 == 1 || p_2 == 1){
+ dole();
+ }
+ }
+ break;
+ }
+ case 4:{
+ if(p_0 == 1 || p_1 == 1 || p_2 == 1 || p_3 == 1){
+ dole();
+ }
+ break;
+ }
+ }
+}
+
+
+
+void nudzovaBrzda()
+{
+ int aDataIn[] = {0x01};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(EM_BRAKE,0x00,dataSize, aDataIn);
+}
+
+void uvolniNB()
+{
+ int aDataIn[] = {0x00};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(EM_BRAKE,0x00,dataSize, aDataIn);
+}
+
+void lcd_display()
+{
+ int aDataIn[2];
+ int dataSize = ARRAY_SIZE(aDataIn);
+ switch(init_pohyb){
+ case 0:{
+ aDataIn[0] = 0x03;
+ break;
+ }
+ case 1:{
+ aDataIn[0] = 0x01;
+ break;
+ }
+ case -1:{
+ aDataIn[0] = 0x02;
+ break;
+ }
+ }
+ switch(cisloPochodia_dislpej){
+ case 0:{
+ aDataIn[1] = 0x50;
+ break;
+ }
+ case 1:{
+ aDataIn[1] = 0x31;
+ break;
+ }
+ case 2:{
+ aDataIn[1] = 0x32;
+ break;
+ }
+ case 3:{
+ aDataIn[1] = 0x33;
+ break;
+ }
+ case 4:{
+ aDataIn[1] = 0x34;
+ break;
+ }
+ }
+ if(cisloPochodia_dislpej != disp_poschodie){
+ posliSpravu(C_LCD,0x00,dataSize, aDataIn);
+ }else if(init_pohyb != disp_poschodieSmer){
+ posliSpravu(C_LCD,0x00,dataSize, aDataIn);
+ }
+ disp_poschodie = cisloPochodia_dislpej;
+ disp_poschodieSmer = init_pohyb;
+}
+
+void riadeniePohybu()
+{
+ if (dajPoziciu() != pozicia){
+ if(inicializacia == 1){
+ if(dajPoziciu() == SWITCH_P){
+ inicializacia = 0;
+ zastav();
+ }
+ }else{
+ if(smer == 1){
+ if(p_0 == 1){
+ if(dajPoziciu() == SWITCH_P){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_0 = 0;
+ kere_poschodie = 0;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_1 == 1){
+ if(dajPoziciu() == SWITCH_1){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_1 = 0;
+ kere_poschodie = 1;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_2 == 1){
+ if(dajPoziciu() == SWITCH_2){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_2 = 0;
+ kere_poschodie = 2;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_3 == 1){
+ if(dajPoziciu() == SWITCH_3){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_3 = 0;
+ kere_poschodie = 3;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_4 == 1){
+ if(dajPoziciu() == SWITCH_4){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_4 = 0;
+ kere_poschodie = 4;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }
+ }else if(smer == -1){
+ if(p_4 == 1){
+ if(dajPoziciu() == SWITCH_4){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_4 = 0;
+ kere_poschodie = 4;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_3 == 1){
+ if(dajPoziciu() == SWITCH_3){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_3 = 0;
+ kere_poschodie = 3;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_2 == 1){
+ if(dajPoziciu() == SWITCH_2){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_2 = 0;
+ kere_poschodie = 2;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_1 == 1){
+ if(dajPoziciu() == SWITCH_1){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_1 = 0;
+ kere_poschodie = 1;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }else if(p_0 == 1){
+ if(dajPoziciu() == SWITCH_P){
+ zastav();
+ if(init_kabina > 0){
+
+ init_kabina--;
+ }
+ if(stena > 0){
+
+ stena--;
+ }
+ p_0 = 0;
+ kere_poschodie = 0;
+ cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);
+ }
+ }
+ }
+ }
+ }
+ pozicia = dajPoziciu();
+}
+
+void watchdog()
+{
+ int aDataIn[] = {0x00};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(WATCHDOG,0x00,dataSize, aDataIn);
+}
+
+void resetuj_watchdog()
+{
+ int aDataIn[] = {0x01};
+ int dataSize = ARRAY_SIZE(aDataIn);
+ posliSpravu(WATCHDOG,0x00,dataSize, aDataIn);
+}
+