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@4:8ac0f7c17ac7, 2019-05-23 (annotated)

Committer:
kyxstark
Date:
Thu May 23 18:42:34 2019 +0000
Revision:
4:8ac0f7c17ac7
Parent:
3:a630a1ccf5f0
Child:
5:bb533bf81ee6

        

        
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







kyxstark

3:a630a1ccf5f0


    47

void gabarit_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







kyxstark

3:a630a1ccf5f0


    51

    uint16_t pos_av_centre[2] = {550,550};//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();





kyxstark

3:a630a1ccf5f0


    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

                }*/





kyxstark

4:8ac0f7c17ac7


   127

                SendCharCan(0x600,1);





Artiom

0:f900f4ea9dfd


   128

                etat = atome;





Artiom

0:f900f4ea9dfd


   129

            }





Artiom

0:f900f4ea9dfd


   130

            break;





Artiom

0:f900f4ea9dfd


   131







Artiom

0:f900f4ea9dfd


   132

        case atome :





Artiom

0:f900f4ea9dfd


   133

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





Artiom

0:f900f4ea9dfd


   134

            if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {





Artiom

0:f900f4ea9dfd


   135

                if(buffer_couleur_bas[FIFO_couleur_lecture] == etat_cap) {





Artiom

0:f900f4ea9dfd


   136

                    /*if(flag_sortie) {





Artiom

0:f900f4ea9dfd


   137

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





Artiom

0:f900f4ea9dfd


   138

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





Artiom

0:f900f4ea9dfd


   139

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





Artiom

0:f900f4ea9dfd


   140

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





Artiom

0:f900f4ea9dfd


   141

                        }





Artiom

0:f900f4ea9dfd


   142

                        flag_sortie = 0;





Artiom

0:f900f4ea9dfd


   143

                    }





Artiom

0:f900f4ea9dfd


   144

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





Artiom

0:f900f4ea9dfd


   145

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





Artiom

0:f900f4ea9dfd


   146

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





Artiom

0:f900f4ea9dfd


   147

                        flag_sortie = 1;





Artiom

0:f900f4ea9dfd


   148

                    }*/





Artiom

0:f900f4ea9dfd


   149

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





Artiom

0:f900f4ea9dfd


   150

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





Artiom

0:f900f4ea9dfd


   151

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





Artiom

0:f900f4ea9dfd


   152

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





Artiom

0:f900f4ea9dfd


   153

                    }





Artiom

0:f900f4ea9dfd


   154

                    etat = tmp;





Artiom

0:f900f4ea9dfd


   155

                } else if(etat_cap !=  0) {





Artiom

0:f900f4ea9dfd


   156

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





Artiom

0:f900f4ea9dfd


   157

                    etat = tmp;





Artiom

0:f900f4ea9dfd


   158

                }





Artiom

0:f900f4ea9dfd


   159

            }





Artiom

0:f900f4ea9dfd


   160

            break;





Artiom

0:f900f4ea9dfd


   161







Artiom

0:f900f4ea9dfd


   162

        case tmp :





Artiom

0:f900f4ea9dfd


   163

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





Artiom

0:f900f4ea9dfd


   164

            if(etat_cap == 0) {





Artiom

0:f900f4ea9dfd


   165

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





Artiom

0:f900f4ea9dfd


   166

                FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;





Artiom

0:f900f4ea9dfd


   167

                etat = atome;





Artiom

0:f900f4ea9dfd


   168

            }





Artiom

0:f900f4ea9dfd


   169

            break;





Artiom

0:f900f4ea9dfd


   170

    }





Artiom

0:f900f4ea9dfd


   171

}





kyxstark

1:af508f84a079


   172







Artiom

0:f900f4ea9dfd


   173

void presentoir_avant(void)





Artiom

0:f900f4ea9dfd


   174

{





kyxstark

2:33583329d6c8


   175

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





kyxstark

2:33583329d6c8


   176







kyxstark

3:a630a1ccf5f0


   177

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





kyxstark

2:33583329d6c8


   178







kyxstark

2:33583329d6c8


   179

    int speed=1;





kyxstark

2:33583329d6c8


   180







kyxstark

2:33583329d6c8


   181

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





kyxstark

2:33583329d6c8


   182

    verification();





Artiom

0:f900f4ea9dfd


   183

}





Artiom

0:f900f4ea9dfd


   184







Artiom

0:f900f4ea9dfd


   185







Artiom

0:f900f4ea9dfd


   186

void automate_ventouse_presentoir_avant(void)





Artiom

0:f900f4ea9dfd


   187

{





kyxstark

2:33583329d6c8


   188

    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;





kyxstark

2:33583329d6c8


   189

    static type_etat etat = init;





kyxstark

2:33583329d6c8


   190







kyxstark

2:33583329d6c8


   191

    switch(etat) {





kyxstark

2:33583329d6c8


   192

        case init: //attente d'initialisation





kyxstark

2:33583329d6c8


   193

            if(fpresentoir_avant)





kyxstark

2:33583329d6c8


   194

                etat=envoi_instruction;





kyxstark

2:33583329d6c8


   195

            break;





kyxstark

2:33583329d6c8


   196







kyxstark

2:33583329d6c8


   197

        case envoi_instruction://envoi instruction





kyxstark

2:33583329d6c8


   198

            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);





kyxstark

2:33583329d6c8


   199

            presentoir_avant();





kyxstark

2:33583329d6c8


   200

            SendRawId(HACHEUR_GET_PRESENTOIR_AV);





kyxstark

2:33583329d6c8


   201

            SendRawId(HACHEUR_STATUT_VENTOUSES);





kyxstark

2:33583329d6c8


   202

            etat = attente_ack_ventouse;





kyxstark

2:33583329d6c8


   203

            break;





kyxstark

2:33583329d6c8


   204







kyxstark

2:33583329d6c8


   205

        case attente_ack_ventouse:





kyxstark

2:33583329d6c8


   206

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





kyxstark

2:33583329d6c8


   207

                fpresentoir_avant=0;





kyxstark

2:33583329d6c8


   208

                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);





kyxstark

2:33583329d6c8


   209

                etat = init;





kyxstark

2:33583329d6c8


   210

            }





kyxstark

2:33583329d6c8


   211

            break;





kyxstark

2:33583329d6c8


   212







kyxstark

2:33583329d6c8


   213

    }





Artiom

0:f900f4ea9dfd


   214

}





Artiom

0:f900f4ea9dfd


   215







Artiom

0:f900f4ea9dfd


   216

void automate_ventouse_relache_avant(void)





Artiom

0:f900f4ea9dfd


   217

{





Artiom

0:f900f4ea9dfd


   218







kyxstark

2:33583329d6c8


   219

    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;





kyxstark

2:33583329d6c8


   220

    static type_etat etat = init;





kyxstark

2:33583329d6c8


   221







kyxstark

2:33583329d6c8


   222

    switch(etat) {





kyxstark

2:33583329d6c8


   223

        case init: //attente d'initialisation





kyxstark

2:33583329d6c8


   224

            if(favant_relache)





kyxstark

2:33583329d6c8


   225

                etat=envoi_instruction;





kyxstark

2:33583329d6c8


   226

            break;





kyxstark

2:33583329d6c8


   227







kyxstark

2:33583329d6c8


   228

        case envoi_instruction://envoi instruction





kyxstark

2:33583329d6c8


   229

            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);





kyxstark

2:33583329d6c8


   230

            SendRawId(HACHEUR_STATUT_VENTOUSES);





kyxstark

2:33583329d6c8


   231

            etat = attente_ack_ventouse;





kyxstark

2:33583329d6c8


   232

            break;





kyxstark

2:33583329d6c8


   233







kyxstark

2:33583329d6c8


   234

        case attente_ack_ventouse:





kyxstark

2:33583329d6c8


   235

            SendRawId(HACHEUR_RELEASE_AV);





kyxstark

2:33583329d6c8


   236

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





kyxstark

2:33583329d6c8


   237

                favant_relache=0;





kyxstark

2:33583329d6c8


   238

                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);





kyxstark

2:33583329d6c8


   239

                etat = init;





kyxstark

2:33583329d6c8


   240

            }





kyxstark

2:33583329d6c8


   241

            break;





kyxstark

2:33583329d6c8


   242







kyxstark

2:33583329d6c8


   243

    }





Artiom

0:f900f4ea9dfd


   244







Artiom

0:f900f4ea9dfd


   245

}





Artiom

0:f900f4ea9dfd


   246







Artiom

0:f900f4ea9dfd


   247

void goldenium_avant(void)





Artiom

0:f900f4ea9dfd


   248

{





Artiom

0:f900f4ea9dfd


   249







Artiom

0:f900f4ea9dfd


   250

}





Artiom

0:f900f4ea9dfd


   251







Artiom

0:f900f4ea9dfd


   252

void automate_ventouse_goldenium_avant (void)





Artiom

0:f900f4ea9dfd


   253

{





Artiom

0:f900f4ea9dfd


   254

}





Artiom

0:f900f4ea9dfd


   255







Artiom

0:f900f4ea9dfd


   256

void accelerateur_avant(void)





Artiom

0:f900f4ea9dfd


   257

{





Artiom

0:f900f4ea9dfd


   258

}





Artiom

0:f900f4ea9dfd


   259







Artiom

0:f900f4ea9dfd


   260

void automate_ventouse_accelerateur_avant(void)





Artiom

0:f900f4ea9dfd


   261

{





Artiom

0:f900f4ea9dfd


   262







kyxstark

1:af508f84a079


   263

}





Artiom

0:f900f4ea9dfd


   264

#endif