CRAC Team
/
AX12_PR
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
test_carteAToutFaire_PR
by 
CRAC Team
Actionneur.cpp
- Committer:
- EdouardGE1
- Date:
- 2017-05-20
- Revision:
- 3:0175a071dd65
- Parent:
- 2:9d280856a536
- Child:
- 4:5cf096b1c65d
File content as of revision 3:0175a071dd65:
#include "all_includes.h"
extern CAN can;
/* DECLARATION VARIABLES */
extern unsigned char FlagAx12;
extern "C" void mbed_reset();//Pour pouvoir reset la carte
extern DigitalOut led2;
extern Serial pc;
extern Timer t;
extern void GetPositionAx12(void);
AX12 *BaseBrasCentralPR, *CoudeBrasCentralPR, *PinceDBrasCentralPR, *PinceGBrasCentralPR, *DoigtBrasCentralPR, *BrasCentralPRAx12, *TabBrasCentralPR,
*CrocBrasGauchePR, *EpauleBrasGauchePR, *CoudeBrasGauchePR, *BrasGauchePRAx12, *TabBrasGauchePR,
*CrocBrasDroitPR, *EpauleBrasDroitPR, *CoudeBrasDroitPR, *BrasDroitPRAx12, *TabBrasDroitPR;
/* ANGLE */
/* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02
30° = 0x64, 0x00 | 130°= 0xB1, 0x01
40° = 0x85, 0x00 | 140°= 0xD2, 0x01
50° = 0xA6, 0x00 | 150°= 0xF4, 0x01
60° = 0xC8, 0x00 | 160°= 0x15, 0x02
70° = 0xE9, 0x00 | 170°= 0x36, 0x02
80° = 0x0A, 0x01 | 180°= 0x58, 0x02
90° = 0x2C, 0x01 | 190°= 0x79, 0x02
100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
/* NUMERO AX12 */
/* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
1 = 0x01 | 10 = 0x0A
2 = 0x02 | 11 = 0x0B
3 = 0x03 | 12 = 0x0C
4 = 0x04 | 13 = 0x0D
5 = 0x05 | 14 = 0x0E
6 = 0x06 | 15 = 0x0F
7 = 0x07 | 16 = 0x10
8 = 0x08 | 17 = 0x11 */
/* MAIN */
void initialisation_AX12(void)
{
short vitesse=700;
BaseBrasCentralPR = new AX12(p9, p10, 5, 1000000);
CoudeBrasCentralPR = new AX12(p9, p10, 6, 1000000);
PinceDBrasCentralPR = new AX12(p9, p10, 8, 1000000);
PinceGBrasCentralPR = new AX12(p9, p10, 7, 1000000);
DoigtBrasCentralPR = new AX12(p9, p10, 4, 1000000);
CrocBrasGauchePR = new AX12(p13, p14, 3, 1000000);
CoudeBrasGauchePR = new AX12(p13, p14, 2, 1000000);
EpauleBrasGauchePR = new AX12(p13, p14, 1, 1000000);
CrocBrasDroitPR = new AX12(p28, p27, 11, 1000000);
CoudeBrasDroitPR = new AX12(p28, p27, 10, 1000000);
EpauleBrasDroitPR = new AX12(p28, p27, 9, 1000000);
BrasCentralPRAx12 = new AX12(p9,p10,0xFE,1000000);
BrasGauchePRAx12 = new AX12(p13,p14,0xFE,1000000);
BrasDroitPRAx12 = new AX12(p28,p27,0xFE,1000000);
BaseBrasCentralPR->Set_Goal_speed(vitesse);
CoudeBrasCentralPR->Set_Goal_speed(vitesse);
PinceDBrasCentralPR->Set_Goal_speed(vitesse);
PinceGBrasCentralPR->Set_Goal_speed(vitesse);
DoigtBrasCentralPR->Set_Goal_speed(vitesse);
CrocBrasGauchePR->Set_Goal_speed(vitesse);
CoudeBrasGauchePR->Set_Goal_speed(vitesse);
EpauleBrasGauchePR->Set_Goal_speed(vitesse);
CrocBrasDroitPR->Set_Goal_speed(vitesse);
CoudeBrasDroitPR->Set_Goal_speed(vitesse);
EpauleBrasDroitPR->Set_Goal_speed(vitesse);
BaseBrasCentralPR->Set_Mode(0);
CoudeBrasCentralPR->Set_Mode(0);
PinceDBrasCentralPR->Set_Mode(0);
PinceGBrasCentralPR->Set_Mode(0);
DoigtBrasCentralPR->Set_Mode(0);
CrocBrasGauchePR->Set_Mode(0);
CoudeBrasGauchePR->Set_Mode(0);
EpauleBrasGauchePR->Set_Mode(0);
CrocBrasDroitPR->Set_Mode(0);
CoudeBrasDroitPR->Set_Mode(0);
EpauleBrasDroitPR->Set_Mode(0);
}
void GetPositionAx12(void) {
pc.printf("\n\r ");
pc.printf("BaseC : %lf \n\r ", BaseBrasCentralPR->Get_Position() );
pc.printf("CoudeC : %lf \n\r ", CoudeBrasCentralPR->Get_Position() );
pc.printf("PinceCD : %lf \n\r ", PinceDBrasCentralPR->Get_Position());
pc.printf("PinceCG : %lf \n\r ", PinceGBrasCentralPR->Get_Position());
pc.printf("DoigtC : %lf \n\r ", DoigtBrasCentralPR->Get_Position() );
pc.printf("EpauleG : %lf \n\r ", EpauleBrasGauchePR->Get_Position());
pc.printf("CoudeG : %lf \n\r ", CoudeBrasGauchePR->Get_Position() );
pc.printf("CrocG : %lf \n\r ", CrocBrasGauchePR->Get_Position() );
pc.printf("EpauleD : %lf \n\r ", EpauleBrasDroitPR->Get_Position());
pc.printf("CoudeD : %lf \n\r ", CoudeBrasDroitPR->Get_Position() );
pc.printf("CrocD : %lf \n\r ", CrocBrasDroitPR->Get_Position() );
}
/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12 */
/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
/****************************************************************************************/
void AX12_automate(unsigned char etat_ax12){
unsigned short speed;
unsigned int GoalPosDoigt, GoalPosBase, GoalPosCoude, GoalPosPinceG, GoalPosPinceD,
GoalPosEpauleTournanteG, GoalPosCoudeTournanteG,
GoalPosEpauleTournanteD, GoalPosCoudeTournanteD;
speed = 1000;
switch(etat_ax12){
case AX12_PINCE_CENTRALE_POSITION_INITIALE :
wait_ms(TIME);
speed = 511;
GoalPosDoigt=1150;
GoalPosBase=1490;
GoalPosCoude=1470;
GoalPosPinceG=1090;
GoalPosPinceD=1930;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
wait_ms(TIME);
GoalPosDoigt=90;
GoalPosBase=170;
GoalPosCoude=1000;
GoalPosPinceG=1090;
GoalPosPinceD=1930;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_PINCE_CENTRALE_PRISE_MODULE :
wait_ms(TIME);
GoalPosDoigt=90;
GoalPosBase=170;
GoalPosCoude=1000;
GoalPosPinceG=500;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
wait_ms(TIME);
GoalPosDoigt=90;
GoalPosBase=1300;
GoalPosCoude=700;
GoalPosPinceG=500;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
wait(TIME*5);
GoalPosDoigt=90;
GoalPosBase=1300;
GoalPosCoude=1200;
GoalPosPinceG=500;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
wait(TIME*5);
GoalPosDoigt=90;
GoalPosBase=1300;
GoalPosCoude=1200;
GoalPosPinceG=1090;
GoalPosPinceD=1930;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
wait_ms(TIME);
GoalPosDoigt=90;
GoalPosBase=1200;
GoalPosCoude=443;
GoalPosPinceG=500;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
wait_ms(TIME);
GoalPosDoigt=90;
GoalPosBase=639;
GoalPosCoude=557;
GoalPosPinceG=400;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
wait(TIME*10);
GoalPosDoigt=90;
GoalPosBase=400;
GoalPosCoude=400;
GoalPosPinceG=500;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_PINCE_CENTRALE_DEPOSER :
//DEPOSER
wait_ms(TIME);
GoalPosDoigt=90;
GoalPosBase=440;
GoalPosCoude=440;
GoalPosPinceG=1090;
GoalPosPinceD=1930;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_PINCE_CENTRALE_DEPOT_HAUT :
wait(TIME*5);
GoalPosDoigt=90;
GoalPosBase=1300;
GoalPosCoude=1200;
GoalPosPinceG=1090;
GoalPosPinceD=1930;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
wait(TIME*10);
GoalPosDoigt=90;
GoalPosBase=1300;
GoalPosCoude=1200;
GoalPosPinceG=500;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
wait_ms(TIME);
GoalPosDoigt=90;
GoalPosBase=1300;
GoalPosCoude=700;
GoalPosPinceG=500;
GoalPosPinceD=2500;
mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
Fin_action();
break;
case AX12_GAUCHE_CROC_OUVERT :
wait_ms(TIME);
CrocBrasGauchePR->Set_Secure_Goal(200);
Fin_action();
break;
case AX12_GAUCHE_CROC_FERME :
wait_ms(TIME);
CrocBrasGauchePR->Set_Secure_Goal(240);
Fin_action();
break;
case AX12_GAUCHE_CROC_INITIALE :
wait_ms(TIME);
CrocBrasGauchePR->Set_Secure_Goal(300);
Fin_action();
break;
case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
wait_ms(TIME);
speed = 511;
GoalPosCoudeTournanteG=1450;
GoalPosEpauleTournanteG=600;
mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
Fin_action();
break;
case AX12_TOURNANTE_GAUCHE_PREPARATION :
wait_ms(TIME);
speed = 511;
GoalPosCoudeTournanteG=930;
GoalPosEpauleTournanteG=1962;
mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
Fin_action();
break;
case AX12_TOURNANTE_GAUCHE_MODULE :
wait_ms(TIME);
speed = 511;
GoalPosCoudeTournanteG=894;
GoalPosEpauleTournanteG=2200;
mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
Fin_action();
break;
case AX12_DROIT_CROC_OUVERT :
wait_ms(TIME);
CrocBrasDroitPR->Set_Secure_Goal(106);
Fin_action();
break;
case AX12_DROIT_CROC_FERME :
wait_ms(TIME);
CrocBrasDroitPR->Set_Secure_Goal(55);
Fin_action();
break;
case AX12_DROIT_CROC_INITIALE :
wait_ms(TIME);
CrocBrasDroitPR->Set_Secure_Goal(0);
Fin_action();
break;
case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
wait_ms(TIME);
speed = 511;
GoalPosCoudeTournanteD=1450;
GoalPosEpauleTournanteD=600;
mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
Fin_action();
break;
case AX12_TOURNANTE_DROIT_PREPARATION :
wait_ms(TIME);
speed = 511;
GoalPosCoudeTournanteD=930;
GoalPosEpauleTournanteD=1962;
mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
Fin_action();
break;
case AX12_TOURNANTE_DROIT_MODULE :
wait_ms(TIME);
speed = 511;
GoalPosCoudeTournanteD=894;
GoalPosEpauleTournanteD=2200;
mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
Fin_action();
break;
case AX12_DEFAUT :
break;
case AX12_POSITION :
GetPositionAx12();
break;
}
}
void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
{
char TabBrasCentralPR[25];
unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
GPosition1_1=((unsigned long)GPosition1*341/1000);
GPosition2_1=((unsigned long)GPosition2*341/1000);
GPosition3_1=((unsigned long)GPosition3*341/1000);
GPosition4_1=((unsigned long)GPosition4*341/1000);
GPosition5_1=((unsigned long)GPosition5*341/1000);
TabBrasCentralPR[0] = ID1;
TabBrasCentralPR[1] = GPosition1_1;
TabBrasCentralPR[2] = GPosition1_1>>8;
TabBrasCentralPR[3] = GSpeed1;
TabBrasCentralPR[4] = GSpeed1>>8;
TabBrasCentralPR[5] = ID2;
TabBrasCentralPR[6] = GPosition2_1;
TabBrasCentralPR[7] = GPosition2_1>>8;
TabBrasCentralPR[8] = GSpeed2;
TabBrasCentralPR[9] = GSpeed2>>8;
TabBrasCentralPR[10] = ID3;
TabBrasCentralPR[11] = GPosition3_1;
TabBrasCentralPR[12] = GPosition3_1>>8;
TabBrasCentralPR[13] = GSpeed3;
TabBrasCentralPR[14] = GSpeed3>>8 ;
TabBrasCentralPR[15] = ID4;
TabBrasCentralPR[16] = GPosition4_1;
TabBrasCentralPR[17] = GPosition4_1>>8;
TabBrasCentralPR[18] = GSpeed4;
TabBrasCentralPR[19] = GSpeed4>>8 ;
TabBrasCentralPR[20] = ID5;
TabBrasCentralPR[21] = GPosition5_1;
TabBrasCentralPR[22] = GPosition5_1>>8;
TabBrasCentralPR[23] = GSpeed5;
TabBrasCentralPR[24] = GSpeed5>>8 ;
BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
wait(TIME);
/*
while (((unsigned short)(BaseBrasCentralPR->Get_Position()*10)>GPosition2*108/100) || ((unsigned short)(BaseBrasCentralPR->Get_Position()*10)<GPosition2*92/100)) {
BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
pc.printf("1 \n\r ");
wait(5*TIME);
}
while (((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)>GPosition3*108/100) || ((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)<GPosition3*92/100)) {
BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
pc.printf("2 \n\r ");
wait(5*TIME);
}
while (((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)>GPosition5*108/100) || ((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)<GPosition5*92/100)) {
BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
pc.printf("3 \n\r ");
wait(5*TIME);
}
while (((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)>GPosition4*108/100) || ((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)<GPosition4*92/100)) {
BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
pc.printf("4 \n\r ");
wait(5*TIME);
}
while (((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)>GPosition1*108/100) || ((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)<GPosition1*92/100)) {
BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
pc.printf("5 \n\r ");
wait(5*TIME);
}
*/
}
void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
{
char TabBrasGauchePR[10];
unsigned short GPosition1_1, GPosition2_1;
GPosition1_1=((unsigned long)GPosition1*341/1000);
GPosition2_1=((unsigned long)GPosition2*341/1000);
TabBrasGauchePR[0] = ID1;
TabBrasGauchePR[1] = GPosition1_1;
TabBrasGauchePR[2] = GPosition1_1>>8;
TabBrasGauchePR[3] = GSpeed1;
TabBrasGauchePR[4] = GSpeed1>>8;
TabBrasGauchePR[5] = ID2;
TabBrasGauchePR[6] = GPosition2_1;
TabBrasGauchePR[7] = GPosition2_1>>8;
TabBrasGauchePR[8] = GSpeed2;
TabBrasGauchePR[9] = GSpeed2>>8;
BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
wait(TIME);
}
void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
{
char TabBrasDroitPR[10];
unsigned short GPosition1_1, GPosition2_1;
GPosition1_1=((unsigned long)GPosition1*341/1000);
GPosition2_1=((unsigned long)GPosition2*341/1000);
TabBrasDroitPR[0] = ID1;
TabBrasDroitPR[1] = GPosition1_1;
TabBrasDroitPR[2] = GPosition1_1>>8;
TabBrasDroitPR[3] = GSpeed1;
TabBrasDroitPR[4] = GSpeed1>>8;
TabBrasDroitPR[5] = ID2;
TabBrasDroitPR[6] = GPosition2_1;
TabBrasDroitPR[7] = GPosition2_1>>8;
TabBrasDroitPR[8] = GSpeed2;
TabBrasDroitPR[9] = GSpeed2>>8;
BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
wait(TIME);
}
/****************************************************************************************/
/* FUNCTION NAME: Fin_action */
/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
/****************************************************************************************/
void Fin_action(void){
CANMessage msgTx=CANMessage();
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.id = SERVO_AX12_END;
msgTx.len = 1;
msgTx.data[0] = 0;
can.write(msgTx);
}