vytah okomentovany

Fork of f by Radovan Zubaľ

main.cpp

Committer:
radovan
Date:
2017-12-23
Revision:
1:d7c31658c3dd
Parent:
0:7a5957039c5f

File content as of revision 1:d7c31658c3dd:

#include "mbed.h"
#include "MMA8451Q.h"
//#include "main.h"

#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
PinName const SDA = PTE25;
PinName const SCL = PTE24;
#elif defined (TARGET_KL05Z)
PinName const SDA = PTB4;
PinName const SCL = PTB3;
#elif defined (TARGET_K20D50M)
PinName const SDA = PTB1;
PinName const SCL = PTB0;
#else
#error TARGET NOT DEFINED
#endif

#define MMA8451_I2C_ADDRESS (0x1d<<1)

#define ELEVATOR_BUTTONf_FAMILY 0xc0
#define DIODEf_FAMILY           0x10
#define LIMIT_SWITCH_FAMILY     0xe0
#define ELEVATOR_BUTTONc_FAMILY 0xb0
#define DIODEc_FAMILY           0x20

#define ELEVATOR_CABIN          0xf0
#define MOTOR                   0xf1
#define WATCHDOG_TIMER          0xfe

#define INFORMATION_DISPLAY     0x30
//crc pri mojej adrese 0xAA
#define CRC_LEDc2OFF                0xd2
#define CRC_LEDc2ON                 0x8c
#define CRC_LEDc3OFF                0x79
#define CRC_LEDc3ON                 0x27
#define CRC_LEDc1OFF                0x36
#define CRC_LEDc1ON                 0x68
#define CRC_LEDc0OFF                0x9d
#define CRC_LEDc0ON                 0xc3
#define CRC_LEDc4OFF                0x03
#define CRC_LEDc4ON                 0x5d

#define CRC_ELElock                 0xda
#define CRC_ELEunlock               0x84

#define CRC_MOTORdown               0xfc
#define CRC_MOTORstop               0x71
#define CRC_MOTORup                 0xf7

#define CRC_INFDd4                  0x9a
#define CRC_INFDd3                  0x19
#define CRC_INFDd2                  0x47
#define CRC_INFDd1                  0xa5

#define CRC_INFDn3                  0xdd
#define CRC_INFDn4                  0x5e
#define CRC_INFDn2                  0x83
#define CRC_INFDn1                  0x61
#define CRC_INFDn0                  0x5a

#define CRC_INFDu0                  0xcb
#define CRC_INFDu1                  0xf0
#define CRC_INFDu2                  0x12
#define CRC_INFDu3                  0x4c

#define CRC_LEDf3OFF                0xa7
#define CRC_LEDf3ON                 0xf9
#define CRC_LEDf4OFF                0xdd
#define CRC_LEDf4ON                 0x83
#define CRC_LEDf2OFF                0x0c
#define CRC_LEDf2ON                 0x52
#define CRC_LEDf1OFF                0xe8
#define CRC_LEDf1ON                 0xb6
#define CRC_LEDf0OFF                0x43
#define CRC_LEDf0ON                 0x1d

#define CRC_WATCHDOG                 0x4f

InterruptIn intin(PTA15);
MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
Serial pc(USBTX, USBRX,57600);
char odpovedzac[4];
char poziadavky[5]= {'N','N','N','N','N'};
char poziciaVytahu=0x04;
char bufpole[200];
int zb=0;
int kb=0;
char smerPohybu=0x03;
char aktualnePoschodie='4';
unsigned char spravaVS[10];

void posli(int pokial);
void watchdogReset();
/**
*void citaj()
*funkcia volana pri vzniknuti prerusenia pre citanie od serioveho portu
*do kruhoveho buffera "bufpole" nacita 1 znak.
*/
void citaj()
{
    while(pc.readable()==1) {
        bufpole[kb]=pc.getc();
        kb=(kb+1)%200;
    }
}
/** void prerusenieACC()
*funkcia volana pri vzniknuti prerusenia od accelerometra
*posle simulatoru spravu pre zastavenie vytahu
*/
void prerusenieACC()
{
    uint8_t datan[1];
    acc.readRegs(0x0C,datan,1);

    if((datan[0]&0x04)==0x04) {
        acc.readRegs(0x16,datan,1);
        char breakOn[6]= {0xA0,0xf,0xAA,0x01,0x01,0x08};
        //poslem spravu
        for(int ind=0; ind<6; ind++) {
            while(pc.writable()==0) {}
            pc.putc(breakOn[ind]);
        }

    }
}
/** void main()
*nastavuje akcelerometer pre vytvaranie prerusenia pre freefall
*spracovava v nekonecnej slucke spravy z kruhoveho buffera
*v pravidelnych intervaloch resetuje watchdog
*/
int main()
{
    spravaVS[0]=0xA0;//zaciatok
    spravaVS[2]=0xAA;//ja
    
    Timer timerReset;

    pc.attach(&citaj,pc.RxIrq);
    timerReset.start();
    intin.fall(&prerusenieACC);

    uint8_t pomPole[2] = {0x2A, 0x20};
    acc.writeRegs(pomPole, 2);

    pomPole[0]=0x15;
    pomPole[1]=0xB8;
    acc.writeRegs(pomPole,2);

    pomPole[0]=0x17;
    pomPole[1]=0x03;
    acc.writeRegs(pomPole,2);

    pomPole[0]=0x18;
    pomPole[1]=0x06;
    acc.writeRegs(pomPole,2);

    pomPole[0]=0x2D;
    pomPole[1]=0x04;
    acc.writeRegs(pomPole,2);

    pomPole[0]=0x2E;
    pomPole[1]=0x00;
    acc.writeRegs(pomPole,2);

    uint8_t data[1];
    acc.readRegs(0x2A,data,1);
    data[0]=data[0]| 0x01;

    pomPole[0]=0x2A;
    pomPole[1]=data[0];
    acc.writeRegs(pomPole,2);

    watchdogReset();
    int cas=timerReset.read_ms();
    while(true) {

        int pocetdat=0;
        if(zb<=kb) {
            pocetdat=kb-zb;
        } else {
            pocetdat=(199-zb)+1+kb;
        }

        if(pocetdat>4&&pocetdat>=4+bufpole[zb+3]+1) {//spracuvavam spravu
            odpovedzac[0]=bufpole[zb];
            zb=(zb+1)%200;

            odpovedzac[1]=bufpole[zb];
            zb=(zb+1)%200;

            odpovedzac[2]=bufpole[zb];
            zb=(zb+1)%200;

            odpovedzac[3]=bufpole[zb];

            int v=odpovedzac[3]+1;
            char data;
            for(int ind=0; ind<v; ind++) {
                zb=(zb+1)%200;
                if(ind==0) {
                    data= bufpole[zb];
                }
            }
            zb=(zb+1)%200;

            char rodina=(odpovedzac[2]&0xf0);
            char jedinec=(odpovedzac[2]&0x0f);
            watchdogReset();

            if(rodina==ELEVATOR_BUTTONf_FAMILY) {

                if(poziadavky[0]=='N'&&jedinec!=poziciaVytahu) {

                    poziadavky[0]=jedinec;//ulozim na ktore poschodie ma vytah ist
                    //svietim diodu na poschodi
                    spravaVS[1]=DIODEf_FAMILY|jedinec;//prijimatel
                    spravaVS[3]=1;//pocet dat
                    spravaVS[4]=0x01;//data

                    if(jedinec==0x04) {
                        spravaVS[5]=CRC_LEDf4ON;
                    } else if(jedinec==0x03) {
                        spravaVS[5]=CRC_LEDf3ON;
                    } else if(jedinec==0x02) {
                        spravaVS[5]=CRC_LEDf2ON;
                    } else if(jedinec==0x01) {
                        spravaVS[5]=CRC_LEDf1ON;
                    } else {
                        spravaVS[5]=CRC_LEDf0ON;
                    }
                    posli(6);

                    //zamykam vytah
                    spravaVS[1]=ELEVATOR_CABIN;//prijimatel
                    spravaVS[3]=1;//pocet dat
                    spravaVS[4]=0x01;//data
                    spravaVS[5]=CRC_ELElock;
                    posli(6);

                    //uved motor do pohybu
                    spravaVS[1]=MOTOR;//prijimatel
                    spravaVS[3]=5;//pocet dat
                    spravaVS[4]=0x2;//pohni vytah

                    if(poziadavky[0]>poziciaVytahu) {
                        smerPohybu=0x01;
                        spravaVS[5]=0x64;
                        spravaVS[6]=0x00;
                        spravaVS[7]=0x00;
                        spravaVS[8]=0x00;
                        spravaVS[9]=CRC_MOTORup;
                    } else {
                        smerPohybu=0x02;
                        spravaVS[5]=0x9C;
                        spravaVS[6]=0xFF;
                        spravaVS[7]=0xFF;
                        spravaVS[8]=0xFF;
                        spravaVS[9]=CRC_MOTORdown;
                    }
                    posli(10);

                    //zobrazenie pohybu
                    spravaVS[1]=INFORMATION_DISPLAY;//prijimatel
                    spravaVS[3]=2;//pocet dat
                    spravaVS[4]=smerPohybu;//data
                    spravaVS[5]=aktualnePoschodie;//data

                    if(smerPohybu==0x02) { //smer pohybu dole
                        if(aktualnePoschodie=='4') {
                            spravaVS[6]=CRC_INFDd4;
                        } else if(aktualnePoschodie=='3') {
                            spravaVS[6]=CRC_INFDd3;
                        } else if(aktualnePoschodie=='2') {
                            spravaVS[6]=CRC_INFDd2;
                        } else {
                            spravaVS[6]=CRC_INFDd1;
                        }
                    } else {//smer pohybu hore
                        if(aktualnePoschodie=='3') {
                            spravaVS[6]=CRC_INFDu3;
                        } else if(aktualnePoschodie=='2') {
                            spravaVS[6]=CRC_INFDu2;
                        } else if(aktualnePoschodie=='1') {
                            spravaVS[6]=CRC_INFDu1;
                        } else {
                            spravaVS[6]=CRC_INFDu0;
                        }
                    }
                    posli(7);

                }//if poziadavky[0]=='N'&&poziadavky[0]!=poziciaVytahu

            } else if(rodina==ELEVATOR_BUTTONc_FAMILY) {

                //ak bolo stlacene tlacidlo na inom poschodi ako akualnom a tato poziadavka este nie je v poli
                if(poziciaVytahu!=jedinec) {
                    char pom=0x00;
                    char pohni=0x00;
                    for(int i=0; i<5; i++) {
                        if(poziadavky[i]==jedinec) {
                            pom=0x01;
                        }
                        if(poziadavky[i]=='N'&&pom==0x00) {
                            poziadavky[i]=jedinec;
                            pom=0x02;
                            if(i==0) {
                                pohni=0x03;
                            }
                            break;
                        }
                    }
                    if(pom==0x02) {
                        //zasvieti diodu vnutri vytahu
                        spravaVS[1]=DIODEc_FAMILY|jedinec;//prijimatel
                        spravaVS[3]=1;//pocet dat
                        spravaVS[4]=0x01;//data

                        if(jedinec==0x04) {
                            spravaVS[5]=CRC_LEDc4ON;
                        } else if(jedinec==0x03) {
                            spravaVS[5]=CRC_LEDc3ON;
                        } else if(jedinec==0x02) {
                            spravaVS[5]=CRC_LEDc2ON;
                        } else if(jedinec==0x01) {
                            spravaVS[5]=CRC_LEDc1ON;
                        } else {
                            spravaVS[5]=CRC_LEDc0ON;
                        }
                        posli(6);

                    }

                    if(pohni==0x03) {
                        //uzamknutie vytahu
                        spravaVS[1]=ELEVATOR_CABIN;//prijimatel
                        spravaVS[3]=1;//pocet dat
                        spravaVS[4]=0x01;//data
                        spravaVS[5]=CRC_ELElock;
                        posli(6);

                        //uved motor do pohybu
                        spravaVS[1]=MOTOR;//prijimatel
                        spravaVS[3]=5;//pocet dat
                        spravaVS[4]=0x2;//data

                        if(poziadavky[0]>poziciaVytahu) {
                            smerPohybu=0x01;
                            spravaVS[5]=0x64;
                            spravaVS[6]=0x00;
                            spravaVS[7]=0x00;
                            spravaVS[8]=0x00;
                            spravaVS[9]=CRC_MOTORup;

                        } else {
                            smerPohybu=0x02;
                            spravaVS[5]=0x9C;
                            spravaVS[6]=0xFF;
                            spravaVS[7]=0xFF;
                            spravaVS[8]=0xFF;
                            spravaVS[9]=CRC_MOTORdown;
                        }
                        posli(10);

                        //zobrazenie pohybu
                        spravaVS[1]=INFORMATION_DISPLAY;//prijimatel
                        spravaVS[3]=2;//pocet dat
                        spravaVS[4]=smerPohybu;//data
                        spravaVS[5]=aktualnePoschodie;//data

                        if(smerPohybu==0x02) { //smer pohybu dole
                            if(aktualnePoschodie=='4') {
                                spravaVS[6]=CRC_INFDd4;
                            } else if(aktualnePoschodie=='3') {
                                spravaVS[6]=CRC_INFDd3;
                            } else if(aktualnePoschodie=='2') {
                                spravaVS[6]=CRC_INFDd2;
                            } else {
                                spravaVS[6]=CRC_INFDd1;
                            }
                        } else {//smer pohybu hore
                            if(aktualnePoschodie=='3') {
                                spravaVS[6]=CRC_INFDu3;
                            } else if(aktualnePoschodie=='2') {
                                spravaVS[6]=CRC_INFDu2;
                            } else if(aktualnePoschodie=='1') {
                                spravaVS[6]=CRC_INFDu1;
                            } else {
                                spravaVS[6]=CRC_INFDu0;
                            }
                        }
                        posli(7);
                    }
                }

            } else if(rodina==LIMIT_SWITCH_FAMILY) {

                if(data==0x01&&poziadavky[0]==jedinec) { //dalej od switchu
                    //zastavenie vytahu
                    spravaVS[1]=MOTOR;//prijimatel
                    spravaVS[3]=1;//pocet dat
                    spravaVS[4]=0x1;//data
                    spravaVS[5]=CRC_MOTORstop;
                    posli(6);

                } else if(data==0x02&&poziadavky[0]==jedinec) { //blizko pri switchi spravne poschodie
                    //posunutie poziadaviek
                    for(int i=0; i<4; i++) {
                        poziadavky[i]=poziadavky[i+1];
                    }
                    poziadavky[4]='N';
                    poziciaVytahu=jedinec;
                    //odomknutie vytahu
                    spravaVS[1]=ELEVATOR_CABIN;//prijimatel
                    spravaVS[3]=1;//pocet dat
                    spravaVS[4]=0x00;//data
                    spravaVS[5]=CRC_ELEunlock;
                    posli(6);

                    //zhasnutie diody na poschodi
                    spravaVS[1]=DIODEf_FAMILY|jedinec;//prijimatel
                    spravaVS[3]=1;//pocet dat
                    spravaVS[4]=0x00;//data

                    if(jedinec==0x04) {
                        spravaVS[5]=CRC_LEDf4OFF;
                    } else if(jedinec==0x03) {
                        spravaVS[5]=CRC_LEDf3OFF;
                    } else if(jedinec==0x02) {
                        spravaVS[5]=CRC_LEDf2OFF;
                    } else if(jedinec==0x01) {
                        spravaVS[5]=CRC_LEDf1OFF;
                    } else {
                        spravaVS[5]=CRC_LEDf0OFF;
                    }

                    posli(6);

                    //zhasne diodu vnutri vytahu
                    spravaVS[1]=DIODEc_FAMILY|jedinec;//prijimatel
                    spravaVS[3]=1;//pocet dat
                    spravaVS[4]=0x00;//data

                    if(jedinec==0x04) {
                        spravaVS[5]=CRC_LEDc4OFF;
                        aktualnePoschodie='4';
                    } else if(jedinec==0x03) {
                        spravaVS[5]=CRC_LEDc3OFF;
                        aktualnePoschodie='3';
                    } else if(jedinec==0x02) {
                        spravaVS[5]=CRC_LEDc2OFF;
                        aktualnePoschodie='2';
                    } else if(jedinec==0x01) {
                        spravaVS[5]=CRC_LEDc1OFF;
                        aktualnePoschodie='1';
                    } else {
                        spravaVS[5]=CRC_LEDc0OFF;
                        aktualnePoschodie='P';
                    }

                    posli(6);

                    smerPohybu=0x03;
                    //zobrazenie pohybu
                    spravaVS[1]=INFORMATION_DISPLAY;//prijimatel
                    spravaVS[3]=2;//pocet dat
                    spravaVS[4]=smerPohybu;//data
                    spravaVS[5]=aktualnePoschodie;//data

                    if(jedinec==0x04) {
                        spravaVS[6]=CRC_INFDn4;
                    } else if(jedinec==0x03) {
                        spravaVS[6]=CRC_INFDn3;
                    } else if(jedinec==0x02) {
                        spravaVS[6]=CRC_INFDn2;
                    } else if(jedinec==0x01) {
                        spravaVS[6]=CRC_INFDn1;
                    } else {
                        spravaVS[6]=CRC_INFDn0;
                    }
                    posli(7);
                    watchdogReset();

                    //cakam vzdy po zastaveni vytahu
                    int zac;
                    Timer cakaj;
                    for(int t=0; t<=10; t++) {
                        cakaj.start();
                        zac=cakaj.read_ms();
                        while(true) {
                            if((cakaj.read_ms()-zac)>=500) {
                                watchdogReset();
                                break;
                            }
                        }

                        cakaj.stop();
                        cakaj.reset();
                    }

                    if(poziadavky[0]!='N') { //ak este mam niekde ist tak spustim vytah

                        //uzamknutie vytahu
                        spravaVS[1]=ELEVATOR_CABIN;//prijimatel
                        spravaVS[3]=1;//pocet dat
                        spravaVS[4]=0x01;//data
                        spravaVS[5]=CRC_ELElock;
                        posli(6);

                        //uved motor do pohybu
                        spravaVS[1]=MOTOR;//prijimatel
                        spravaVS[3]=5;//pocet dat
                        spravaVS[4]=0x2;//data

                        if(poziadavky[0]>poziciaVytahu) {
                            smerPohybu=0x01;
                            spravaVS[5]=0x64;
                            spravaVS[6]=0x00;
                            spravaVS[7]=0x00;
                            spravaVS[8]=0x00;
                            spravaVS[9]=CRC_MOTORup;

                        } else {
                            smerPohybu=0x02;
                            spravaVS[5]=0x9C;
                            spravaVS[6]=0xFF;
                            spravaVS[7]=0xFF;
                            spravaVS[8]=0xFF;
                            spravaVS[9]=CRC_MOTORdown;
                        }
                        posli(10);

                        //zobrazenie pohybu
                        spravaVS[1]=INFORMATION_DISPLAY;//prijimatel
                        spravaVS[3]=2;//pocet dat
                        spravaVS[4]=smerPohybu;//data
                        spravaVS[5]=aktualnePoschodie;//data

                        if(smerPohybu==0x02) { //smer pohybu dole
                            if(aktualnePoschodie=='4') {
                                spravaVS[6]=CRC_INFDd4;
                            } else if(aktualnePoschodie=='3') {
                                spravaVS[6]=CRC_INFDd3;
                            } else if(aktualnePoschodie=='2') {
                                spravaVS[6]=CRC_INFDd2;
                            } else {
                                spravaVS[6]=CRC_INFDd1;
                            }
                        } else {//smer pohybu hore
                            if(aktualnePoschodie=='3') {
                                spravaVS[6]=CRC_INFDu3;
                            } else if(aktualnePoschodie=='2') {
                                spravaVS[6]=CRC_INFDu2;
                            } else if(aktualnePoschodie=='1') {
                                spravaVS[6]=CRC_INFDu1;
                            } else {
                                spravaVS[6]=CRC_INFDu0;
                            }
                        }
                        posli(7);
                    }

                } else if(data==0x02&&poziadavky[0]!=jedinec) {//blizko ale nezastavujem

                    if(jedinec==0x04) {
                        aktualnePoschodie='4';
                    } else if(jedinec==0x03) {
                        aktualnePoschodie='3';
                    } else if(jedinec==0x02) {
                        aktualnePoschodie='2';
                    } else if(jedinec==0x01) {
                        aktualnePoschodie='1';
                    } else {
                        aktualnePoschodie='P';
                    }

                    //zobrazenie pohybu
                    spravaVS[1]=INFORMATION_DISPLAY;//prijimatel
                    spravaVS[3]=2;//pocet dat
                    spravaVS[4]=smerPohybu;//data
                    spravaVS[5]=aktualnePoschodie;//data

                    if(smerPohybu==0x02) { //smer pohybu dole
                        if(aktualnePoschodie=='4') {
                            spravaVS[6]=CRC_INFDd4;
                        } else if(aktualnePoschodie=='3') {
                            spravaVS[6]=CRC_INFDd3;
                        } else if(aktualnePoschodie=='2') {
                            spravaVS[6]=CRC_INFDd2;
                        } else {
                            spravaVS[6]=CRC_INFDd1;
                        }
                    } else {//smer pohybu hore
                        if(aktualnePoschodie=='3') {
                            spravaVS[6]=CRC_INFDu3;
                        } else if(aktualnePoschodie=='2') {
                            spravaVS[6]=CRC_INFDu2;
                        } else if(aktualnePoschodie=='1') {
                            spravaVS[6]=CRC_INFDu1;
                        } else {
                            spravaVS[6]=CRC_INFDu0;
                        }
                    }
                    posli(7);
                }
            }//LIMIT_SWITCH_FAMILY
        }//sprava dostatocne velka
        if((timerReset.read_ms()-cas)>=600) {
            watchdogReset();
            cas=timerReset.read_ms();
        }
    }//while true
}//main
/**
*void posli()
*posle spravu z pola spravaVS pokial nedostane potvrdzujucu odpoved do 200ms posiela spravu znova
*@param <pokial> pocet znakov na odoslanie
*/
void posli(int pokial)
{
    Timer timer;
    int potvrdenie=0;
    timer.start();
    while(potvrdenie==0) {
        //poslem spravu
        for(int ind=0; ind<pokial; ind++) {
            while(pc.writable()==0) {}
            pc.putc(spravaVS[ind]);
        }
        int pocdat=0;
        int begin=timer.read_ms();
        while((timer.read_ms()-begin)<200) {
            if(zb<=kb) {
                pocdat=kb-zb;
            } else {
                pocdat=200-zb+kb;
            }
            char prerus=0x00;
            for(int t=0; t<=pocdat-5; t++) {
                if(bufpole[(zb+t)%200]==0xA0&&bufpole[(zb+t+3)%200]==0x00) {
                    int k=(zb+t-1)%200;//index dat od ktoreho treba zacat presuvat
                    if(t==0) {
                        zb=(zb+5)%200;
                        prerus=0x01;
                        break;
                    }
                    while(true) {
                        bufpole[(k+5)%200]=bufpole[k];
                        if(k==zb) {
                            zb=(zb+5)%200;
                            break;
                        }
                        k--;
                        if(k<0) {
                            k=199;
                        }
                    }//while
                    prerus=0x01;
                    break;
                } //if
            }//for
            if(prerus==0x01) {
                potvrdenie=1;
                break;
            }
        }//while timer
    }//while potvrdenie==0
    timer.stop();
    timer.reset();

}//koniec posli
/*
*void watchdogReset()
*psole spravu pre resetovanie watchdogu caka na potvrdenie
*/
void watchdogReset()
{
    spravaVS[1]=WATCHDOG_TIMER;//prijimatel
    spravaVS[3]=1;//pocet dat
    spravaVS[4]=0x05;//data
    spravaVS[5]=CRC_WATCHDOG;//data
    posli(6);
}