carte_esclave2019 - carte esclave petit robot

CRAC Team » Code » carte_esclave2019

CRAC Team / Mbed 2 deprecated carte_esclave2019

carte esclave petit robot

Dependencies: mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Asservissement/Asservissement.cpp@44:381ecf63e6ab, 2019-05-26 (annotated)

Committer:: Artiom
Date:: Sun May 26 10:51:35 2019 +0000
Revision:: 44:381ecf63e6ab
Parent:: 39:124575240d09

Who changed what in which revision?

User	Revision	Line number	New contents of line
kyxstark	21:edf6d03fdb20	1
kyxstark	25:9627700d989f	2	#include "Asservissement.h"
kyxstark	21:edf6d03fdb20	3
kyxstark	21:edf6d03fdb20	4	/*********************************************************************************************************/
kyxstark	21:edf6d03fdb20	5	/* FUNCTION NAME: SendRawId */
kyxstark	21:edf6d03fdb20	6	/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
kyxstark	21:edf6d03fdb20	7	/*********************************************************************************************************/
kyxstark	21:edf6d03fdb20	8	void SendRawId (unsigned short id)
kyxstark	21:edf6d03fdb20	9	{
kyxstark	21:edf6d03fdb20	10	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	11	msgTx.id=id;
kyxstark	21:edf6d03fdb20	12	msgTx.len=0;
kyxstark	21:edf6d03fdb20	13	can.write(msgTx);
kyxstark	21:edf6d03fdb20	14	wait_us(200);
kyxstark	21:edf6d03fdb20	15	}
kyxstark	21:edf6d03fdb20	16
kyxstark	21:edf6d03fdb20	17	/*********************************************************************************************/
kyxstark	21:edf6d03fdb20	18	/* FUNCTION NAME: SendAck */
kyxstark	21:edf6d03fdb20	19	/* DESCRIPTION : Envoyer un acknowledge */
kyxstark	21:edf6d03fdb20	20	/*********************************************************************************************/
kyxstark	21:edf6d03fdb20	21	void SendAck(unsigned short id, unsigned short from)
kyxstark	21:edf6d03fdb20	22	{
kyxstark	21:edf6d03fdb20	23	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	24	msgTx.id=id;
kyxstark	21:edf6d03fdb20	25	msgTx.len=2;
kyxstark	21:edf6d03fdb20	26	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	27	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	28	// from sur 2 octets
kyxstark	21:edf6d03fdb20	29	msgTx.data[0]=(unsigned char)from;
kyxstark	21:edf6d03fdb20	30	msgTx.data[1]=(unsigned char)(from>>8);
kyxstark	21:edf6d03fdb20	31
kyxstark	21:edf6d03fdb20	32	can.write(msgTx);
kyxstark	21:edf6d03fdb20	33	}
kyxstark	21:edf6d03fdb20	34
kyxstark	21:edf6d03fdb20	35
kyxstark	21:edf6d03fdb20	36	void Send2Short(unsigned short id, unsigned short d1, unsigned short d2)
kyxstark	21:edf6d03fdb20	37	{
kyxstark	21:edf6d03fdb20	38	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	39	msgTx.id=id;
kyxstark	21:edf6d03fdb20	40	msgTx.len=4;
kyxstark	21:edf6d03fdb20	41	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	42	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	43	// from sur 2 octets
kyxstark	21:edf6d03fdb20	44	msgTx.data[0]=(unsigned char)d1;
kyxstark	21:edf6d03fdb20	45	msgTx.data[1]=(unsigned char)(d1>>8);
kyxstark	21:edf6d03fdb20	46	msgTx.data[2]=(unsigned char)d2;
kyxstark	21:edf6d03fdb20	47	msgTx.data[3]=(unsigned char)(d2>>8);
kyxstark	21:edf6d03fdb20	48
kyxstark	21:edf6d03fdb20	49	can.write(msgTx);
kyxstark	21:edf6d03fdb20	50	}
kyxstark	21:edf6d03fdb20	51
kyxstark	21:edf6d03fdb20	52	void SendMsgCan(unsigned short id, unsigned char* data, int len)
kyxstark	21:edf6d03fdb20	53	{
kyxstark	21:edf6d03fdb20	54	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	55	msgTx.id=id;
kyxstark	21:edf6d03fdb20	56	msgTx.len=len;
kyxstark	21:edf6d03fdb20	57	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	58	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	59	// from sur 2 octets
kyxstark	21:edf6d03fdb20	60	for(int i = 0; i<len; i++)
kyxstark	21:edf6d03fdb20	61	{
kyxstark	21:edf6d03fdb20	62	msgTx.data[i]=data[i];
kyxstark	21:edf6d03fdb20	63	}
kyxstark	21:edf6d03fdb20	64
kyxstark	21:edf6d03fdb20	65	can.write(msgTx);
kyxstark	21:edf6d03fdb20	66	}
kyxstark	39:124575240d09	67	void SendCharCan(unsigned short id, unsigned char data)
kyxstark	39:124575240d09	68	{
kyxstark	39:124575240d09	69	CANMessage msgTx=CANMessage();
kyxstark	39:124575240d09	70	msgTx.id=id;
kyxstark	39:124575240d09	71	msgTx.len=1;
kyxstark	39:124575240d09	72	msgTx.format=CANStandard;
kyxstark	39:124575240d09	73	msgTx.type=CANData;
kyxstark	39:124575240d09	74	msgTx.data[0]=data;
kyxstark	39:124575240d09	75
kyxstark	39:124575240d09	76	can.write(msgTx);
kyxstark	39:124575240d09	77	}
kyxstark	21:edf6d03fdb20	78
kyxstark	21:edf6d03fdb20	79
kyxstark	21:edf6d03fdb20	80
kyxstark	21:edf6d03fdb20	81	/*********************************************************************************************/
kyxstark	21:edf6d03fdb20	82	/* FUNCTION NAME: GoToPosition */
kyxstark	21:edf6d03fdb20	83	/* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
kyxstark	21:edf6d03fdb20	84	/*********************************************************************************************/
kyxstark	21:edf6d03fdb20	85	void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
kyxstark	21:edf6d03fdb20	86	{
kyxstark	21:edf6d03fdb20	87	//id_to_expect=ACK_CONSIGNE;
kyxstark	21:edf6d03fdb20	88
kyxstark	21:edf6d03fdb20	89	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	90	msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
kyxstark	21:edf6d03fdb20	91	msgTx.len=7;
kyxstark	21:edf6d03fdb20	92	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	93	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	94	// x sur 2 octets
kyxstark	21:edf6d03fdb20	95	msgTx.data[0]=(unsigned char)x;
kyxstark	21:edf6d03fdb20	96	msgTx.data[1]=(unsigned char)(x>>8);
kyxstark	21:edf6d03fdb20	97	// y sur 2 octets
kyxstark	21:edf6d03fdb20	98	msgTx.data[2]=(unsigned char)y;
kyxstark	21:edf6d03fdb20	99	msgTx.data[3]=(unsigned char)(y>>8);
kyxstark	21:edf6d03fdb20	100	// theta signé sur 2 octets
kyxstark	21:edf6d03fdb20	101	msgTx.data[4]=(unsigned char)theta;
kyxstark	21:edf6d03fdb20	102	msgTx.data[5]=(unsigned char)(theta>>8);
kyxstark	21:edf6d03fdb20	103	msgTx.data[6]=sens;
kyxstark	21:edf6d03fdb20	104
kyxstark	21:edf6d03fdb20	105	can.write(msgTx);
kyxstark	21:edf6d03fdb20	106	}
kyxstark	21:edf6d03fdb20	107
kyxstark	21:edf6d03fdb20	108	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	109	/* FUNCTION NAME: Rotate */
kyxstark	21:edf6d03fdb20	110	/* DESCRIPTION : Transmission CAN correspondant à une rotation */
kyxstark	21:edf6d03fdb20	111	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	112	void Rotate (signed short angle)
kyxstark	21:edf6d03fdb20	113	{
kyxstark	21:edf6d03fdb20	114	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	115	msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot
kyxstark	21:edf6d03fdb20	116	msgTx.len=2;
kyxstark	21:edf6d03fdb20	117	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	118	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	119	// Angle signé sur 2 octets
kyxstark	21:edf6d03fdb20	120	msgTx.data[0]=(unsigned char)angle;
kyxstark	21:edf6d03fdb20	121	msgTx.data[1]=(unsigned char)(angle>>8);
kyxstark	21:edf6d03fdb20	122
kyxstark	21:edf6d03fdb20	123	can.write(msgTx);
kyxstark	21:edf6d03fdb20	124	}
kyxstark	21:edf6d03fdb20	125
kyxstark	21:edf6d03fdb20	126
kyxstark	21:edf6d03fdb20	127	/*********************************************************************************************/
kyxstark	21:edf6d03fdb20	128	/* FUNCTION NAME: GoStraight */
kyxstark	21:edf6d03fdb20	129	/* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
kyxstark	21:edf6d03fdb20	130	/* recalage : 0 => pas de recalage */
kyxstark	21:edf6d03fdb20	131	/* 1 => recalage en X */
kyxstark	21:edf6d03fdb20	132	/* 2 => Recalage en Y */
kyxstark	21:edf6d03fdb20	133	/* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
kyxstark	21:edf6d03fdb20	134	/* isEnchainement : Indique si il faut executer l'instruction en enchainement */
kyxstark	21:edf6d03fdb20	135	/* 0 => non */
kyxstark	21:edf6d03fdb20	136	/* 1 => oui */
kyxstark	21:edf6d03fdb20	137	/* 2 => dernière instruction de l'enchainement */
kyxstark	21:edf6d03fdb20	138	/*********************************************************************************************/
kyxstark	21:edf6d03fdb20	139	void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
kyxstark	21:edf6d03fdb20	140	{
kyxstark	21:edf6d03fdb20	141	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	142	msgTx.id=ASSERVISSEMENT_RECALAGE;
kyxstark	21:edf6d03fdb20	143	msgTx.len=6;
kyxstark	21:edf6d03fdb20	144	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	145	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	146	// x sur 2 octets
kyxstark	21:edf6d03fdb20	147	msgTx.data[0]=(unsigned char)distance;
kyxstark	21:edf6d03fdb20	148	msgTx.data[1]=(unsigned char)(distance>>8);
kyxstark	21:edf6d03fdb20	149	//Recalage sur 1 octet
kyxstark	21:edf6d03fdb20	150	msgTx.data[2]=recalage;
kyxstark	21:edf6d03fdb20	151	//Valeur du recalage sur 2 octets
kyxstark	21:edf6d03fdb20	152	msgTx.data[3]=(unsigned char)newValue;
kyxstark	21:edf6d03fdb20	153	msgTx.data[4]=(unsigned char)(newValue>>8);
kyxstark	21:edf6d03fdb20	154	//Enchainement sur 1 octet
kyxstark	21:edf6d03fdb20	155	msgTx.data[5]=isEnchainement;
kyxstark	21:edf6d03fdb20	156
kyxstark	21:edf6d03fdb20	157	can.write(msgTx);
kyxstark	21:edf6d03fdb20	158	//wait_ms(500);
kyxstark	21:edf6d03fdb20	159	}
kyxstark	21:edf6d03fdb20	160
kyxstark	21:edf6d03fdb20	161	/********************************************************************************************/
kyxstark	21:edf6d03fdb20	162	/* FUNCTION NAME: BendRadius */
kyxstark	21:edf6d03fdb20	163	/* DESCRIPTION : Transmission CAN correspondant à un rayon de courbure */
kyxstark	21:edf6d03fdb20	164	/********************************************************************************************/
kyxstark	21:edf6d03fdb20	165	void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement)
kyxstark	21:edf6d03fdb20	166	{
kyxstark	21:edf6d03fdb20	167	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	168	msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure
kyxstark	21:edf6d03fdb20	169	msgTx.len=6;
kyxstark	21:edf6d03fdb20	170	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	171	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	172	// Rayon sur 2 octets
kyxstark	21:edf6d03fdb20	173	msgTx.data[0]=(unsigned char)rayon;
kyxstark	21:edf6d03fdb20	174	msgTx.data[1]=(unsigned char)(rayon>>8);
kyxstark	21:edf6d03fdb20	175	// Angle signé sur 2 octets
kyxstark	21:edf6d03fdb20	176	msgTx.data[2]=(unsigned char)angle;
kyxstark	21:edf6d03fdb20	177	msgTx.data[3]=(unsigned char)(angle>>8);
kyxstark	21:edf6d03fdb20	178	// Sens signé sur 1 octet
kyxstark	21:edf6d03fdb20	179	msgTx.data[4]=sens;
kyxstark	21:edf6d03fdb20	180	// Enchainement sur 1 octet
kyxstark	21:edf6d03fdb20	181	msgTx.data[5]=enchainement;
kyxstark	21:edf6d03fdb20	182
kyxstark	21:edf6d03fdb20	183	can.write(msgTx);
kyxstark	21:edf6d03fdb20	184	}
kyxstark	21:edf6d03fdb20	185
kyxstark	21:edf6d03fdb20	186	void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta)
kyxstark	21:edf6d03fdb20	187	{
kyxstark	21:edf6d03fdb20	188	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	189	msgTx.id=canId;
kyxstark	21:edf6d03fdb20	190	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	191	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	192	msgTx.len=6;
kyxstark	21:edf6d03fdb20	193
kyxstark	21:edf6d03fdb20	194	// x sur 2 octets
kyxstark	21:edf6d03fdb20	195	msgTx.data[0]=(unsigned char)x;
kyxstark	21:edf6d03fdb20	196	msgTx.data[1]=(unsigned char)(x>>8);
kyxstark	21:edf6d03fdb20	197	// y sur 2 octets
kyxstark	21:edf6d03fdb20	198	msgTx.data[2]=(unsigned char)y;
kyxstark	21:edf6d03fdb20	199	msgTx.data[3]=(unsigned char)(y>>8);
kyxstark	21:edf6d03fdb20	200	// theta signé sur 2 octets
kyxstark	21:edf6d03fdb20	201	msgTx.data[4]=(unsigned char)theta;
kyxstark	21:edf6d03fdb20	202	msgTx.data[5]=(unsigned char)(theta>>8);
kyxstark	21:edf6d03fdb20	203
kyxstark	21:edf6d03fdb20	204	can.write(msgTx);
kyxstark	21:edf6d03fdb20	205	}
kyxstark	21:edf6d03fdb20	206
kyxstark	21:edf6d03fdb20	207	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	208	/* FUNCTION NAME: setAsservissementEtat */
kyxstark	21:edf6d03fdb20	209	/* DESCRIPTION : Activer ou désactiver l'asservissement */
kyxstark	21:edf6d03fdb20	210	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	211	void setAsservissementEtat(unsigned char enable)
kyxstark	21:edf6d03fdb20	212	{
kyxstark	21:edf6d03fdb20	213	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	214	msgTx.id=ASSERVISSEMENT_ENABLE; // Tx rotation autour du centre du robot
kyxstark	21:edf6d03fdb20	215	msgTx.len=1;
kyxstark	21:edf6d03fdb20	216	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	217	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	218	// Angle signé sur 2 octets
kyxstark	21:edf6d03fdb20	219	msgTx.data[0]=(unsigned char)((enable==0)?0:1);
kyxstark	21:edf6d03fdb20	220
kyxstark	21:edf6d03fdb20	221	can.write(msgTx);
kyxstark	21:edf6d03fdb20	222	}
kyxstark	21:edf6d03fdb20	223
kyxstark	21:edf6d03fdb20	224
kyxstark	21:edf6d03fdb20	225	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	226	/* FUNCTION NAME: SendSpeed */
kyxstark	21:edf6d03fdb20	227	/* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
kyxstark	21:edf6d03fdb20	228	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	229	void SendSpeed (unsigned short vitesse, unsigned short acceleration,unsigned short deceleration)
kyxstark	21:edf6d03fdb20	230	{
kyxstark	21:edf6d03fdb20	231	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	232	msgTx.id=ASSERVISSEMENT_CONFIG;
kyxstark	21:edf6d03fdb20	233	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	234	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	235	msgTx.len=8;
kyxstark	21:edf6d03fdb20	236	msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
kyxstark	21:edf6d03fdb20	237	msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
kyxstark	21:edf6d03fdb20	238
kyxstark	21:edf6d03fdb20	239	msgTx.data[2]=(unsigned char)(acceleration&0x00FF);
kyxstark	21:edf6d03fdb20	240	msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8);
kyxstark	21:edf6d03fdb20	241
kyxstark	21:edf6d03fdb20	242	msgTx.data[4]=(unsigned char)(deceleration&0x00FF);
kyxstark	21:edf6d03fdb20	243	msgTx.data[5]=(unsigned char)((deceleration&0xFF00)>>8);
kyxstark	21:edf6d03fdb20	244
kyxstark	21:edf6d03fdb20	245	msgTx.data[6]=(unsigned char)(acceleration&0x00FF);//cloto
kyxstark	21:edf6d03fdb20	246	msgTx.data[7]=(unsigned char)((acceleration&0xFF00)>>8);//cloto
kyxstark	21:edf6d03fdb20	247
kyxstark	21:edf6d03fdb20	248	can.write(msgTx);
kyxstark	21:edf6d03fdb20	249
kyxstark	21:edf6d03fdb20	250	}
kyxstark	21:edf6d03fdb20	251
kyxstark	21:edf6d03fdb20	252	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	253	/* FUNCTION NAME: SendSpeedDecel */
kyxstark	21:edf6d03fdb20	254	/* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
kyxstark	21:edf6d03fdb20	255	/****************************************************************************************/
kyxstark	21:edf6d03fdb20	256	/*
kyxstark	21:edf6d03fdb20	257	void SendSpeedDecel (unsigned short vitesse, unsigned short deceleration)
kyxstark	21:edf6d03fdb20	258	{
kyxstark	21:edf6d03fdb20	259	CANMessage msgTx=CANMessage();
kyxstark	21:edf6d03fdb20	260	msgTx.id=ASSERVISSEMENT_CONFIG_DECEL;
kyxstark	21:edf6d03fdb20	261	msgTx.format=CANStandard;
kyxstark	21:edf6d03fdb20	262	msgTx.type=CANData;
kyxstark	21:edf6d03fdb20	263	msgTx.len=4;
kyxstark	21:edf6d03fdb20	264	msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
kyxstark	21:edf6d03fdb20	265	msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
kyxstark	21:edf6d03fdb20	266	msgTx.data[2]=(unsigned char)(deceleration&0x00FF);
kyxstark	21:edf6d03fdb20	267	msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8);
kyxstark	21:edf6d03fdb20	268
kyxstark	21:edf6d03fdb20	269	can.write(msgTx);
kyxstark	21:edf6d03fdb20	270
kyxstark	21:edf6d03fdb20	271	}*/
kyxstark	21:edf6d03fdb20	272
kyxstark	21:edf6d03fdb20	273

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	10 May 2019
Imports:	11
Forks:	0
Commits:	45
Dependents:	0
Dependencies:	5
Followers:	96

Asservissement/Asservissement.cpp@44:381ecf63e6ab, 2019-05-26 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning