CRAC Team / actions_Gr

librairie actions gros robot carte esclave

Dependents:   carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019

actions_Gr.cpp@2:33583329d6c8, 2019-05-23 (annotated)

Committer:
kyxstark
Date:
Thu May 23 14:15:46 2019 +0000
Revision:
2:33583329d6c8
Parent:
1:af508f84a079
Child:
3:a630a1ccf5f0

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


Artiom

0:f900f4ea9dfd


     1

#include "actions_Gr.h"





Artiom

0:f900f4ea9dfd


     2







Artiom

0:f900f4ea9dfd


     3







Artiom

0:f900f4ea9dfd


     4

#ifdef ROBOT_BIG





Artiom

0:f900f4ea9dfd


     5

#define MASK_PRESENTOIR_AV 0x07





Artiom

0:f900f4ea9dfd


     6

#define MASK_PRESENTOIR_AR 0x70





Artiom

0:f900f4ea9dfd


     7







Artiom

0:f900f4ea9dfd


     8

#define MASK_GOLDENIUM_AV 0x02





Artiom

0:f900f4ea9dfd


     9

#define MASK_GOLDENIUM_AR 0x20





Artiom

0:f900f4ea9dfd


    10







Artiom

0:f900f4ea9dfd


    11

#define MASK_SOL_AV 0x08





Artiom

0:f900f4ea9dfd


    12

#define MASK_SOL_AR 0x80





Artiom

0:f900f4ea9dfd


    13







Artiom

0:f900f4ea9dfd


    14

#define MASK_AV_DROIT 0x01





Artiom

0:f900f4ea9dfd


    15

#define MASK_AR_DROIT 0x10





Artiom

0:f900f4ea9dfd


    16







Artiom

0:f900f4ea9dfd


    17

#define MASK_AV_DROIT_GAUCHE 0x05





Artiom

0:f900f4ea9dfd


    18

#define MASK_AR_DROIT_GAUCHE 0x50





Artiom

0:f900f4ea9dfd


    19







Artiom

0:f900f4ea9dfd


    20







Artiom

0:f900f4ea9dfd


    21

char status_pompe=0;





Artiom

0:f900f4ea9dfd


    22

bool flag_ascenseur = 0;





Artiom

0:f900f4ea9dfd


    23







Artiom

0:f900f4ea9dfd


    24

char fpresentoir_avant=0, fpresentoir_arriere=0;





Artiom

0:f900f4ea9dfd


    25

char fgoldenium_avant=0, fgoldenium_arriere=0;





Artiom

0:f900f4ea9dfd


    26

char fsol_avant=0,fsol_arriere=0;





Artiom

0:f900f4ea9dfd


    27

char fsol_avant_relache=0,fsol_arriere_relache=0;





Artiom

0:f900f4ea9dfd


    28

char fbalance_avant=0,fbalance_arriere=0;





Artiom

0:f900f4ea9dfd


    29

char favant_relache=0,farriere_relache=0;





Artiom

0:f900f4ea9dfd


    30

char faccelerateur_avant=0,faccelerateur_arriere=0;





Artiom

0:f900f4ea9dfd


    31







Artiom

0:f900f4ea9dfd


    32







Artiom

0:f900f4ea9dfd


    33

DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1





Artiom

0:f900f4ea9dfd


    34

DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2





Artiom

0:f900f4ea9dfd


    35

DigitalIn presence_droit(PB_5);





Artiom

0:f900f4ea9dfd


    36

DigitalIn presence_gauche(PB_4);





Artiom

0:f900f4ea9dfd


    37







Artiom

0:f900f4ea9dfd


    38







Artiom

0:f900f4ea9dfd


    39

char buffer_couleur_bas[SIZE_FIFO];





Artiom

0:f900f4ea9dfd


    40

unsigned char FIFO_couleur_ecriture=0;





Artiom

0:f900f4ea9dfd


    41

signed char FIFO_couleur_lecture=0;





Artiom

0:f900f4ea9dfd


    42







Artiom

0:f900f4ea9dfd


    43







Artiom

0:f900f4ea9dfd


    44







Artiom

0:f900f4ea9dfd


    45







Artiom

0:f900f4ea9dfd


    46







Artiom

0:f900f4ea9dfd


    47

void gabarit_gros_robot(void)





Artiom

0:f900f4ea9dfd


    48

{





Artiom

0:f900f4ea9dfd


    49

    uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};





Artiom

0:f900f4ea9dfd


    50







Artiom

0:f900f4ea9dfd


    51

    uint16_t pos_av_centre[2] = {550,600};//470,350





Artiom

0:f900f4ea9dfd


    52







Artiom

0:f900f4ea9dfd


    53







Artiom

0:f900f4ea9dfd


    54

    int speed=100;





Artiom

0:f900f4ea9dfd


    55







Artiom

0:f900f4ea9dfd


    56

    deverouillage_torque();





Artiom

0:f900f4ea9dfd


    57

    //positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);





Artiom

0:f900f4ea9dfd


    58







Artiom

0:f900f4ea9dfd


    59

}





Artiom

0:f900f4ea9dfd


    60







Artiom

0:f900f4ea9dfd


    61







Artiom

0:f900f4ea9dfd


    62

void fifo_couleur(void)





Artiom

0:f900f4ea9dfd


    63

{





Artiom

0:f900f4ea9dfd


    64







Artiom

0:f900f4ea9dfd


    65

    typedef enum {n_atome, atome, tmp} type_etat ;





Artiom

0:f900f4ea9dfd


    66

    static type_etat etat = n_atome;





Artiom

0:f900f4ea9dfd


    67







Artiom

0:f900f4ea9dfd


    68

    int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;





Artiom

0:f900f4ea9dfd


    69

    switch(etat) {





Artiom

0:f900f4ea9dfd


    70

        case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO





Artiom

0:f900f4ea9dfd


    71

            if(etat_cap) {





Artiom

0:f900f4ea9dfd


    72

                buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert





Artiom

0:f900f4ea9dfd


    73

                flag_ascenseur = 1;





Artiom

0:f900f4ea9dfd


    74

                if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {





Artiom

0:f900f4ea9dfd


    75

                    if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {





Artiom

0:f900f4ea9dfd


    76

                        positionControl(doigt,384,10,BLED_ON,2);   //position herkulex stockage bleu





Artiom

0:f900f4ea9dfd


    77

                    } else {





Artiom

0:f900f4ea9dfd


    78

                        positionControl(doigt,640,10,GLED_ON,2);   //position herkulex stockage rouge/vert





Artiom

0:f900f4ea9dfd


    79

                    }





Artiom

0:f900f4ea9dfd


    80

                }





Artiom

0:f900f4ea9dfd


    81

                //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);





Artiom

0:f900f4ea9dfd


    82

                FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;





Artiom

0:f900f4ea9dfd


    83

                etat = tmp;





Artiom

0:f900f4ea9dfd


    84

            }





Artiom

0:f900f4ea9dfd


    85

            break;





Artiom

0:f900f4ea9dfd


    86







Artiom

0:f900f4ea9dfd


    87

        case atome : //on stocke dans la fifo la couleur de l'atome





Artiom

0:f900f4ea9dfd


    88

            buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert





Artiom

0:f900f4ea9dfd


    89

            flag_ascenseur = 1;





Artiom

0:f900f4ea9dfd


    90

            /*if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {





Artiom

0:f900f4ea9dfd


    91

                    positionControl(doigt,384,1,BLED_ON,2);   //position herkulex stockage bleu





Artiom

0:f900f4ea9dfd


    92

                } else {





Artiom

0:f900f4ea9dfd


    93

                    positionControl(doigt,640,1,GLED_ON,2);   //position herkulex stockage rouge/vert





Artiom

0:f900f4ea9dfd


    94

                }*/





Artiom

0:f900f4ea9dfd


    95

            //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);





Artiom

0:f900f4ea9dfd


    96

            FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;





Artiom

0:f900f4ea9dfd


    97

            etat = tmp;





Artiom

0:f900f4ea9dfd


    98

            break;





Artiom

0:f900f4ea9dfd


    99







Artiom

0:f900f4ea9dfd


   100

        case tmp : //on attend que l'atome traité soit totalement passé





Artiom

0:f900f4ea9dfd


   101

            if(!etat_cap) {





Artiom

0:f900f4ea9dfd


   102

                etat = n_atome;





Artiom

0:f900f4ea9dfd


   103

            }





Artiom

0:f900f4ea9dfd


   104

            break;





Artiom

0:f900f4ea9dfd


   105







Artiom

0:f900f4ea9dfd


   106

    }





Artiom

0:f900f4ea9dfd


   107







Artiom

0:f900f4ea9dfd


   108

}





Artiom

0:f900f4ea9dfd


   109







Artiom

0:f900f4ea9dfd


   110







Artiom

0:f900f4ea9dfd


   111

void ascenseur(void)





Artiom

0:f900f4ea9dfd


   112

{





Artiom

0:f900f4ea9dfd


   113

    typedef enum {init, atome, tmp} type_etat;





Artiom

0:f900f4ea9dfd


   114

    static type_etat etat = init;





Artiom

0:f900f4ea9dfd


   115







Artiom

0:f900f4ea9dfd


   116

    int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;





Artiom

0:f900f4ea9dfd


   117

    static int flag_sortie = 0;





Artiom

0:f900f4ea9dfd


   118

    switch(etat) {





Artiom

0:f900f4ea9dfd


   119

        case init :





Artiom

0:f900f4ea9dfd


   120

            //on attend le premier atome et place le herkulex en fonction





Artiom

0:f900f4ea9dfd


   121

            if(flag_ascenseur) {





Artiom

0:f900f4ea9dfd


   122

                /*if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {





Artiom

0:f900f4ea9dfd


   123

                    positionControl(doigt,384,1,BLED_ON,2);   //position herkulex stockage bleu





Artiom

0:f900f4ea9dfd


   124

                } else {





Artiom

0:f900f4ea9dfd


   125

                    positionControl(doigt,640,1,GLED_ON,2);   //position herkulex stockage rouge/vert





Artiom

0:f900f4ea9dfd


   126

                }*/





Artiom

0:f900f4ea9dfd


   127

                etat = atome;





Artiom

0:f900f4ea9dfd


   128

            }





Artiom

0:f900f4ea9dfd


   129

            break;





Artiom

0:f900f4ea9dfd


   130







Artiom

0:f900f4ea9dfd


   131

        case atome :





Artiom

0:f900f4ea9dfd


   132

            //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO





Artiom

0:f900f4ea9dfd


   133

            if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {





Artiom

0:f900f4ea9dfd


   134

                if(buffer_couleur_bas[FIFO_couleur_lecture] == etat_cap) {





Artiom

0:f900f4ea9dfd


   135

                    /*if(flag_sortie) {





Artiom

0:f900f4ea9dfd


   136

                        if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu





Artiom

0:f900f4ea9dfd


   137

                            positionControl(doigt,384,10,BLED_ON,2);





Artiom

0:f900f4ea9dfd


   138

                        } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert





Artiom

0:f900f4ea9dfd


   139

                            positionControl(doigt,640,10,GLED_ON,2);





Artiom

0:f900f4ea9dfd


   140

                        }





Artiom

0:f900f4ea9dfd


   141

                        flag_sortie = 0;





Artiom

0:f900f4ea9dfd


   142

                    }





Artiom

0:f900f4ea9dfd


   143

                    pc.printf("--fifo lecture %d\n", buffer_couleur_bas[FIFO_couleur_lecture]);





Artiom

0:f900f4ea9dfd


   144

                    //on change la position du herkulex si le prochain atome est différent





Artiom

0:f900f4ea9dfd


   145

                    if(buffer_couleur_bas[FIFO_couleur_lecture] != buffer_couleur_bas[FIFO_couleur_lecture+1] ) {





Artiom

0:f900f4ea9dfd


   146

                        flag_sortie = 1;





Artiom

0:f900f4ea9dfd


   147

                    }*/





Artiom

0:f900f4ea9dfd


   148

                 if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu





Artiom

0:f900f4ea9dfd


   149

                        positionControl(doigt,384,10,BLED_ON,2);





Artiom

0:f900f4ea9dfd


   150

                    } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert





Artiom

0:f900f4ea9dfd


   151

                        positionControl(doigt,640,10,GLED_ON,2);





Artiom

0:f900f4ea9dfd


   152

                    }





Artiom

0:f900f4ea9dfd


   153

                    etat = tmp;





Artiom

0:f900f4ea9dfd


   154

                } else if(etat_cap !=  0) {





Artiom

0:f900f4ea9dfd


   155

                    //pc.printf("--fifo lecture %d --- vu : %d\n", buffer_couleur_bas[FIFO_couleur_lecture], etat_cap);





Artiom

0:f900f4ea9dfd


   156

                    etat = tmp;





Artiom

0:f900f4ea9dfd


   157

                }





Artiom

0:f900f4ea9dfd


   158

            }





Artiom

0:f900f4ea9dfd


   159

            break;





Artiom

0:f900f4ea9dfd


   160







Artiom

0:f900f4ea9dfd


   161

        case tmp :





Artiom

0:f900f4ea9dfd


   162

            //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture





Artiom

0:f900f4ea9dfd


   163

            if(etat_cap == 0) {





Artiom

0:f900f4ea9dfd


   164

                //pc.printf("\t\tfifo + 1\n");





Artiom

0:f900f4ea9dfd


   165

                FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;





Artiom

0:f900f4ea9dfd


   166

                etat = atome;





Artiom

0:f900f4ea9dfd


   167

            }





Artiom

0:f900f4ea9dfd


   168

            break;





Artiom

0:f900f4ea9dfd


   169

    }





Artiom

0:f900f4ea9dfd


   170

}





kyxstark

1:af508f84a079


   171







Artiom

0:f900f4ea9dfd


   172

void presentoir_avant(void)





Artiom

0:f900f4ea9dfd


   173

{





kyxstark

2:33583329d6c8


   174

    uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};





kyxstark

2:33583329d6c8


   175







kyxstark

2:33583329d6c8


   176

    uint16_t pos_av_centre[2] = {470,512};





kyxstark

2:33583329d6c8


   177







kyxstark

2:33583329d6c8


   178

    int speed=1;





kyxstark

2:33583329d6c8


   179







kyxstark

2:33583329d6c8


   180

    positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);





kyxstark

2:33583329d6c8


   181

    verification();





Artiom

0:f900f4ea9dfd


   182

}





Artiom

0:f900f4ea9dfd


   183







Artiom

0:f900f4ea9dfd


   184







Artiom

0:f900f4ea9dfd


   185

void automate_ventouse_presentoir_avant(void)





Artiom

0:f900f4ea9dfd


   186

{





kyxstark

2:33583329d6c8


   187

    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;





kyxstark

2:33583329d6c8


   188

    static type_etat etat = init;





kyxstark

2:33583329d6c8


   189







kyxstark

2:33583329d6c8


   190

    switch(etat) {





kyxstark

2:33583329d6c8


   191

        case init: //attente d'initialisation





kyxstark

2:33583329d6c8


   192

            if(fpresentoir_avant)





kyxstark

2:33583329d6c8


   193

                etat=envoi_instruction;





kyxstark

2:33583329d6c8


   194

            break;





kyxstark

2:33583329d6c8


   195







kyxstark

2:33583329d6c8


   196

        case envoi_instruction://envoi instruction





kyxstark

2:33583329d6c8


   197

            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);





kyxstark

2:33583329d6c8


   198

            presentoir_avant();





kyxstark

2:33583329d6c8


   199

            SendRawId(HACHEUR_GET_PRESENTOIR_AV);





kyxstark

2:33583329d6c8


   200

            SendRawId(HACHEUR_STATUT_VENTOUSES);





kyxstark

2:33583329d6c8


   201

            etat = attente_ack_ventouse;





kyxstark

2:33583329d6c8


   202

            break;





kyxstark

2:33583329d6c8


   203







kyxstark

2:33583329d6c8


   204

        case attente_ack_ventouse:





kyxstark

2:33583329d6c8


   205

            if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {





kyxstark

2:33583329d6c8


   206

                fpresentoir_avant=0;





kyxstark

2:33583329d6c8


   207

                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);





kyxstark

2:33583329d6c8


   208

                etat = init;





kyxstark

2:33583329d6c8


   209

            }





kyxstark

2:33583329d6c8


   210

            break;





kyxstark

2:33583329d6c8


   211







kyxstark

2:33583329d6c8


   212

    }





Artiom

0:f900f4ea9dfd


   213

}





Artiom

0:f900f4ea9dfd


   214







Artiom

0:f900f4ea9dfd


   215

void automate_ventouse_relache_avant(void)





Artiom

0:f900f4ea9dfd


   216

{





Artiom

0:f900f4ea9dfd


   217







kyxstark

2:33583329d6c8


   218

    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;





kyxstark

2:33583329d6c8


   219

    static type_etat etat = init;





kyxstark

2:33583329d6c8


   220







kyxstark

2:33583329d6c8


   221

    switch(etat) {





kyxstark

2:33583329d6c8


   222

        case init: //attente d'initialisation





kyxstark

2:33583329d6c8


   223

            if(favant_relache)





kyxstark

2:33583329d6c8


   224

                etat=envoi_instruction;





kyxstark

2:33583329d6c8


   225

            break;





kyxstark

2:33583329d6c8


   226







kyxstark

2:33583329d6c8


   227

        case envoi_instruction://envoi instruction





kyxstark

2:33583329d6c8


   228

            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);





kyxstark

2:33583329d6c8


   229

            SendRawId(HACHEUR_STATUT_VENTOUSES);





kyxstark

2:33583329d6c8


   230

            etat = attente_ack_ventouse;





kyxstark

2:33583329d6c8


   231

            break;





kyxstark

2:33583329d6c8


   232







kyxstark

2:33583329d6c8


   233

        case attente_ack_ventouse:





kyxstark

2:33583329d6c8


   234

            SendRawId(HACHEUR_RELEASE_AV);





kyxstark

2:33583329d6c8


   235

            if((status_pompe&MASK_PRESENTOIR_AV)== 0) {





kyxstark

2:33583329d6c8


   236

                favant_relache=0;





kyxstark

2:33583329d6c8


   237

                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);





kyxstark

2:33583329d6c8


   238

                etat = init;





kyxstark

2:33583329d6c8


   239

            }





kyxstark

2:33583329d6c8


   240

            break;





kyxstark

2:33583329d6c8


   241







kyxstark

2:33583329d6c8


   242

    }





Artiom

0:f900f4ea9dfd


   243







Artiom

0:f900f4ea9dfd


   244

}





Artiom

0:f900f4ea9dfd


   245







Artiom

0:f900f4ea9dfd


   246

void goldenium_avant(void)





Artiom

0:f900f4ea9dfd


   247

{





Artiom

0:f900f4ea9dfd


   248







Artiom

0:f900f4ea9dfd


   249

}





Artiom

0:f900f4ea9dfd


   250







Artiom

0:f900f4ea9dfd


   251

void automate_ventouse_goldenium_avant (void)





Artiom

0:f900f4ea9dfd


   252

{





Artiom

0:f900f4ea9dfd


   253

}





Artiom

0:f900f4ea9dfd


   254







Artiom

0:f900f4ea9dfd


   255

void accelerateur_avant(void)





Artiom

0:f900f4ea9dfd


   256

{





Artiom

0:f900f4ea9dfd


   257

}





Artiom

0:f900f4ea9dfd


   258







Artiom

0:f900f4ea9dfd


   259

void automate_ventouse_accelerateur_avant(void)





Artiom

0:f900f4ea9dfd


   260

{





Artiom

0:f900f4ea9dfd


   261







kyxstark

1:af508f84a079


   262

}





Artiom

0:f900f4ea9dfd


   263

#endif