Utilisatio_MX12_V2 - Programme d'utilisation des AX12 avec rajout de l…

CRAC Team » Code » Utilisatio_MX12_V2

Programme d'utilisation des AX12 avec rajout de l'MX12

Fork of test_carteAToutFaire_PR by CRAC Team

AX12/AX12.cpp@3:1bb26049bdd1, 2017-05-20 (annotated)

Committer:: R66Y
Date:: Sat May 20 15:23:58 2017 +0000
Revision:: 3:1bb26049bdd1
Parent:: 1:f3f702086a30

Programme de contr?le des AX12 avec rajout de l'MX12;

Who changed what in which revision?

User	Revision	Line number	New contents of line
ClementBreteau	1:f3f702086a30	1	/* mbed AX-12+ Servo Library
ClementBreteau	1:f3f702086a30	2	*
ClementBreteau	1:f3f702086a30	3	* Copyright (c) 2010, cstyles (http://mbed.org)
ClementBreteau	1:f3f702086a30	4	*
ClementBreteau	1:f3f702086a30	5	* Permission is hereby granted, free of charge, to any person obtaining a copy
ClementBreteau	1:f3f702086a30	6	* of this software and associated documentation files (the "Software"), to deal
ClementBreteau	1:f3f702086a30	7	* in the Software without restriction, including without limitation the rights
ClementBreteau	1:f3f702086a30	8	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
ClementBreteau	1:f3f702086a30	9	* copies of the Software, and to permit persons to whom the Software is
ClementBreteau	1:f3f702086a30	10	* furnished to do so, subject to the following conditions:
ClementBreteau	1:f3f702086a30	11	*
ClementBreteau	1:f3f702086a30	12	* The above copyright notice and this permission notice shall be included in
ClementBreteau	1:f3f702086a30	13	* all copies or substantial portions of the Software.
ClementBreteau	1:f3f702086a30	14	*
ClementBreteau	1:f3f702086a30	15	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
ClementBreteau	1:f3f702086a30	16	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
ClementBreteau	1:f3f702086a30	17	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
ClementBreteau	1:f3f702086a30	18	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
ClementBreteau	1:f3f702086a30	19	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
ClementBreteau	1:f3f702086a30	20	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
ClementBreteau	1:f3f702086a30	21	* THE SOFTWARE.
ClementBreteau	1:f3f702086a30	22	*/
ClementBreteau	1:f3f702086a30	23
ClementBreteau	1:f3f702086a30	24	#include "mbed.h"
ClementBreteau	1:f3f702086a30	25	#include "AX12.h"
ClementBreteau	1:f3f702086a30	26
ClementBreteau	1:f3f702086a30	27	#define MAX_TIMEOUT 500
ClementBreteau	1:f3f702086a30	28
ClementBreteau	1:f3f702086a30	29	extern Timer t;
ClementBreteau	1:f3f702086a30	30
ClementBreteau	1:f3f702086a30	31	typedef struct
ClementBreteau	1:f3f702086a30	32	{
ClementBreteau	1:f3f702086a30	33	unsigned short Model_Number;
ClementBreteau	1:f3f702086a30	34	unsigned char Firmware;
ClementBreteau	1:f3f702086a30	35	unsigned char ID;
ClementBreteau	1:f3f702086a30	36	unsigned char Baud_Rate;
ClementBreteau	1:f3f702086a30	37	unsigned char Return_Delay_Time;
ClementBreteau	1:f3f702086a30	38	unsigned short CW_Angle_Limit;
ClementBreteau	1:f3f702086a30	39	unsigned short CCW_Angle_Limit;
ClementBreteau	1:f3f702086a30	40	unsigned char Reserved1;
ClementBreteau	1:f3f702086a30	41	unsigned char Highest_Limit_Temperature;
ClementBreteau	1:f3f702086a30	42	unsigned char Lowest_Limit_voltage;
ClementBreteau	1:f3f702086a30	43	unsigned char Highest_Limit_voltage;
ClementBreteau	1:f3f702086a30	44	unsigned short Max_Torque;
ClementBreteau	1:f3f702086a30	45	unsigned char Status_Return_Level;
ClementBreteau	1:f3f702086a30	46	unsigned char Alarm_LED;
ClementBreteau	1:f3f702086a30	47	unsigned char Alarm_Shutdown;
ClementBreteau	1:f3f702086a30	48	unsigned char Reserved2;
ClementBreteau	1:f3f702086a30	49	unsigned short Down_Calibration;
ClementBreteau	1:f3f702086a30	50	unsigned short Up_Calibration;
ClementBreteau	1:f3f702086a30	51	unsigned char Torque_Enable;
ClementBreteau	1:f3f702086a30	52	unsigned char LED;
ClementBreteau	1:f3f702086a30	53	unsigned char CW_Compliance_Margin;
ClementBreteau	1:f3f702086a30	54	unsigned char CCW_Compliance_Margin;
ClementBreteau	1:f3f702086a30	55	unsigned char CW_Compliance_Slope;
ClementBreteau	1:f3f702086a30	56	unsigned char CCW_Compliance_Slope;
ClementBreteau	1:f3f702086a30	57	unsigned short Goal_Position;
ClementBreteau	1:f3f702086a30	58	unsigned short Moving_Speed;
ClementBreteau	1:f3f702086a30	59	unsigned short Torque_Limit;
ClementBreteau	1:f3f702086a30	60	unsigned short Present_Position;
ClementBreteau	1:f3f702086a30	61	unsigned short Present_Speed;
ClementBreteau	1:f3f702086a30	62	unsigned short Present_Load;
ClementBreteau	1:f3f702086a30	63	unsigned char Present_Voltage;
ClementBreteau	1:f3f702086a30	64	unsigned char Present_Temperature;
ClementBreteau	1:f3f702086a30	65	unsigned char Registered_Instruction;
ClementBreteau	1:f3f702086a30	66	unsigned char Reserved3;
ClementBreteau	1:f3f702086a30	67	unsigned char Moving;
ClementBreteau	1:f3f702086a30	68	unsigned char Lock;
ClementBreteau	1:f3f702086a30	69	unsigned short Punch;
ClementBreteau	1:f3f702086a30	70	} T_AX12;
ClementBreteau	1:f3f702086a30	71
ClementBreteau	1:f3f702086a30	72
ClementBreteau	1:f3f702086a30	73	AX12::AX12(PinName tx, PinName rx, int ID, int baud)
ClementBreteau	1:f3f702086a30	74	: _ax12(tx,rx)
ClementBreteau	1:f3f702086a30	75	{
ClementBreteau	1:f3f702086a30	76	_baud = baud;
ClementBreteau	1:f3f702086a30	77	_ID = ID;
ClementBreteau	1:f3f702086a30	78	_ax12.baud(_baud);
ClementBreteau	1:f3f702086a30	79
ClementBreteau	1:f3f702086a30	80
ClementBreteau	1:f3f702086a30	81	}
ClementBreteau	1:f3f702086a30	82
ClementBreteau	1:f3f702086a30	83	int AX12::Set_Secure_Goal(int degres)
ClementBreteau	1:f3f702086a30	84	{
ClementBreteau	1:f3f702086a30	85	int error = 0;
ClementBreteau	1:f3f702086a30	86	int position = 0;
ClementBreteau	1:f3f702086a30	87	int difference = 0;
ClementBreteau	1:f3f702086a30	88	int timeout_secure = 0;
ClementBreteau	1:f3f702086a30	89	int autorisation = 0;
ClementBreteau	1:f3f702086a30	90
ClementBreteau	1:f3f702086a30	91	position = Get_Position();
ClementBreteau	1:f3f702086a30	92	error = Set_Goal(degres);
ClementBreteau	1:f3f702086a30	93
ClementBreteau	1:f3f702086a30	94	while ((autorisation == 0) && (timeout_secure < 100) )
ClementBreteau	1:f3f702086a30	95	{
ClementBreteau	1:f3f702086a30	96	position = Get_Position();
ClementBreteau	1:f3f702086a30	97	//printf("position : %d", position );
ClementBreteau	1:f3f702086a30	98	error = Set_Goal(degres);
ClementBreteau	1:f3f702086a30	99	//printf("degres : %d", degres);
ClementBreteau	1:f3f702086a30	100	difference = degres - position;
ClementBreteau	1:f3f702086a30	101	//printf ("difference : %d", difference );
ClementBreteau	1:f3f702086a30	102	if (((difference < 2) && (difference > (-2) )))
ClementBreteau	1:f3f702086a30	103	autorisation = 1;
ClementBreteau	1:f3f702086a30	104
ClementBreteau	1:f3f702086a30	105	timeout_secure++;
ClementBreteau	1:f3f702086a30	106	}
ClementBreteau	1:f3f702086a30	107
ClementBreteau	1:f3f702086a30	108	if ( timeout_secure == 100)
ClementBreteau	1:f3f702086a30	109	{
ClementBreteau	1:f3f702086a30	110	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	111	printf (" timeout secure error ");
ClementBreteau	1:f3f702086a30	112	#endif
ClementBreteau	1:f3f702086a30	113	return(-1);
ClementBreteau	1:f3f702086a30	114	}
ClementBreteau	1:f3f702086a30	115	return(error);
ClementBreteau	1:f3f702086a30	116	}
ClementBreteau	1:f3f702086a30	117
ClementBreteau	1:f3f702086a30	118
ClementBreteau	1:f3f702086a30	119	int AX12::Get_Return_Delay_Time(void)
ClementBreteau	1:f3f702086a30	120	{
ClementBreteau	1:f3f702086a30	121	char data[1];
ClementBreteau	1:f3f702086a30	122	int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data);
ClementBreteau	1:f3f702086a30	123	int time = data[0];
ClementBreteau	1:f3f702086a30	124	time = 2.0 * time;
ClementBreteau	1:f3f702086a30	125	return(time);
ClementBreteau	1:f3f702086a30	126	}
ClementBreteau	1:f3f702086a30	127
ClementBreteau	1:f3f702086a30	128
ClementBreteau	1:f3f702086a30	129	int AX12::Get_Baud_Rate(void)
ClementBreteau	1:f3f702086a30	130	{
ClementBreteau	1:f3f702086a30	131	char data[1];
ClementBreteau	1:f3f702086a30	132	int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data);
ClementBreteau	1:f3f702086a30	133	int baud = data[0];
ClementBreteau	1:f3f702086a30	134	baud = 2000000 / ( baud + 1 );
ClementBreteau	1:f3f702086a30	135	return(baud);
ClementBreteau	1:f3f702086a30	136	}
ClementBreteau	1:f3f702086a30	137
ClementBreteau	1:f3f702086a30	138
ClementBreteau	1:f3f702086a30	139	/** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur
ClementBreteau	1:f3f702086a30	140	// minimum >> Ox000 >> decimal 0
ClementBreteau	1:f3f702086a30	141	// maximum >> 0x3FF >> decimal 1023
ClementBreteau	1:f3f702086a30	142	// deflaut >> 0x20 >> decimal 32
ClementBreteau	1:f3f702086a30	143	*/
ClementBreteau	1:f3f702086a30	144	int AX12::Set_Punch(int punch)
ClementBreteau	1:f3f702086a30	145	{
ClementBreteau	1:f3f702086a30	146	char data[2];
ClementBreteau	1:f3f702086a30	147
ClementBreteau	1:f3f702086a30	148	data[0] = punch & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	149	data[1] = punch >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	150
ClementBreteau	1:f3f702086a30	151	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	152	return (write(_ID, AX12_REG_PUNCH, 2, data));
ClementBreteau	1:f3f702086a30	153
ClementBreteau	1:f3f702086a30	154	}
ClementBreteau	1:f3f702086a30	155
ClementBreteau	1:f3f702086a30	156	/** Reset
ClementBreteau	1:f3f702086a30	157	*/
ClementBreteau	1:f3f702086a30	158	int AX12::Reset(int punch)
ClementBreteau	1:f3f702086a30	159	{
ClementBreteau	1:f3f702086a30	160	char data[2];
ClementBreteau	1:f3f702086a30	161
ClementBreteau	1:f3f702086a30	162	data[0] = punch & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	163	data[1] = punch >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	164
ClementBreteau	1:f3f702086a30	165	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	166	return (write(_ID, 0x06, 1,data));
ClementBreteau	1:f3f702086a30	167
ClementBreteau	1:f3f702086a30	168	}
ClementBreteau	1:f3f702086a30	169
ClementBreteau	1:f3f702086a30	170	int AX12::Get_Punch (void)
ClementBreteau	1:f3f702086a30	171	{
ClementBreteau	1:f3f702086a30	172	char data[2];
ClementBreteau	1:f3f702086a30	173	int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data);
ClementBreteau	1:f3f702086a30	174	int punch = data[0] \| (data[1]<<8);
ClementBreteau	1:f3f702086a30	175	return(punch);
ClementBreteau	1:f3f702086a30	176	}
ClementBreteau	1:f3f702086a30	177
ClementBreteau	1:f3f702086a30	178
ClementBreteau	1:f3f702086a30	179	int AX12::Get_Load_Direction (void)
ClementBreteau	1:f3f702086a30	180	{
ClementBreteau	1:f3f702086a30	181	char data[2];
ClementBreteau	1:f3f702086a30	182	int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
ClementBreteau	1:f3f702086a30	183	int direction = (data[1]>>2) & 0x01;
ClementBreteau	1:f3f702086a30	184	return(direction);
ClementBreteau	1:f3f702086a30	185	}
ClementBreteau	1:f3f702086a30	186
ClementBreteau	1:f3f702086a30	187
ClementBreteau	1:f3f702086a30	188	int AX12::Get_Load_Value (void)
ClementBreteau	1:f3f702086a30	189	{
ClementBreteau	1:f3f702086a30	190	char data[2];
ClementBreteau	1:f3f702086a30	191	int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
ClementBreteau	1:f3f702086a30	192	int Load = (data[0] \| (data[1]<<8)) & 0x3FF;
ClementBreteau	1:f3f702086a30	193	return(Load);
ClementBreteau	1:f3f702086a30	194	}
ClementBreteau	1:f3f702086a30	195
ClementBreteau	1:f3f702086a30	196
ClementBreteau	1:f3f702086a30	197	int AX12::Get_Present_Speed (void)
ClementBreteau	1:f3f702086a30	198	{
ClementBreteau	1:f3f702086a30	199	char data[2];
ClementBreteau	1:f3f702086a30	200	int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data);
ClementBreteau	1:f3f702086a30	201	int speed = data[0] \| (data[1]<<8);
ClementBreteau	1:f3f702086a30	202	return(speed);
ClementBreteau	1:f3f702086a30	203	}
ClementBreteau	1:f3f702086a30	204
ClementBreteau	1:f3f702086a30	205
ClementBreteau	1:f3f702086a30	206	int AX12::Get_CCW_Angle_Limit (void)
ClementBreteau	1:f3f702086a30	207	{
ClementBreteau	1:f3f702086a30	208	char data[2];
ClementBreteau	1:f3f702086a30	209	int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data);
ClementBreteau	1:f3f702086a30	210	int angle = data[0] \| (data[1]<<8);
ClementBreteau	1:f3f702086a30	211	angle = (angle * 300) / 1023;
ClementBreteau	1:f3f702086a30	212	return(angle);
ClementBreteau	1:f3f702086a30	213	}
ClementBreteau	1:f3f702086a30	214
ClementBreteau	1:f3f702086a30	215
ClementBreteau	1:f3f702086a30	216	int AX12::Get_CW_Angle_Limit (void)
ClementBreteau	1:f3f702086a30	217	{
ClementBreteau	1:f3f702086a30	218	char data[2];
ClementBreteau	1:f3f702086a30	219	int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data);
ClementBreteau	1:f3f702086a30	220	int angle = data[0] \| (data[1]<<8);
ClementBreteau	1:f3f702086a30	221	angle = (angle * 300) / 1023;
ClementBreteau	1:f3f702086a30	222	return(angle);
ClementBreteau	1:f3f702086a30	223	}
ClementBreteau	1:f3f702086a30	224
ClementBreteau	1:f3f702086a30	225
ClementBreteau	1:f3f702086a30	226
ClementBreteau	1:f3f702086a30	227	int AX12::Get_Torque_Enable(void)
ClementBreteau	1:f3f702086a30	228	{
ClementBreteau	1:f3f702086a30	229	char data[1];
ClementBreteau	1:f3f702086a30	230	int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
ClementBreteau	1:f3f702086a30	231	int enable = data[0];
ClementBreteau	1:f3f702086a30	232	return(enable);
ClementBreteau	1:f3f702086a30	233	}
ClementBreteau	1:f3f702086a30	234
ClementBreteau	1:f3f702086a30	235
ClementBreteau	1:f3f702086a30	236	int AX12::Set_Torque_Enable(int etat)
ClementBreteau	1:f3f702086a30	237	{
ClementBreteau	1:f3f702086a30	238	char data[1];
ClementBreteau	1:f3f702086a30	239	data [0] = etat;
ClementBreteau	1:f3f702086a30	240
ClementBreteau	1:f3f702086a30	241	int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
ClementBreteau	1:f3f702086a30	242	return (error);
ClementBreteau	1:f3f702086a30	243	}
ClementBreteau	1:f3f702086a30	244
ClementBreteau	1:f3f702086a30	245
ClementBreteau	1:f3f702086a30	246
ClementBreteau	1:f3f702086a30	247	int AX12::Get_Up_Calibration (void)
ClementBreteau	1:f3f702086a30	248	{
ClementBreteau	1:f3f702086a30	249	char data[1];
ClementBreteau	1:f3f702086a30	250	int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data);
ClementBreteau	1:f3f702086a30	251	int Up_calibration = data[0] \| (data[1]<<8);
ClementBreteau	1:f3f702086a30	252	return(Up_calibration);
ClementBreteau	1:f3f702086a30	253	}
ClementBreteau	1:f3f702086a30	254
ClementBreteau	1:f3f702086a30	255
ClementBreteau	1:f3f702086a30	256
ClementBreteau	1:f3f702086a30	257	int AX12::Get_Down_Calibration (void)
ClementBreteau	1:f3f702086a30	258	{
ClementBreteau	1:f3f702086a30	259	char data[1];
ClementBreteau	1:f3f702086a30	260	int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data);
ClementBreteau	1:f3f702086a30	261	int Dowm_calibration = data[0] \| (data[1]<<8);
ClementBreteau	1:f3f702086a30	262	return(Dowm_calibration);
ClementBreteau	1:f3f702086a30	263	}
ClementBreteau	1:f3f702086a30	264
ClementBreteau	1:f3f702086a30	265
ClementBreteau	1:f3f702086a30	266
ClementBreteau	1:f3f702086a30	267	int AX12::Get_ID(void)
ClementBreteau	1:f3f702086a30	268	{
ClementBreteau	1:f3f702086a30	269
ClementBreteau	1:f3f702086a30	270	char data[1]={-1};
ClementBreteau	1:f3f702086a30	271	int ErrorCode = read(_ID, AX12_REG_ID, 1, data);
ClementBreteau	1:f3f702086a30	272	int id = data[0];
ClementBreteau	1:f3f702086a30	273	return(id);
ClementBreteau	1:f3f702086a30	274	}
ClementBreteau	1:f3f702086a30	275
ClementBreteau	1:f3f702086a30	276
ClementBreteau	1:f3f702086a30	277	// Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 )
ClementBreteau	1:f3f702086a30	278	int AX12::Get_Max_Torque (void)
ClementBreteau	1:f3f702086a30	279	{
ClementBreteau	1:f3f702086a30	280	char data[2];
ClementBreteau	1:f3f702086a30	281	int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data);
ClementBreteau	1:f3f702086a30	282	int torque = data[0] \| (data[1]<<8);
ClementBreteau	1:f3f702086a30	283	return(torque);
ClementBreteau	1:f3f702086a30	284	}
ClementBreteau	1:f3f702086a30	285
ClementBreteau	1:f3f702086a30	286
ClementBreteau	1:f3f702086a30	287
ClementBreteau	1:f3f702086a30	288	/** Reglage du couple maximum de l'actionneur
ClementBreteau	1:f3f702086a30	289	// minimum >> Ox000 >> decimal 0
ClementBreteau	1:f3f702086a30	290	// maximum >> 0x3FF >> decimal 1023
ClementBreteau	1:f3f702086a30	291	// deflaut >> >> decimal
ClementBreteau	1:f3f702086a30	292	*/
ClementBreteau	1:f3f702086a30	293	int AX12::Set_Max_Torque(int torque)
ClementBreteau	1:f3f702086a30	294	{
ClementBreteau	1:f3f702086a30	295	char data[2];
ClementBreteau	1:f3f702086a30	296
ClementBreteau	1:f3f702086a30	297	data[0] = torque & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	298	data[1] = torque >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	299
ClementBreteau	1:f3f702086a30	300	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	301	return (write(_ID, AX12_REG_MAX_TORQUE, 2, data));
ClementBreteau	1:f3f702086a30	302
ClementBreteau	1:f3f702086a30	303	}
ClementBreteau	1:f3f702086a30	304
ClementBreteau	1:f3f702086a30	305
ClementBreteau	1:f3f702086a30	306
ClementBreteau	1:f3f702086a30	307
ClementBreteau	1:f3f702086a30	308	/** Reglage de la desactivation des actionneurs si une erreur le concernant se produit
ClementBreteau	1:f3f702086a30	309	Bit Function
ClementBreteau	1:f3f702086a30	310	Bit 7 0
ClementBreteau	1:f3f702086a30	311	Bit 6 If set to 1, torque off when an Instruction Error occurs
ClementBreteau	1:f3f702086a30	312	Bit 5 If set to 1, torque off when an Overload Error occurs
ClementBreteau	1:f3f702086a30	313	Bit 4 If set to 1, torque off when a Checksum Error occurs
ClementBreteau	1:f3f702086a30	314	Bit 3 If set to 1, torque off when a Range Error occurs
ClementBreteau	1:f3f702086a30	315	Bit 2 If set to 1, torque off when an Overheating Error occurs
ClementBreteau	1:f3f702086a30	316	Bit 1 If set to 1, torque off when an Angle Limit Error occurs
ClementBreteau	1:f3f702086a30	317	Bit 0 If set to 1, torque off when an Input Voltage Error occurs
ClementBreteau	1:f3f702086a30	318	*/
ClementBreteau	1:f3f702086a30	319	int AX12::Set_Alarm_Shutdown(int valeur)
ClementBreteau	1:f3f702086a30	320	{
ClementBreteau	1:f3f702086a30	321	char data[1];
ClementBreteau	1:f3f702086a30	322	data [0] = valeur;
ClementBreteau	1:f3f702086a30	323
ClementBreteau	1:f3f702086a30	324	int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data);
ClementBreteau	1:f3f702086a30	325	return (val_alarm_shutdown);
ClementBreteau	1:f3f702086a30	326	}
ClementBreteau	1:f3f702086a30	327
ClementBreteau	1:f3f702086a30	328
ClementBreteau	1:f3f702086a30	329
ClementBreteau	1:f3f702086a30	330	/** Reglage de l'activation de l'alarme
ClementBreteau	1:f3f702086a30	331	Bit Function
ClementBreteau	1:f3f702086a30	332	Bit 7 0
ClementBreteau	1:f3f702086a30	333	Bit 6 If set to 1, the LED blinks when an Instruction Error occurs
ClementBreteau	1:f3f702086a30	334	Bit 5 If set to 1, the LED blinks when an Overload Error occurs
ClementBreteau	1:f3f702086a30	335	Bit 4 If set to 1, the LED blinks when a Checksum Error occurs
ClementBreteau	1:f3f702086a30	336	Bit 3 If set to 1, the LED blinks when a Range Error occurs
ClementBreteau	1:f3f702086a30	337	Bit 2 If set to 1, the LED blinks when an Overheating Error occurs
ClementBreteau	1:f3f702086a30	338	Bit 1 If set to 1, the LED blinks when an Angle Limit Error occurs
ClementBreteau	1:f3f702086a30	339	Bit 0 If set to 1, the LED blinks when an Input Voltage Error occurs
ClementBreteau	1:f3f702086a30	340	*/
ClementBreteau	1:f3f702086a30	341	int AX12::Set_Alarm_LED(int valeur)
ClementBreteau	1:f3f702086a30	342	{
ClementBreteau	1:f3f702086a30	343	char data[1];
ClementBreteau	1:f3f702086a30	344	data [0] = valeur;
ClementBreteau	1:f3f702086a30	345
ClementBreteau	1:f3f702086a30	346	int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data);
ClementBreteau	1:f3f702086a30	347	return (val_alarmLED);
ClementBreteau	1:f3f702086a30	348	}
ClementBreteau	1:f3f702086a30	349
ClementBreteau	1:f3f702086a30	350
ClementBreteau	1:f3f702086a30	351
ClementBreteau	1:f3f702086a30	352
ClementBreteau	1:f3f702086a30	353	// Reglage de la réponse à une instruction
ClementBreteau	1:f3f702086a30	354	// 0 >> ne repond a aucune instructions
ClementBreteau	1:f3f702086a30	355	// 1 >> repond seulement aux instructions READ_DATA
ClementBreteau	1:f3f702086a30	356	// 2 >> repond à toutes les instructions
ClementBreteau	1:f3f702086a30	357	int AX12::Set_Status_Return_Level(int etat)
ClementBreteau	1:f3f702086a30	358	{
ClementBreteau	1:f3f702086a30	359	char data[1];
ClementBreteau	1:f3f702086a30	360	data [0] = etat;
ClementBreteau	1:f3f702086a30	361
ClementBreteau	1:f3f702086a30	362	int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data);
ClementBreteau	1:f3f702086a30	363	return (val_return_lvl);
ClementBreteau	1:f3f702086a30	364	}
ClementBreteau	1:f3f702086a30	365
ClementBreteau	1:f3f702086a30	366
ClementBreteau	1:f3f702086a30	367	// Reglage de la tension minimale
ClementBreteau	1:f3f702086a30	368	// minimum >> Ox32 >> decimal 50
ClementBreteau	1:f3f702086a30	369	// maximum >> 0xFA >> decimal 250
ClementBreteau	1:f3f702086a30	370	// deflaut >> 0x3C >> decimal 60
ClementBreteau	1:f3f702086a30	371	int AX12::Set_Lowest_Voltage(int val_lowest_voltage)
ClementBreteau	1:f3f702086a30	372	{
ClementBreteau	1:f3f702086a30	373	char data[1];
ClementBreteau	1:f3f702086a30	374	data [0] = val_lowest_voltage;
ClementBreteau	1:f3f702086a30	375
ClementBreteau	1:f3f702086a30	376	int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data);
ClementBreteau	1:f3f702086a30	377	return (val_lowvolt);
ClementBreteau	1:f3f702086a30	378	}
ClementBreteau	1:f3f702086a30	379
ClementBreteau	1:f3f702086a30	380
ClementBreteau	1:f3f702086a30	381	// Reglage de la tension maximale
ClementBreteau	1:f3f702086a30	382	// minimum >> Ox32 >> decimal 50
ClementBreteau	1:f3f702086a30	383	// maximum >> 0xFA >> decimal 250
ClementBreteau	1:f3f702086a30	384	// deflaut >> 0xBE >> decimal 190
ClementBreteau	1:f3f702086a30	385	int AX12::Set_Highest_Voltage(int val_highest_voltage)
ClementBreteau	1:f3f702086a30	386	{
ClementBreteau	1:f3f702086a30	387	char data[1];
ClementBreteau	1:f3f702086a30	388	data [0] = val_highest_voltage;
ClementBreteau	1:f3f702086a30	389
ClementBreteau	1:f3f702086a30	390	int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data);
ClementBreteau	1:f3f702086a30	391	return (val_highvolt);
ClementBreteau	1:f3f702086a30	392	}
ClementBreteau	1:f3f702086a30	393
ClementBreteau	1:f3f702086a30	394
ClementBreteau	1:f3f702086a30	395	// Reglage du return time delay EN MICRO SECONDE 2uSec * val_delay_time
ClementBreteau	1:f3f702086a30	396	// minimum >> 0 us
ClementBreteau	1:f3f702086a30	397	// maximum >> 508 us
ClementBreteau	1:f3f702086a30	398	// deflaut >> 125 us
ClementBreteau	1:f3f702086a30	399	int AX12::Set_Delay_Time (int val_delay_time )
ClementBreteau	1:f3f702086a30	400	{
ClementBreteau	1:f3f702086a30	401	char data[1];
ClementBreteau	1:f3f702086a30	402	data [0] = val_delay_time/2.0;
ClementBreteau	1:f3f702086a30	403
ClementBreteau	1:f3f702086a30	404	int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data);
ClementBreteau	1:f3f702086a30	405	return (valdelay_time );
ClementBreteau	1:f3f702086a30	406	}
ClementBreteau	1:f3f702086a30	407
ClementBreteau	1:f3f702086a30	408
ClementBreteau	1:f3f702086a30	409	// Reglage de la température max du cervo
ClementBreteau	1:f3f702086a30	410	// minimum >> Ox00 >> decimal 0
ClementBreteau	1:f3f702086a30	411	// maximum >> 0x96 >> decimal 150
ClementBreteau	1:f3f702086a30	412	int AX12::Set_Temperature_Max (int val_temperature )
ClementBreteau	1:f3f702086a30	413	{
ClementBreteau	1:f3f702086a30	414	char data[1];
ClementBreteau	1:f3f702086a30	415	data [0] = val_temperature;
ClementBreteau	1:f3f702086a30	416
ClementBreteau	1:f3f702086a30	417	int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data);
ClementBreteau	1:f3f702086a30	418	return (valtemp_max );
ClementBreteau	1:f3f702086a30	419	}
ClementBreteau	1:f3f702086a30	420
ClementBreteau	1:f3f702086a30	421	// Etat LED
ClementBreteau	1:f3f702086a30	422	// 0 = off
ClementBreteau	1:f3f702086a30	423	// 1 = on
ClementBreteau	1:f3f702086a30	424	int AX12::Set_Etat_LED(int etat)
ClementBreteau	1:f3f702086a30	425	{
ClementBreteau	1:f3f702086a30	426	char data[1];
ClementBreteau	1:f3f702086a30	427	data [0] = etat;
ClementBreteau	1:f3f702086a30	428
ClementBreteau	1:f3f702086a30	429	int valLED = write(_ID, AX12_REG_LED, 1, data);
ClementBreteau	1:f3f702086a30	430	return (valLED);
ClementBreteau	1:f3f702086a30	431	}
ClementBreteau	1:f3f702086a30	432
ClementBreteau	1:f3f702086a30	433	// Set the mode of the servo
ClementBreteau	1:f3f702086a30	434	// 0 = Positional (0-300 degrees)
ClementBreteau	1:f3f702086a30	435	// 1 = Rotational -1 to 1 speed
ClementBreteau	1:f3f702086a30	436	int AX12::Set_Mode(int mode)
ClementBreteau	1:f3f702086a30	437	{
ClementBreteau	1:f3f702086a30	438
ClementBreteau	1:f3f702086a30	439	if (mode == 1) { // set CR
ClementBreteau	1:f3f702086a30	440	//wait(0.001);
ClementBreteau	1:f3f702086a30	441	Set_CW_Angle_Limit(0);
ClementBreteau	1:f3f702086a30	442	//wait(0.001);
ClementBreteau	1:f3f702086a30	443	Set_CCW_Angle_Limit(0);
ClementBreteau	1:f3f702086a30	444	//wait(0.001);
ClementBreteau	1:f3f702086a30	445	Set_CR_Speed(0.0);
ClementBreteau	1:f3f702086a30	446	//wait(0.001);
ClementBreteau	1:f3f702086a30	447	} else {
ClementBreteau	1:f3f702086a30	448	//wait(0.001);
ClementBreteau	1:f3f702086a30	449	Set_CW_Angle_Limit(0);
ClementBreteau	1:f3f702086a30	450	//wait(0.001);
ClementBreteau	1:f3f702086a30	451	Set_CCW_Angle_Limit(300);
ClementBreteau	1:f3f702086a30	452	//wait(0.001);
ClementBreteau	1:f3f702086a30	453	Set_CR_Speed(0.0);
ClementBreteau	1:f3f702086a30	454	//wait(0.001);
ClementBreteau	1:f3f702086a30	455	}
ClementBreteau	1:f3f702086a30	456	return(0);
ClementBreteau	1:f3f702086a30	457	}
ClementBreteau	1:f3f702086a30	458
ClementBreteau	1:f3f702086a30	459	int AX12::Set_Goal_speed(int speed, int flags)
ClementBreteau	1:f3f702086a30	460	{
ClementBreteau	1:f3f702086a30	461
ClementBreteau	1:f3f702086a30	462	char reg_flag = 0;
ClementBreteau	1:f3f702086a30	463	char data[2];
ClementBreteau	1:f3f702086a30	464
ClementBreteau	1:f3f702086a30	465	// set the flag is only the register bit is set in the flag
ClementBreteau	1:f3f702086a30	466	if (flags == 0x2) {
ClementBreteau	1:f3f702086a30	467	reg_flag = 1;
ClementBreteau	1:f3f702086a30	468	}
ClementBreteau	1:f3f702086a30	469
ClementBreteau	1:f3f702086a30	470	data[0] = speed & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	471	data[1] = speed >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	472
ClementBreteau	1:f3f702086a30	473	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	474	int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data, reg_flag);
ClementBreteau	1:f3f702086a30	475
ClementBreteau	1:f3f702086a30	476	/*if (flags == 1) {
ClementBreteau	1:f3f702086a30	477	// block until it comes to a halt
ClementBreteau	1:f3f702086a30	478	while (isMoving())
ClementBreteau	1:f3f702086a30	479	{
ClementBreteau	1:f3f702086a30	480	}
ClementBreteau	1:f3f702086a30	481
ClementBreteau	1:f3f702086a30	482	}*/
ClementBreteau	1:f3f702086a30	483	return(rVal);
ClementBreteau	1:f3f702086a30	484	}
ClementBreteau	1:f3f702086a30	485
ClementBreteau	1:f3f702086a30	486
ClementBreteau	1:f3f702086a30	487	// if flag[0] is set, were blocking
ClementBreteau	1:f3f702086a30	488	// if flag[1] is set, we're registering
ClementBreteau	1:f3f702086a30	489	// they are mutually exclusive operations
ClementBreteau	1:f3f702086a30	490	int AX12::Set_Goal(int degrees, int flags)
ClementBreteau	1:f3f702086a30	491	{
ClementBreteau	1:f3f702086a30	492
ClementBreteau	1:f3f702086a30	493	char reg_flag = 0;
ClementBreteau	1:f3f702086a30	494	char data[2];
ClementBreteau	1:f3f702086a30	495
ClementBreteau	1:f3f702086a30	496	// set the flag is only the register bit is set in the flag
ClementBreteau	1:f3f702086a30	497	if (flags == 0x2) {
ClementBreteau	1:f3f702086a30	498	reg_flag = 1;
ClementBreteau	1:f3f702086a30	499	}
ClementBreteau	1:f3f702086a30	500
ClementBreteau	1:f3f702086a30	501	// 1023 / 300 * degrees
ClementBreteau	1:f3f702086a30	502	short goal = (1023 * degrees) / 300;
ClementBreteau	1:f3f702086a30	503	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	504	printf("SetGoal to 0x%x\n",goal);
ClementBreteau	1:f3f702086a30	505	#endif
ClementBreteau	1:f3f702086a30	506
ClementBreteau	1:f3f702086a30	507	data[0] = goal & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	508	data[1] = goal >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	509
ClementBreteau	1:f3f702086a30	510	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	511	int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
ClementBreteau	1:f3f702086a30	512
ClementBreteau	1:f3f702086a30	513	/*if (flags == 1) {
ClementBreteau	1:f3f702086a30	514	// block until it comes to a halt
ClementBreteau	1:f3f702086a30	515	while (isMoving())
ClementBreteau	1:f3f702086a30	516	{
ClementBreteau	1:f3f702086a30	517	}
ClementBreteau	1:f3f702086a30	518
ClementBreteau	1:f3f702086a30	519	}*/
ClementBreteau	1:f3f702086a30	520	return(rVal);
ClementBreteau	1:f3f702086a30	521	}
ClementBreteau	1:f3f702086a30	522
ClementBreteau	1:f3f702086a30	523
ClementBreteau	1:f3f702086a30	524	// Set continuous rotation speed from -1 to 1
ClementBreteau	1:f3f702086a30	525	int AX12::Set_CR_Speed(float speed)
ClementBreteau	1:f3f702086a30	526	{
ClementBreteau	1:f3f702086a30	527
ClementBreteau	1:f3f702086a30	528	// bit 10 = direction, 0 = CCW, 1=CW
ClementBreteau	1:f3f702086a30	529	// bits 9-0 = Speed
ClementBreteau	1:f3f702086a30	530	char data[2];
ClementBreteau	1:f3f702086a30	531
ClementBreteau	1:f3f702086a30	532	int goal = (0x3ff * abs(speed));
ClementBreteau	1:f3f702086a30	533
ClementBreteau	1:f3f702086a30	534	// Set direction CW if we have a negative speed
ClementBreteau	1:f3f702086a30	535	if (speed < 0) {
ClementBreteau	1:f3f702086a30	536	goal \|= (0x1 << 10);
ClementBreteau	1:f3f702086a30	537	}
ClementBreteau	1:f3f702086a30	538
ClementBreteau	1:f3f702086a30	539	data[0] = goal & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	540	data[1] = goal >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	541
ClementBreteau	1:f3f702086a30	542	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	543	int rVal = write(_ID, 0x20, 2, data);
ClementBreteau	1:f3f702086a30	544
ClementBreteau	1:f3f702086a30	545	return(rVal);
ClementBreteau	1:f3f702086a30	546	}
ClementBreteau	1:f3f702086a30	547
ClementBreteau	1:f3f702086a30	548
ClementBreteau	1:f3f702086a30	549	int AX12::Set_CW_Angle_Limit (int degrees)
ClementBreteau	1:f3f702086a30	550	{
ClementBreteau	1:f3f702086a30	551
ClementBreteau	1:f3f702086a30	552	char data[2];
ClementBreteau	1:f3f702086a30	553
ClementBreteau	1:f3f702086a30	554	// 1023 / 300 * degrees
ClementBreteau	1:f3f702086a30	555	short limit = (1023 * degrees) / 300;
ClementBreteau	1:f3f702086a30	556
ClementBreteau	1:f3f702086a30	557	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	558	printf("SetCWLimit to 0x%x\n",limit);
ClementBreteau	1:f3f702086a30	559	#endif
ClementBreteau	1:f3f702086a30	560
ClementBreteau	1:f3f702086a30	561	data[0] = limit & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	562	data[1] = limit >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	563
ClementBreteau	1:f3f702086a30	564	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	565	return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
ClementBreteau	1:f3f702086a30	566
ClementBreteau	1:f3f702086a30	567	}
ClementBreteau	1:f3f702086a30	568
ClementBreteau	1:f3f702086a30	569	int AX12::Set_CCW_Angle_Limit (int degrees)
ClementBreteau	1:f3f702086a30	570	{
ClementBreteau	1:f3f702086a30	571
ClementBreteau	1:f3f702086a30	572	char data[2];
ClementBreteau	1:f3f702086a30	573
ClementBreteau	1:f3f702086a30	574	// 1023 / 300 * degrees
ClementBreteau	1:f3f702086a30	575	short limit = (1023 * degrees) / 300;
ClementBreteau	1:f3f702086a30	576
ClementBreteau	1:f3f702086a30	577	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	578	printf("SetCCWLimit to 0x%x\n",limit);
ClementBreteau	1:f3f702086a30	579	#endif
ClementBreteau	1:f3f702086a30	580
ClementBreteau	1:f3f702086a30	581	data[0] = limit & 0xff; // bottom 8 bits
ClementBreteau	1:f3f702086a30	582	data[1] = limit >> 8; // top 8 bits
ClementBreteau	1:f3f702086a30	583
ClementBreteau	1:f3f702086a30	584	// write the packet, return the error code
ClementBreteau	1:f3f702086a30	585	return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
ClementBreteau	1:f3f702086a30	586	}
ClementBreteau	1:f3f702086a30	587
ClementBreteau	1:f3f702086a30	588
ClementBreteau	1:f3f702086a30	589	int AX12::Set_ID (int CurrentID, int NewID)
ClementBreteau	1:f3f702086a30	590	{
ClementBreteau	1:f3f702086a30	591
ClementBreteau	1:f3f702086a30	592	char data[1];
ClementBreteau	1:f3f702086a30	593	data[0] = NewID;
ClementBreteau	1:f3f702086a30	594
ClementBreteau	1:f3f702086a30	595	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	596	printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
ClementBreteau	1:f3f702086a30	597	#endif
ClementBreteau	1:f3f702086a30	598
ClementBreteau	1:f3f702086a30	599	return (write(CurrentID, AX12_REG_ID, 1, data));
ClementBreteau	1:f3f702086a30	600
ClementBreteau	1:f3f702086a30	601	}
ClementBreteau	1:f3f702086a30	602
ClementBreteau	1:f3f702086a30	603
ClementBreteau	1:f3f702086a30	604	int AX12::Set_Baud (int baud)
ClementBreteau	1:f3f702086a30	605	{
ClementBreteau	1:f3f702086a30	606
ClementBreteau	1:f3f702086a30	607	char data[1];
ClementBreteau	1:f3f702086a30	608	data[0] = baud;
ClementBreteau	1:f3f702086a30	609
ClementBreteau	1:f3f702086a30	610	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	611	printf("Setting Baud rate to %d\n",baud);
ClementBreteau	1:f3f702086a30	612	#endif
ClementBreteau	1:f3f702086a30	613
ClementBreteau	1:f3f702086a30	614	return (write(_ID, AX12_REG_BAUD, 1, data));
ClementBreteau	1:f3f702086a30	615
ClementBreteau	1:f3f702086a30	616	}
ClementBreteau	1:f3f702086a30	617
ClementBreteau	1:f3f702086a30	618
ClementBreteau	1:f3f702086a30	619
ClementBreteau	1:f3f702086a30	620	// return 1 is the servo is still in flight
ClementBreteau	1:f3f702086a30	621	int AX12::isMoving(void)
ClementBreteau	1:f3f702086a30	622	{
ClementBreteau	1:f3f702086a30	623
ClementBreteau	1:f3f702086a30	624	char data[1];
ClementBreteau	1:f3f702086a30	625	read(_ID,AX12_REG_MOVING,1,data);
ClementBreteau	1:f3f702086a30	626	return(data[0]);
ClementBreteau	1:f3f702086a30	627	}
ClementBreteau	1:f3f702086a30	628
ClementBreteau	1:f3f702086a30	629	void AX12::reset()
ClementBreteau	1:f3f702086a30	630	{
ClementBreteau	1:f3f702086a30	631
ClementBreteau	1:f3f702086a30	632	unsigned char TxBuf[16];
ClementBreteau	1:f3f702086a30	633	unsigned char sum = 0;
ClementBreteau	1:f3f702086a30	634	unsigned long debut=0;
ClementBreteau	1:f3f702086a30	635
ClementBreteau	1:f3f702086a30	636	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	637	// Build the TxPacket first in RAM, then we'll send in one go
ClementBreteau	1:f3f702086a30	638	printf("\nreset\n");
ClementBreteau	1:f3f702086a30	639	printf("\nreset Packet\n Header : 0xFF, 0xFF\n");
ClementBreteau	1:f3f702086a30	640	#endif
ClementBreteau	1:f3f702086a30	641
ClementBreteau	1:f3f702086a30	642	TxBuf[0] = 0xFF;
ClementBreteau	1:f3f702086a30	643	TxBuf[1] = 0xFF;
ClementBreteau	1:f3f702086a30	644
ClementBreteau	1:f3f702086a30	645	// ID - Broadcast
ClementBreteau	1:f3f702086a30	646	TxBuf[2] =_ID;
ClementBreteau	1:f3f702086a30	647	sum += TxBuf[2];
ClementBreteau	1:f3f702086a30	648
ClementBreteau	1:f3f702086a30	649	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	650	printf(" ID : %d\n",TxBuf[2]);
ClementBreteau	1:f3f702086a30	651	#endif
ClementBreteau	1:f3f702086a30	652
ClementBreteau	1:f3f702086a30	653	// Length
ClementBreteau	1:f3f702086a30	654	TxBuf[3] = 0x02;
ClementBreteau	1:f3f702086a30	655	sum += TxBuf[3];
ClementBreteau	1:f3f702086a30	656
ClementBreteau	1:f3f702086a30	657	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	658	printf(" Length %d\n",TxBuf[3]);
ClementBreteau	1:f3f702086a30	659	#endif
ClementBreteau	1:f3f702086a30	660
ClementBreteau	1:f3f702086a30	661	// Instruction - ACTION
ClementBreteau	1:f3f702086a30	662	TxBuf[4] = 0x06; //reset
ClementBreteau	1:f3f702086a30	663	sum += TxBuf[4];
ClementBreteau	1:f3f702086a30	664
ClementBreteau	1:f3f702086a30	665	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	666	printf(" Instruction 0x%X\n",TxBuf[5]);
ClementBreteau	1:f3f702086a30	667	#endif
ClementBreteau	1:f3f702086a30	668
ClementBreteau	1:f3f702086a30	669	// Checksum
ClementBreteau	1:f3f702086a30	670	TxBuf[5] = 0xFF - sum;
ClementBreteau	1:f3f702086a30	671	//#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	672	printf(" Checksum 0x%X\n",TxBuf[5]);
ClementBreteau	1:f3f702086a30	673	//#endif
ClementBreteau	1:f3f702086a30	674
ClementBreteau	1:f3f702086a30	675	// Transmit the packet in one burst with no pausing
ClementBreteau	1:f3f702086a30	676	for (int i = 0; i < 6 ; i++)
ClementBreteau	1:f3f702086a30	677	{
ClementBreteau	1:f3f702086a30	678	while(_ax12.writeable()==0);
ClementBreteau	1:f3f702086a30	679	_ax12.putc(TxBuf[i]);
ClementBreteau	1:f3f702086a30	680
ClementBreteau	1:f3f702086a30	681	}
ClementBreteau	1:f3f702086a30	682	wait(0.001);
ClementBreteau	1:f3f702086a30	683	debut=t.read_ms();
ClementBreteau	1:f3f702086a30	684
ClementBreteau	1:f3f702086a30	685	do
ClementBreteau	1:f3f702086a30	686	{
ClementBreteau	1:f3f702086a30	687	if (_ax12.readable()==-1) // reception du premier Header ( 0xFF )
ClementBreteau	1:f3f702086a30	688	printf("%02x",_ax12.getc());
ClementBreteau	1:f3f702086a30	689	}
ClementBreteau	1:f3f702086a30	690	while((t.read_ms()-debut)<500);
ClementBreteau	1:f3f702086a30	691
ClementBreteau	1:f3f702086a30	692	printf("\n");
ClementBreteau	1:f3f702086a30	693	return;
ClementBreteau	1:f3f702086a30	694	}
ClementBreteau	1:f3f702086a30	695
ClementBreteau	1:f3f702086a30	696	void AX12::read_all_info(unsigned char start, unsigned char longueur)
ClementBreteau	1:f3f702086a30	697	{
ClementBreteau	1:f3f702086a30	698
ClementBreteau	1:f3f702086a30	699	unsigned char TxBuf[16];
ClementBreteau	1:f3f702086a30	700	unsigned char sum = 0;
ClementBreteau	1:f3f702086a30	701	unsigned long debut=0;
ClementBreteau	1:f3f702086a30	702
ClementBreteau	1:f3f702086a30	703	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	704	// Build the TxPacket first in RAM, then we'll send in one go
ClementBreteau	1:f3f702086a30	705	printf("\nreset\n");
ClementBreteau	1:f3f702086a30	706	printf("\nreset Packet\n Header : 0xFF, 0xFF\n");
ClementBreteau	1:f3f702086a30	707	#endif
ClementBreteau	1:f3f702086a30	708
ClementBreteau	1:f3f702086a30	709	TxBuf[0] = 0xFF;
ClementBreteau	1:f3f702086a30	710	TxBuf[1] = 0xFF;
ClementBreteau	1:f3f702086a30	711
ClementBreteau	1:f3f702086a30	712	// ID - Broadcast
ClementBreteau	1:f3f702086a30	713	TxBuf[2] =_ID;
ClementBreteau	1:f3f702086a30	714	sum += TxBuf[2];
ClementBreteau	1:f3f702086a30	715
ClementBreteau	1:f3f702086a30	716	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	717	printf(" ID : %d\n",TxBuf[2]);
ClementBreteau	1:f3f702086a30	718	#endif
ClementBreteau	1:f3f702086a30	719
ClementBreteau	1:f3f702086a30	720	// Length
ClementBreteau	1:f3f702086a30	721	TxBuf[3] = 0x04;
ClementBreteau	1:f3f702086a30	722	sum += TxBuf[3];
ClementBreteau	1:f3f702086a30	723
ClementBreteau	1:f3f702086a30	724	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	725	printf(" Length %d\n",TxBuf[3]);
ClementBreteau	1:f3f702086a30	726	#endif
ClementBreteau	1:f3f702086a30	727
ClementBreteau	1:f3f702086a30	728	// Instruction - ACTION
ClementBreteau	1:f3f702086a30	729	TxBuf[4] = INST_READ; //reset
ClementBreteau	1:f3f702086a30	730	sum += TxBuf[4];
ClementBreteau	1:f3f702086a30	731
ClementBreteau	1:f3f702086a30	732	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	733	printf(" Instruction 0x%X\n",TxBuf[4]);
ClementBreteau	1:f3f702086a30	734	#endif
ClementBreteau	1:f3f702086a30	735
ClementBreteau	1:f3f702086a30	736	TxBuf[5] = start; //reset
ClementBreteau	1:f3f702086a30	737	sum += TxBuf[5];
ClementBreteau	1:f3f702086a30	738
ClementBreteau	1:f3f702086a30	739	TxBuf[6] = longueur; //reset
ClementBreteau	1:f3f702086a30	740	sum += TxBuf[6];
ClementBreteau	1:f3f702086a30	741
ClementBreteau	1:f3f702086a30	742
ClementBreteau	1:f3f702086a30	743	// Checksum
ClementBreteau	1:f3f702086a30	744	TxBuf[7] = 0xFF - sum;
ClementBreteau	1:f3f702086a30	745	//#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	746	//printf(" Checksum 0x%X\n\r",TxBuf[7]);
ClementBreteau	1:f3f702086a30	747	//#endif
ClementBreteau	1:f3f702086a30	748
ClementBreteau	1:f3f702086a30	749	// Transmit the packet in one burst with no pausing
ClementBreteau	1:f3f702086a30	750	for (int i = 0; i < 8 ; i++)
ClementBreteau	1:f3f702086a30	751	{
ClementBreteau	1:f3f702086a30	752	while(_ax12.writeable()==0);
ClementBreteau	1:f3f702086a30	753	_ax12.putc(TxBuf[i]);
ClementBreteau	1:f3f702086a30	754
ClementBreteau	1:f3f702086a30	755	}
ClementBreteau	1:f3f702086a30	756
ClementBreteau	1:f3f702086a30	757	debut=t.read_ms();
ClementBreteau	1:f3f702086a30	758	int i=0;
ClementBreteau	1:f3f702086a30	759	do
ClementBreteau	1:f3f702086a30	760	{
ClementBreteau	1:f3f702086a30	761	if (_ax12.readable())
ClementBreteau	1:f3f702086a30	762	{ // reception du premier Header ( 0xFF )
ClementBreteau	1:f3f702086a30	763	printf("%02d:%02x ",start+i,_ax12.getc());
ClementBreteau	1:f3f702086a30	764	i++;
ClementBreteau	1:f3f702086a30	765	}
ClementBreteau	1:f3f702086a30	766	}
ClementBreteau	1:f3f702086a30	767	while((t.read_ms()-debut)<5000);
ClementBreteau	1:f3f702086a30	768
ClementBreteau	1:f3f702086a30	769	printf("\n");
ClementBreteau	1:f3f702086a30	770	return;
ClementBreteau	1:f3f702086a30	771	}
ClementBreteau	1:f3f702086a30	772
ClementBreteau	1:f3f702086a30	773
ClementBreteau	1:f3f702086a30	774	void AX12::trigger(void)
ClementBreteau	1:f3f702086a30	775	{
ClementBreteau	1:f3f702086a30	776
ClementBreteau	1:f3f702086a30	777	char TxBuf[16];
ClementBreteau	1:f3f702086a30	778	char sum = 0;
ClementBreteau	1:f3f702086a30	779
ClementBreteau	1:f3f702086a30	780	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	781	// Build the TxPacket first in RAM, then we'll send in one go
ClementBreteau	1:f3f702086a30	782	printf("\nTriggered\n");
ClementBreteau	1:f3f702086a30	783	printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
ClementBreteau	1:f3f702086a30	784	#endif
ClementBreteau	1:f3f702086a30	785
ClementBreteau	1:f3f702086a30	786	TxBuf[0] = 0xFF;
ClementBreteau	1:f3f702086a30	787	TxBuf[1] = 0xFF;
ClementBreteau	1:f3f702086a30	788
ClementBreteau	1:f3f702086a30	789	// ID - Broadcast
ClementBreteau	1:f3f702086a30	790	TxBuf[2] = 0xFE;
ClementBreteau	1:f3f702086a30	791	sum += TxBuf[2];
ClementBreteau	1:f3f702086a30	792
ClementBreteau	1:f3f702086a30	793	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	794	printf(" ID : %d\n",TxBuf[2]);
ClementBreteau	1:f3f702086a30	795	#endif
ClementBreteau	1:f3f702086a30	796
ClementBreteau	1:f3f702086a30	797	// Length
ClementBreteau	1:f3f702086a30	798	TxBuf[3] = 0x02;
ClementBreteau	1:f3f702086a30	799	sum += TxBuf[3];
ClementBreteau	1:f3f702086a30	800
ClementBreteau	1:f3f702086a30	801	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	802	printf(" Length %d\n",TxBuf[3]);
ClementBreteau	1:f3f702086a30	803	#endif
ClementBreteau	1:f3f702086a30	804
ClementBreteau	1:f3f702086a30	805	// Instruction - ACTION
ClementBreteau	1:f3f702086a30	806	TxBuf[4] = 0x04;
ClementBreteau	1:f3f702086a30	807	sum += TxBuf[4];
ClementBreteau	1:f3f702086a30	808
ClementBreteau	1:f3f702086a30	809	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	810	printf(" Instruction 0x%X\n",TxBuf[5]);
ClementBreteau	1:f3f702086a30	811	#endif
ClementBreteau	1:f3f702086a30	812
ClementBreteau	1:f3f702086a30	813	// Checksum
ClementBreteau	1:f3f702086a30	814	TxBuf[5] = 0xFF - sum;
ClementBreteau	1:f3f702086a30	815	#ifdef AX12_TRIGGER_DEBUG
ClementBreteau	1:f3f702086a30	816	printf(" Checksum 0x%X\n",TxBuf[5]);
ClementBreteau	1:f3f702086a30	817	#endif
ClementBreteau	1:f3f702086a30	818
ClementBreteau	1:f3f702086a30	819	// Transmit the packet in one burst with no pausing
ClementBreteau	1:f3f702086a30	820	for (int i = 0; i < 6 ; i++) {
ClementBreteau	1:f3f702086a30	821	_ax12.putc(TxBuf[i]);
ClementBreteau	1:f3f702086a30	822	}
ClementBreteau	1:f3f702086a30	823
ClementBreteau	1:f3f702086a30	824	// This is a broadcast packet, so there will be no reply
ClementBreteau	1:f3f702086a30	825	return;
ClementBreteau	1:f3f702086a30	826	}
ClementBreteau	1:f3f702086a30	827
ClementBreteau	1:f3f702086a30	828
ClementBreteau	1:f3f702086a30	829	float AX12::Get_Position(void)
ClementBreteau	1:f3f702086a30	830	{
ClementBreteau	1:f3f702086a30	831
ClementBreteau	1:f3f702086a30	832	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	833	printf("\nGetPositionID(%d)",_ID);
ClementBreteau	1:f3f702086a30	834	#endif
ClementBreteau	1:f3f702086a30	835
ClementBreteau	1:f3f702086a30	836	char data[2];
ClementBreteau	1:f3f702086a30	837
ClementBreteau	1:f3f702086a30	838	int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
ClementBreteau	1:f3f702086a30	839	int position = data[0] \| (data[1] << 8);
ClementBreteau	1:f3f702086a30	840	float angle = ((float)position * 300.0)/1023.0;
ClementBreteau	1:f3f702086a30	841
ClementBreteau	1:f3f702086a30	842	return (angle);
ClementBreteau	1:f3f702086a30	843	}
ClementBreteau	1:f3f702086a30	844
ClementBreteau	1:f3f702086a30	845
ClementBreteau	1:f3f702086a30	846	float AX12::Get_Temp ()
ClementBreteau	1:f3f702086a30	847	{
ClementBreteau	1:f3f702086a30	848
ClementBreteau	1:f3f702086a30	849	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	850	printf("\nGetTemp(%d)",_ID);
ClementBreteau	1:f3f702086a30	851	#endif
ClementBreteau	1:f3f702086a30	852
ClementBreteau	1:f3f702086a30	853	char data[1];
ClementBreteau	1:f3f702086a30	854	int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
ClementBreteau	1:f3f702086a30	855	float temp = data[0];
ClementBreteau	1:f3f702086a30	856	return(temp);
ClementBreteau	1:f3f702086a30	857	}
ClementBreteau	1:f3f702086a30	858
ClementBreteau	1:f3f702086a30	859
ClementBreteau	1:f3f702086a30	860	float AX12::Get_Volts (void)
ClementBreteau	1:f3f702086a30	861	{
ClementBreteau	1:f3f702086a30	862
ClementBreteau	1:f3f702086a30	863	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	864	printf("\nGetVolts(%d)",_ID);
ClementBreteau	1:f3f702086a30	865	#endif
ClementBreteau	1:f3f702086a30	866
ClementBreteau	1:f3f702086a30	867	char data[1];
ClementBreteau	1:f3f702086a30	868	int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
ClementBreteau	1:f3f702086a30	869	float volts = data[0]/10.0;
ClementBreteau	1:f3f702086a30	870	return(volts);
ClementBreteau	1:f3f702086a30	871	}
ClementBreteau	1:f3f702086a30	872
ClementBreteau	1:f3f702086a30	873
ClementBreteau	1:f3f702086a30	874	int AX12::read(int ID, int start, int bytes, char* data) {
ClementBreteau	1:f3f702086a30	875
ClementBreteau	1:f3f702086a30	876
ClementBreteau	1:f3f702086a30	877	char PacketLength = 0x3;
ClementBreteau	1:f3f702086a30	878	char TxBuf[16];
ClementBreteau	1:f3f702086a30	879	char sum = 0;
ClementBreteau	1:f3f702086a30	880	char Status[16];
ClementBreteau	1:f3f702086a30	881
ClementBreteau	1:f3f702086a30	882	int timeout = 0;
ClementBreteau	1:f3f702086a30	883	int plen = 0;
ClementBreteau	1:f3f702086a30	884	int flag_out = 0;
ClementBreteau	1:f3f702086a30	885	int timeout_transmit = 0;
ClementBreteau	1:f3f702086a30	886	int i = 0;
ClementBreteau	1:f3f702086a30	887	int enable = 0;
ClementBreteau	1:f3f702086a30	888	// int poubelle = 0;
ClementBreteau	1:f3f702086a30	889	// int count = 0;
ClementBreteau	1:f3f702086a30	890	// char vidage[50];
ClementBreteau	1:f3f702086a30	891
ClementBreteau	1:f3f702086a30	892	typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat;
ClementBreteau	1:f3f702086a30	893	type_etat etat = Header1;
ClementBreteau	1:f3f702086a30	894
ClementBreteau	1:f3f702086a30	895	Status[4] = 0xFE; // return code
ClementBreteau	1:f3f702086a30	896
ClementBreteau	1:f3f702086a30	897
ClementBreteau	1:f3f702086a30	898
ClementBreteau	1:f3f702086a30	899
ClementBreteau	1:f3f702086a30	900
ClementBreteau	1:f3f702086a30	901	/********************************* CREATION DE LA TRAME A EVOYER ***************************************/
ClementBreteau	1:f3f702086a30	902
ClementBreteau	1:f3f702086a30	903	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	904	printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
ClementBreteau	1:f3f702086a30	905	#endif
ClementBreteau	1:f3f702086a30	906
ClementBreteau	1:f3f702086a30	907	// Build the TxPacket first in RAM, then we'll send in one go
ClementBreteau	1:f3f702086a30	908	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	909	printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
ClementBreteau	1:f3f702086a30	910	#endif
ClementBreteau	1:f3f702086a30	911
ClementBreteau	1:f3f702086a30	912	TxBuf[0] = 0xff;
ClementBreteau	1:f3f702086a30	913	TxBuf[1] = 0xff;
ClementBreteau	1:f3f702086a30	914
ClementBreteau	1:f3f702086a30	915	// ID
ClementBreteau	1:f3f702086a30	916	TxBuf[2] = ID;
ClementBreteau	1:f3f702086a30	917	sum += TxBuf[2];
ClementBreteau	1:f3f702086a30	918
ClementBreteau	1:f3f702086a30	919	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	920	printf(" ID : %d\n",TxBuf[2]);
ClementBreteau	1:f3f702086a30	921	#endif
ClementBreteau	1:f3f702086a30	922
ClementBreteau	1:f3f702086a30	923	// Packet Length
ClementBreteau	1:f3f702086a30	924	TxBuf[3] = 4;//PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
ClementBreteau	1:f3f702086a30	925	sum += TxBuf[3]; // Accululate the packet sum
ClementBreteau	1:f3f702086a30	926
ClementBreteau	1:f3f702086a30	927	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	928	printf(" Length : 0x%x\n",TxBuf[3]);
ClementBreteau	1:f3f702086a30	929	#endif
ClementBreteau	1:f3f702086a30	930
ClementBreteau	1:f3f702086a30	931	// Instruction - Read
ClementBreteau	1:f3f702086a30	932	TxBuf[4] = 0x2;
ClementBreteau	1:f3f702086a30	933	sum += TxBuf[4];
ClementBreteau	1:f3f702086a30	934
ClementBreteau	1:f3f702086a30	935	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	936	printf(" Instruction : 0x%x\n",TxBuf[4]);
ClementBreteau	1:f3f702086a30	937	#endif
ClementBreteau	1:f3f702086a30	938
ClementBreteau	1:f3f702086a30	939	// Start Address
ClementBreteau	1:f3f702086a30	940	TxBuf[5] = start;
ClementBreteau	1:f3f702086a30	941	sum += TxBuf[5];
ClementBreteau	1:f3f702086a30	942
ClementBreteau	1:f3f702086a30	943	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	944	printf(" Start Address : 0x%x\n",TxBuf[5]);
ClementBreteau	1:f3f702086a30	945	#endif
ClementBreteau	1:f3f702086a30	946
ClementBreteau	1:f3f702086a30	947	// Bytes to read
ClementBreteau	1:f3f702086a30	948	TxBuf[6] = bytes;
ClementBreteau	1:f3f702086a30	949	sum += TxBuf[6];
ClementBreteau	1:f3f702086a30	950
ClementBreteau	1:f3f702086a30	951	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	952	printf(" No bytes : 0x%x\n",TxBuf[6]);
ClementBreteau	1:f3f702086a30	953	#endif
ClementBreteau	1:f3f702086a30	954
ClementBreteau	1:f3f702086a30	955	// Checksum
ClementBreteau	1:f3f702086a30	956	TxBuf[7] = 0xFF - sum;
ClementBreteau	1:f3f702086a30	957	#ifdef AX12_READ_DEBUG
ClementBreteau	1:f3f702086a30	958	printf(" Checksum : 0x%x\n",TxBuf[7]);
ClementBreteau	1:f3f702086a30	959	#endif
ClementBreteau	1:f3f702086a30	960	/******************************************TRAME CONSTRUITE DANS TxBuf*************************************/
ClementBreteau	1:f3f702086a30	961
ClementBreteau	1:f3f702086a30	962
ClementBreteau	1:f3f702086a30	963
ClementBreteau	1:f3f702086a30	964
ClementBreteau	1:f3f702086a30	965	/* Transmission de la trame construite precedemment dans le tableau TxBuf
ClementBreteau	1:f3f702086a30	966	*/
ClementBreteau	1:f3f702086a30	967	while ((timeout_transmit<5000) && (i < (7+bytes)))
ClementBreteau	1:f3f702086a30	968	{
ClementBreteau	1:f3f702086a30	969	if (_ax12.writeable())
ClementBreteau	1:f3f702086a30	970	{
ClementBreteau	1:f3f702086a30	971	_ax12.putc(TxBuf[i]);
ClementBreteau	1:f3f702086a30	972	i++;
ClementBreteau	1:f3f702086a30	973	timeout_transmit = 0;
ClementBreteau	1:f3f702086a30	974	}
ClementBreteau	1:f3f702086a30	975	else timeout_transmit++;
ClementBreteau	1:f3f702086a30	976	}
ClementBreteau	1:f3f702086a30	977
ClementBreteau	1:f3f702086a30	978	if (timeout_transmit == 5000 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
ClementBreteau	1:f3f702086a30	979	{
ClementBreteau	1:f3f702086a30	980	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	981	printf ("timeout transmit erreur\r\n");
ClementBreteau	1:f3f702086a30	982	#endif
ClementBreteau	1:f3f702086a30	983	return(-1);
ClementBreteau	1:f3f702086a30	984	}
ClementBreteau	1:f3f702086a30	985	/* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
ClementBreteau	1:f3f702086a30	986	*/
ClementBreteau	1:f3f702086a30	987
ClementBreteau	1:f3f702086a30	988
ClementBreteau	1:f3f702086a30	989	// Wait for the bytes to be transmitted
ClementBreteau	1:f3f702086a30	990	wait (0.001);
ClementBreteau	1:f3f702086a30	991
ClementBreteau	1:f3f702086a30	992
ClementBreteau	1:f3f702086a30	993
ClementBreteau	1:f3f702086a30	994	// Skip if the read was to the broadcast address
ClementBreteau	1:f3f702086a30	995	if (_ID != 0xFE) {
ClementBreteau	1:f3f702086a30	996
ClementBreteau	1:f3f702086a30	997
ClementBreteau	1:f3f702086a30	998
ClementBreteau	1:f3f702086a30	999	/* Partie de reception de la trame de retour
ClementBreteau	1:f3f702086a30	1000	*/
ClementBreteau	1:f3f702086a30	1001	while ((flag_out != 1) && (timeout < (1000*bytes)))
ClementBreteau	1:f3f702086a30	1002	{
ClementBreteau	1:f3f702086a30	1003	// Les differents etats de l'automate on été créés au debut de la fonction write !
ClementBreteau	1:f3f702086a30	1004	switch (etat)
ClementBreteau	1:f3f702086a30	1005	{
ClementBreteau	1:f3f702086a30	1006	case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF )
ClementBreteau	1:f3f702086a30	1007	{
ClementBreteau	1:f3f702086a30	1008	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1009	timeout = 0;
ClementBreteau	1:f3f702086a30	1010	if (Status[plen] == 0xFF )
ClementBreteau	1:f3f702086a30	1011	{
ClementBreteau	1:f3f702086a30	1012	etat = Header2;
ClementBreteau	1:f3f702086a30	1013	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1014	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1015	#endif
ClementBreteau	1:f3f702086a30	1016	plen++;
ClementBreteau	1:f3f702086a30	1017
ClementBreteau	1:f3f702086a30	1018	}
ClementBreteau	1:f3f702086a30	1019	else etat = Header1;
ClementBreteau	1:f3f702086a30	1020	}
ClementBreteau	1:f3f702086a30	1021	else timeout++;
ClementBreteau	1:f3f702086a30	1022	break;
ClementBreteau	1:f3f702086a30	1023
ClementBreteau	1:f3f702086a30	1024
ClementBreteau	1:f3f702086a30	1025	case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF )
ClementBreteau	1:f3f702086a30	1026	{
ClementBreteau	1:f3f702086a30	1027	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1028	timeout = 0;
ClementBreteau	1:f3f702086a30	1029	if (Status[plen] == 0xFF )
ClementBreteau	1:f3f702086a30	1030	{
ClementBreteau	1:f3f702086a30	1031	etat = ident;
ClementBreteau	1:f3f702086a30	1032	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1033	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1034	#endif
ClementBreteau	1:f3f702086a30	1035	plen++;
ClementBreteau	1:f3f702086a30	1036
ClementBreteau	1:f3f702086a30	1037	}
ClementBreteau	1:f3f702086a30	1038	else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
ClementBreteau	1:f3f702086a30	1039	{
ClementBreteau	1:f3f702086a30	1040	Status[plen] = 0;
ClementBreteau	1:f3f702086a30	1041	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1042	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1043	#endif
ClementBreteau	1:f3f702086a30	1044	plen++;
ClementBreteau	1:f3f702086a30	1045	Status[plen] = ID;
ClementBreteau	1:f3f702086a30	1046	etat = length;
ClementBreteau	1:f3f702086a30	1047	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1048	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1049	#endif
ClementBreteau	1:f3f702086a30	1050	plen++;
ClementBreteau	1:f3f702086a30	1051
ClementBreteau	1:f3f702086a30	1052	}
ClementBreteau	1:f3f702086a30	1053	else
ClementBreteau	1:f3f702086a30	1054	{
ClementBreteau	1:f3f702086a30	1055
ClementBreteau	1:f3f702086a30	1056	etat = Header1;
ClementBreteau	1:f3f702086a30	1057	plen = 0;
ClementBreteau	1:f3f702086a30	1058	}
ClementBreteau	1:f3f702086a30	1059	}
ClementBreteau	1:f3f702086a30	1060	else timeout++;
ClementBreteau	1:f3f702086a30	1061	break;
ClementBreteau	1:f3f702086a30	1062
ClementBreteau	1:f3f702086a30	1063	case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur
ClementBreteau	1:f3f702086a30	1064	{
ClementBreteau	1:f3f702086a30	1065	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1066	timeout = 0;
ClementBreteau	1:f3f702086a30	1067	if (Status[plen] == ID )
ClementBreteau	1:f3f702086a30	1068	{
ClementBreteau	1:f3f702086a30	1069	etat = length;
ClementBreteau	1:f3f702086a30	1070	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1071	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1072	#endif
ClementBreteau	1:f3f702086a30	1073	plen++;
ClementBreteau	1:f3f702086a30	1074
ClementBreteau	1:f3f702086a30	1075	}
ClementBreteau	1:f3f702086a30	1076	else
ClementBreteau	1:f3f702086a30	1077	{
ClementBreteau	1:f3f702086a30	1078	etat = Header1;
ClementBreteau	1:f3f702086a30	1079	plen = 0;
ClementBreteau	1:f3f702086a30	1080	}
ClementBreteau	1:f3f702086a30	1081	}
ClementBreteau	1:f3f702086a30	1082	else timeout++;
ClementBreteau	1:f3f702086a30	1083	break;
ClementBreteau	1:f3f702086a30	1084
ClementBreteau	1:f3f702086a30	1085	case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
ClementBreteau	1:f3f702086a30	1086	{
ClementBreteau	1:f3f702086a30	1087	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1088	timeout = 0;
ClementBreteau	1:f3f702086a30	1089	if (Status[plen] == (bytes+2) )
ClementBreteau	1:f3f702086a30	1090	{
ClementBreteau	1:f3f702086a30	1091	etat = erreur;
ClementBreteau	1:f3f702086a30	1092	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1093	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1094	#endif
ClementBreteau	1:f3f702086a30	1095	plen++;
ClementBreteau	1:f3f702086a30	1096
ClementBreteau	1:f3f702086a30	1097	}
ClementBreteau	1:f3f702086a30	1098	else
ClementBreteau	1:f3f702086a30	1099	{
ClementBreteau	1:f3f702086a30	1100	etat = Header1;
ClementBreteau	1:f3f702086a30	1101	plen = 0;
ClementBreteau	1:f3f702086a30	1102	}
ClementBreteau	1:f3f702086a30	1103	}
ClementBreteau	1:f3f702086a30	1104	else timeout++;
ClementBreteau	1:f3f702086a30	1105	break;
ClementBreteau	1:f3f702086a30	1106
ClementBreteau	1:f3f702086a30	1107	case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
ClementBreteau	1:f3f702086a30	1108	{
ClementBreteau	1:f3f702086a30	1109	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1110	timeout = 0;
ClementBreteau	1:f3f702086a30	1111	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1112	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1113	#endif
ClementBreteau	1:f3f702086a30	1114	plen++;
ClementBreteau	1:f3f702086a30	1115
ClementBreteau	1:f3f702086a30	1116	etat = reception;
ClementBreteau	1:f3f702086a30	1117	}
ClementBreteau	1:f3f702086a30	1118	else timeout++;
ClementBreteau	1:f3f702086a30	1119
ClementBreteau	1:f3f702086a30	1120	case reception: while ( enable < bytes ) // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length )
ClementBreteau	1:f3f702086a30	1121	{
ClementBreteau	1:f3f702086a30	1122	if (_ax12.readable())
ClementBreteau	1:f3f702086a30	1123	{
ClementBreteau	1:f3f702086a30	1124	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1125	timeout = 0;
ClementBreteau	1:f3f702086a30	1126	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1127	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1128	#endif
ClementBreteau	1:f3f702086a30	1129	plen++;
ClementBreteau	1:f3f702086a30	1130	enable++;
ClementBreteau	1:f3f702086a30	1131
ClementBreteau	1:f3f702086a30	1132	}
ClementBreteau	1:f3f702086a30	1133	else timeout++;
ClementBreteau	1:f3f702086a30	1134	}
ClementBreteau	1:f3f702086a30	1135	etat = checksum;
ClementBreteau	1:f3f702086a30	1136	break;
ClementBreteau	1:f3f702086a30	1137
ClementBreteau	1:f3f702086a30	1138	case checksum: if (_ax12.readable()) // reception du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!!
ClementBreteau	1:f3f702086a30	1139	{
ClementBreteau	1:f3f702086a30	1140	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1141	timeout = 0;
ClementBreteau	1:f3f702086a30	1142	flag_out = 1;
ClementBreteau	1:f3f702086a30	1143	etat = Header1;
ClementBreteau	1:f3f702086a30	1144
ClementBreteau	1:f3f702086a30	1145	#ifdef AX12_DEBUG_READ
ClementBreteau	1:f3f702086a30	1146	printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1147	#endif
ClementBreteau	1:f3f702086a30	1148	}
ClementBreteau	1:f3f702086a30	1149	else timeout++;
ClementBreteau	1:f3f702086a30	1150	break;
ClementBreteau	1:f3f702086a30	1151
ClementBreteau	1:f3f702086a30	1152	default: break;
ClementBreteau	1:f3f702086a30	1153	}
ClementBreteau	1:f3f702086a30	1154	}
ClementBreteau	1:f3f702086a30	1155
ClementBreteau	1:f3f702086a30	1156
ClementBreteau	1:f3f702086a30	1157	if (timeout == (1000*bytes) ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
ClementBreteau	1:f3f702086a30	1158	{
ClementBreteau	1:f3f702086a30	1159	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1160	printf ("timeout erreur\n");
ClementBreteau	1:f3f702086a30	1161	#endif
ClementBreteau	1:f3f702086a30	1162	return(-1);
ClementBreteau	1:f3f702086a30	1163	}
ClementBreteau	1:f3f702086a30	1164
ClementBreteau	1:f3f702086a30	1165
ClementBreteau	1:f3f702086a30	1166	// copie des données dans le tableau data
ClementBreteau	1:f3f702086a30	1167	for (int i=0; i < Status[3]-2 ; i++)
ClementBreteau	1:f3f702086a30	1168	{
ClementBreteau	1:f3f702086a30	1169	data[i] = Status[5+i];
ClementBreteau	1:f3f702086a30	1170	}
ClementBreteau	1:f3f702086a30	1171
ClementBreteau	1:f3f702086a30	1172	} // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE)
ClementBreteau	1:f3f702086a30	1173
ClementBreteau	1:f3f702086a30	1174	return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
ClementBreteau	1:f3f702086a30	1175	}
ClementBreteau	1:f3f702086a30	1176
ClementBreteau	1:f3f702086a30	1177	void AX12::multiple_goal_and_speed(int number_ax12,char* tab)
ClementBreteau	1:f3f702086a30	1178	{
ClementBreteau	1:f3f702086a30	1179	char TxBuf[50];
ClementBreteau	1:f3f702086a30	1180	char sum = 0;
ClementBreteau	1:f3f702086a30	1181	int timeout_transmit =0;
ClementBreteau	1:f3f702086a30	1182	int i=0, k=0, j=0;
ClementBreteau	1:f3f702086a30	1183	int L=4; // nombre instructions par paquets
ClementBreteau	1:f3f702086a30	1184	int bytes= ((L+1)*number_ax12)+4;
ClementBreteau	1:f3f702086a30	1185
ClementBreteau	1:f3f702086a30	1186	typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
ClementBreteau	1:f3f702086a30	1187	type_etat etat= Header1;
ClementBreteau	1:f3f702086a30	1188
ClementBreteau	1:f3f702086a30	1189	for(j=0; j<50; j++)
ClementBreteau	1:f3f702086a30	1190	{
ClementBreteau	1:f3f702086a30	1191	TxBuf[i]=0;
ClementBreteau	1:f3f702086a30	1192	}
ClementBreteau	1:f3f702086a30	1193
ClementBreteau	1:f3f702086a30	1194	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1195	//printf(" MULTIPLE_GOAL_AND_SPEED \n ");
ClementBreteau	1:f3f702086a30	1196	#endif
ClementBreteau	1:f3f702086a30	1197
ClementBreteau	1:f3f702086a30	1198	// Build the TxPacket first in RAM, then we'll send in one go
ClementBreteau	1:f3f702086a30	1199	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1200	//printf("\nInstruction Packet\n Header :%d, %d\n",TxBuf[0], TxBuf[1]);
ClementBreteau	1:f3f702086a30	1201	#endif
ClementBreteau	1:f3f702086a30	1202
ClementBreteau	1:f3f702086a30	1203	TxBuf[0]=0xFF; // bit de start
ClementBreteau	1:f3f702086a30	1204	TxBuf[1]=0xFF; // bit de start
ClementBreteau	1:f3f702086a30	1205
ClementBreteau	1:f3f702086a30	1206	TxBuf[2] = 0xFE; //ID broadcast
ClementBreteau	1:f3f702086a30	1207	sum += TxBuf[2];
ClementBreteau	1:f3f702086a30	1208
ClementBreteau	1:f3f702086a30	1209	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1210	printf(" adresse de difusion : %d\n",TxBuf[2]);
ClementBreteau	1:f3f702086a30	1211	#endif
ClementBreteau	1:f3f702086a30	1212
ClementBreteau	1:f3f702086a30	1213	TxBuf[3] =bytes; // longueur
ClementBreteau	1:f3f702086a30	1214	sum += TxBuf[3];
ClementBreteau	1:f3f702086a30	1215
ClementBreteau	1:f3f702086a30	1216	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1217	printf(" Longueur : %d\n",TxBuf[3]);
ClementBreteau	1:f3f702086a30	1218	#endif
ClementBreteau	1:f3f702086a30	1219
ClementBreteau	1:f3f702086a30	1220	TxBuf[4]=0x83; //SYNC_WRITE
ClementBreteau	1:f3f702086a30	1221	sum += TxBuf[4];
ClementBreteau	1:f3f702086a30	1222
ClementBreteau	1:f3f702086a30	1223	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1224	printf(" Instruction : 0x%x\n",TxBuf[4]);
ClementBreteau	1:f3f702086a30	1225	#endif
ClementBreteau	1:f3f702086a30	1226
ClementBreteau	1:f3f702086a30	1227	TxBuf[5] = 0x1E; // addresse "GOAL_POSITION"
ClementBreteau	1:f3f702086a30	1228	sum += TxBuf[5];
ClementBreteau	1:f3f702086a30	1229
ClementBreteau	1:f3f702086a30	1230	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1231	printf(" Adresse de debut : 0x%x\n",TxBuf[5]);
ClementBreteau	1:f3f702086a30	1232	#endif
ClementBreteau	1:f3f702086a30	1233
ClementBreteau	1:f3f702086a30	1234	TxBuf[6]=L; // Nombre instruction par paquets
ClementBreteau	1:f3f702086a30	1235	sum += TxBuf[6];
ClementBreteau	1:f3f702086a30	1236
ClementBreteau	1:f3f702086a30	1237	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1238	printf(" nombre instruction/paquet : 0x%x\n",TxBuf[6]);
ClementBreteau	1:f3f702086a30	1239	#endif
ClementBreteau	1:f3f702086a30	1240
ClementBreteau	1:f3f702086a30	1241	for(i=0; i<(number_ax12*5); i++) // Copie des data de TAB sur TxBuf
ClementBreteau	1:f3f702086a30	1242	{
ClementBreteau	1:f3f702086a30	1243
ClementBreteau	1:f3f702086a30	1244	TxBuf[i+7]=tab[i];
ClementBreteau	1:f3f702086a30	1245	sum += TxBuf[i+7];
ClementBreteau	1:f3f702086a30	1246
ClementBreteau	1:f3f702086a30	1247	}
ClementBreteau	1:f3f702086a30	1248
ClementBreteau	1:f3f702086a30	1249	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1250	for(i=0; i<(number_ax12*5); i++)
ClementBreteau	1:f3f702086a30	1251	{
ClementBreteau	1:f3f702086a30	1252
ClementBreteau	1:f3f702086a30	1253	printf(" Data : 0x%x\n",TxBuf[i+7]);
ClementBreteau	1:f3f702086a30	1254
ClementBreteau	1:f3f702086a30	1255	}
ClementBreteau	1:f3f702086a30	1256	#endif
ClementBreteau	1:f3f702086a30	1257
ClementBreteau	1:f3f702086a30	1258	TxBuf[(number_ax12*5)+7] = 0xFF - sum ; // CHECKSUM
ClementBreteau	1:f3f702086a30	1259
ClementBreteau	1:f3f702086a30	1260	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1261	printf(" Checksum : 0x%x\n",TxBuf[(number_ax12*5)+9]);
ClementBreteau	1:f3f702086a30	1262	#endif
ClementBreteau	1:f3f702086a30	1263
ClementBreteau	1:f3f702086a30	1264	for(k=0; k<((number_ax12*5)+8); k++) // TRANSMISSION DE LA TRAME
ClementBreteau	1:f3f702086a30	1265	{
ClementBreteau	1:f3f702086a30	1266	_ax12.putc(TxBuf[k]);
ClementBreteau	1:f3f702086a30	1267	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1268	printf(" transmission : 0x%x\n",TxBuf[k]);
ClementBreteau	1:f3f702086a30	1269	#endif
ClementBreteau	1:f3f702086a30	1270	}
ClementBreteau	1:f3f702086a30	1271
ClementBreteau	1:f3f702086a30	1272
ClementBreteau	1:f3f702086a30	1273	}
ClementBreteau	1:f3f702086a30	1274
ClementBreteau	1:f3f702086a30	1275	float AX12::read_and_test(float angle,char* Tab)
ClementBreteau	1:f3f702086a30	1276	{
ClementBreteau	1:f3f702086a30	1277	int k=0;
ClementBreteau	1:f3f702086a30	1278	unsigned short val_angle=0, val_reche=0;
ClementBreteau	1:f3f702086a30	1279
ClementBreteau	1:f3f702086a30	1280	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1281	printf("\nread_and_test");
ClementBreteau	1:f3f702086a30	1282	#endif
ClementBreteau	1:f3f702086a30	1283
ClementBreteau	1:f3f702086a30	1284	if( _ID==0x12)
ClementBreteau	1:f3f702086a30	1285	{ k=1;}
ClementBreteau	1:f3f702086a30	1286	else if( _ID==0x04)
ClementBreteau	1:f3f702086a30	1287	{ k=6;}
ClementBreteau	1:f3f702086a30	1288	else if( _ID==0x07)
ClementBreteau	1:f3f702086a30	1289	{ k=11;}
ClementBreteau	1:f3f702086a30	1290	else if( _ID==0x0F)
ClementBreteau	1:f3f702086a30	1291	{ k=16;}
ClementBreteau	1:f3f702086a30	1292
ClementBreteau	1:f3f702086a30	1293	val_angle = (unsigned short) (angle/0.3);
ClementBreteau	1:f3f702086a30	1294	val_reche = (unsigned short) Tab[k] + ((unsigned short)Tab[k+1]<<8);
ClementBreteau	1:f3f702086a30	1295
ClementBreteau	1:f3f702086a30	1296	if((val_angle < (val_reche+(28))) && (val_angle > (val_reche-(28))))
ClementBreteau	1:f3f702086a30	1297	{
ClementBreteau	1:f3f702086a30	1298	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1299	printf("\nreturn1");
ClementBreteau	1:f3f702086a30	1300	#endif
ClementBreteau	1:f3f702086a30	1301	return 1;
ClementBreteau	1:f3f702086a30	1302	}
ClementBreteau	1:f3f702086a30	1303	else
ClementBreteau	1:f3f702086a30	1304	{
ClementBreteau	1:f3f702086a30	1305	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1306	printf("\nreturn0");
ClementBreteau	1:f3f702086a30	1307	#endif
ClementBreteau	1:f3f702086a30	1308	return 0;
ClementBreteau	1:f3f702086a30	1309	}
ClementBreteau	1:f3f702086a30	1310
ClementBreteau	1:f3f702086a30	1311	}
ClementBreteau	1:f3f702086a30	1312
ClementBreteau	1:f3f702086a30	1313	int AX12::write(int ID, int start, int bytes, char* data, int flag)
ClementBreteau	1:f3f702086a30	1314	{
ClementBreteau	1:f3f702086a30	1315	// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
ClementBreteau	1:f3f702086a30	1316
ClementBreteau	1:f3f702086a30	1317	char TxBuf[16];
ClementBreteau	1:f3f702086a30	1318	char sum = 0;
ClementBreteau	1:f3f702086a30	1319	char Status[6];
ClementBreteau	1:f3f702086a30	1320
ClementBreteau	1:f3f702086a30	1321	int timeout = 0;
ClementBreteau	1:f3f702086a30	1322	int plen = 0;
ClementBreteau	1:f3f702086a30	1323	int flag_out = 0;
ClementBreteau	1:f3f702086a30	1324	int timeout_transmit = 0;
ClementBreteau	1:f3f702086a30	1325	int i = 0;
ClementBreteau	1:f3f702086a30	1326	int poubelle = 0;
ClementBreteau	1:f3f702086a30	1327	int count = 0;
ClementBreteau	1:f3f702086a30	1328	char vidage[50];
ClementBreteau	1:f3f702086a30	1329
ClementBreteau	1:f3f702086a30	1330	typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
ClementBreteau	1:f3f702086a30	1331	type_etat etat = Header1;
ClementBreteau	1:f3f702086a30	1332
ClementBreteau	1:f3f702086a30	1333	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1334	printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
ClementBreteau	1:f3f702086a30	1335	#endif
ClementBreteau	1:f3f702086a30	1336
ClementBreteau	1:f3f702086a30	1337	// Build the TxPacket first in RAM, then we'll send in one go
ClementBreteau	1:f3f702086a30	1338	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1339	printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
ClementBreteau	1:f3f702086a30	1340	#endif
ClementBreteau	1:f3f702086a30	1341
ClementBreteau	1:f3f702086a30	1342	TxBuf[0] = 0xff;
ClementBreteau	1:f3f702086a30	1343	TxBuf[1] = 0xff;
ClementBreteau	1:f3f702086a30	1344
ClementBreteau	1:f3f702086a30	1345	// ID
ClementBreteau	1:f3f702086a30	1346	TxBuf[2] = ID;
ClementBreteau	1:f3f702086a30	1347	sum += TxBuf[2];
ClementBreteau	1:f3f702086a30	1348
ClementBreteau	1:f3f702086a30	1349	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1350	printf(" ID : %d\n",TxBuf[2]);
ClementBreteau	1:f3f702086a30	1351	#endif
ClementBreteau	1:f3f702086a30	1352
ClementBreteau	1:f3f702086a30	1353	// packet Length
ClementBreteau	1:f3f702086a30	1354	TxBuf[3] = 3+bytes;
ClementBreteau	1:f3f702086a30	1355	sum += TxBuf[3];
ClementBreteau	1:f3f702086a30	1356
ClementBreteau	1:f3f702086a30	1357	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1358	printf(" Length : %d\n",TxBuf[3]);
ClementBreteau	1:f3f702086a30	1359	#endif
ClementBreteau	1:f3f702086a30	1360
ClementBreteau	1:f3f702086a30	1361	// Instruction
ClementBreteau	1:f3f702086a30	1362	if (flag == 1) {
ClementBreteau	1:f3f702086a30	1363	TxBuf[4]=0x04;
ClementBreteau	1:f3f702086a30	1364	sum += TxBuf[4];
ClementBreteau	1:f3f702086a30	1365	} else {
ClementBreteau	1:f3f702086a30	1366	TxBuf[4]=0x03;
ClementBreteau	1:f3f702086a30	1367	sum += TxBuf[4];
ClementBreteau	1:f3f702086a30	1368	}
ClementBreteau	1:f3f702086a30	1369
ClementBreteau	1:f3f702086a30	1370	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1371	printf(" Instruction : 0x%x\n",TxBuf[4]);
ClementBreteau	1:f3f702086a30	1372	#endif
ClementBreteau	1:f3f702086a30	1373
ClementBreteau	1:f3f702086a30	1374	// Start Address
ClementBreteau	1:f3f702086a30	1375	TxBuf[5] = start;
ClementBreteau	1:f3f702086a30	1376	sum += TxBuf[5];
ClementBreteau	1:f3f702086a30	1377
ClementBreteau	1:f3f702086a30	1378	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1379	printf(" Start : 0x%x\n",TxBuf[5]);
ClementBreteau	1:f3f702086a30	1380	#endif
ClementBreteau	1:f3f702086a30	1381
ClementBreteau	1:f3f702086a30	1382	// data
ClementBreteau	1:f3f702086a30	1383	for (char i=0; i<bytes ; i++) {
ClementBreteau	1:f3f702086a30	1384	TxBuf[6+i] = data[i];
ClementBreteau	1:f3f702086a30	1385	sum += TxBuf[6+i];
ClementBreteau	1:f3f702086a30	1386
ClementBreteau	1:f3f702086a30	1387	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1388	printf(" Data : 0x%x\n",TxBuf[6+i]);
ClementBreteau	1:f3f702086a30	1389	#endif
ClementBreteau	1:f3f702086a30	1390
ClementBreteau	1:f3f702086a30	1391	}
ClementBreteau	1:f3f702086a30	1392
ClementBreteau	1:f3f702086a30	1393	// checksum
ClementBreteau	1:f3f702086a30	1394	TxBuf[6+bytes] = 0xFF - sum;
ClementBreteau	1:f3f702086a30	1395
ClementBreteau	1:f3f702086a30	1396	#ifdef AX12_WRITE_DEBUG
ClementBreteau	1:f3f702086a30	1397	printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
ClementBreteau	1:f3f702086a30	1398	#endif
ClementBreteau	1:f3f702086a30	1399
ClementBreteau	1:f3f702086a30	1400
ClementBreteau	1:f3f702086a30	1401	/* Transmission de la trame construite precedemment dans le tableau TxBuf
ClementBreteau	1:f3f702086a30	1402	*/
ClementBreteau	1:f3f702086a30	1403	while ((timeout_transmit<100) && (i < (7+bytes)))
ClementBreteau	1:f3f702086a30	1404	{
ClementBreteau	1:f3f702086a30	1405	if (_ax12.writeable())
ClementBreteau	1:f3f702086a30	1406	{
ClementBreteau	1:f3f702086a30	1407	_ax12.putc(TxBuf[i]);
ClementBreteau	1:f3f702086a30	1408	i++;
ClementBreteau	1:f3f702086a30	1409	timeout_transmit = 0;
ClementBreteau	1:f3f702086a30	1410	}
ClementBreteau	1:f3f702086a30	1411	else timeout_transmit++;
ClementBreteau	1:f3f702086a30	1412	}
ClementBreteau	1:f3f702086a30	1413
ClementBreteau	1:f3f702086a30	1414	if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
ClementBreteau	1:f3f702086a30	1415	{
ClementBreteau	1:f3f702086a30	1416	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1417	printf ("TIMEOUT TRANSMIT ERROR\r\n");
ClementBreteau	1:f3f702086a30	1418	#endif
ClementBreteau	1:f3f702086a30	1419	return(-1);
ClementBreteau	1:f3f702086a30	1420	}
ClementBreteau	1:f3f702086a30	1421	/* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
ClementBreteau	1:f3f702086a30	1422	*/
ClementBreteau	1:f3f702086a30	1423
ClementBreteau	1:f3f702086a30	1424
ClementBreteau	1:f3f702086a30	1425	// Wait for data to transmit
ClementBreteau	1:f3f702086a30	1426	wait (0.005);
ClementBreteau	1:f3f702086a30	1427
ClementBreteau	1:f3f702086a30	1428	// make sure we have a valid return
ClementBreteau	1:f3f702086a30	1429	Status[4]=0x00;
ClementBreteau	1:f3f702086a30	1430
ClementBreteau	1:f3f702086a30	1431	// we'll only get a reply if it was not broadcast
ClementBreteau	1:f3f702086a30	1432	if (_ID!=0xFE) {
ClementBreteau	1:f3f702086a30	1433
ClementBreteau	1:f3f702086a30	1434
ClementBreteau	1:f3f702086a30	1435	/* Partie de reception de la trame de retour
ClementBreteau	1:f3f702086a30	1436	*/
ClementBreteau	1:f3f702086a30	1437	while ((flag_out != 1) && (timeout < MAX_TIMEOUT))
ClementBreteau	1:f3f702086a30	1438	{
ClementBreteau	1:f3f702086a30	1439	// Les differents etats de l'automate on été créés au debut de la fonction write !
ClementBreteau	1:f3f702086a30	1440	switch (etat)
ClementBreteau	1:f3f702086a30	1441	{
ClementBreteau	1:f3f702086a30	1442	case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF )
ClementBreteau	1:f3f702086a30	1443	{
ClementBreteau	1:f3f702086a30	1444	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1445	timeout = 0;
ClementBreteau	1:f3f702086a30	1446	if (Status[plen] == 0xFF )
ClementBreteau	1:f3f702086a30	1447	{
ClementBreteau	1:f3f702086a30	1448	etat = Header2;
ClementBreteau	1:f3f702086a30	1449	#ifdef AX12_DEBUG_WRITE
ClementBreteau	1:f3f702086a30	1450	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1451	#endif
ClementBreteau	1:f3f702086a30	1452	plen++;
ClementBreteau	1:f3f702086a30	1453
ClementBreteau	1:f3f702086a30	1454	}
ClementBreteau	1:f3f702086a30	1455	else etat = Header1;
ClementBreteau	1:f3f702086a30	1456	}
ClementBreteau	1:f3f702086a30	1457	else timeout++;
ClementBreteau	1:f3f702086a30	1458	break;
ClementBreteau	1:f3f702086a30	1459
ClementBreteau	1:f3f702086a30	1460
ClementBreteau	1:f3f702086a30	1461	case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF )
ClementBreteau	1:f3f702086a30	1462	{
ClementBreteau	1:f3f702086a30	1463	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1464	timeout = 0;
ClementBreteau	1:f3f702086a30	1465	if (Status[plen] == 0xFF )
ClementBreteau	1:f3f702086a30	1466	{
ClementBreteau	1:f3f702086a30	1467	etat = ident;
ClementBreteau	1:f3f702086a30	1468	#ifdef AX12_DEBUG_WRITE
ClementBreteau	1:f3f702086a30	1469	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1470	#endif
ClementBreteau	1:f3f702086a30	1471	plen++;
ClementBreteau	1:f3f702086a30	1472	}
ClementBreteau	1:f3f702086a30	1473	else
ClementBreteau	1:f3f702086a30	1474	{
ClementBreteau	1:f3f702086a30	1475	etat = Header1;
ClementBreteau	1:f3f702086a30	1476	plen = 0;
ClementBreteau	1:f3f702086a30	1477	}
ClementBreteau	1:f3f702086a30	1478	}
ClementBreteau	1:f3f702086a30	1479	else timeout++;
ClementBreteau	1:f3f702086a30	1480	break;
ClementBreteau	1:f3f702086a30	1481
ClementBreteau	1:f3f702086a30	1482	case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur
ClementBreteau	1:f3f702086a30	1483	{
ClementBreteau	1:f3f702086a30	1484	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1485	timeout = 0;
ClementBreteau	1:f3f702086a30	1486	if (Status[plen] == ID )
ClementBreteau	1:f3f702086a30	1487	{
ClementBreteau	1:f3f702086a30	1488	etat = length;
ClementBreteau	1:f3f702086a30	1489	#ifdef AX12_DEBUG_WRITE
ClementBreteau	1:f3f702086a30	1490	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1491	#endif
ClementBreteau	1:f3f702086a30	1492	plen++;
ClementBreteau	1:f3f702086a30	1493	}
ClementBreteau	1:f3f702086a30	1494	else
ClementBreteau	1:f3f702086a30	1495	{
ClementBreteau	1:f3f702086a30	1496	etat = Header1;
ClementBreteau	1:f3f702086a30	1497	plen = 0;
ClementBreteau	1:f3f702086a30	1498	}
ClementBreteau	1:f3f702086a30	1499	}
ClementBreteau	1:f3f702086a30	1500	else timeout++;
ClementBreteau	1:f3f702086a30	1501	break;
ClementBreteau	1:f3f702086a30	1502
ClementBreteau	1:f3f702086a30	1503	case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
ClementBreteau	1:f3f702086a30	1504	{
ClementBreteau	1:f3f702086a30	1505	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1506	timeout = 0;
ClementBreteau	1:f3f702086a30	1507	if (Status[plen] == 2 ) // dans la trame de retour d'un write il n'y a pas de paramètre la taille vaudra donc 2!!
ClementBreteau	1:f3f702086a30	1508	{
ClementBreteau	1:f3f702086a30	1509	etat = erreur;
ClementBreteau	1:f3f702086a30	1510	#ifdef AX12_DEBUG_WRITE
ClementBreteau	1:f3f702086a30	1511	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1512	#endif
ClementBreteau	1:f3f702086a30	1513	plen++;
ClementBreteau	1:f3f702086a30	1514	}
ClementBreteau	1:f3f702086a30	1515	else
ClementBreteau	1:f3f702086a30	1516	{
ClementBreteau	1:f3f702086a30	1517	etat = Header1;
ClementBreteau	1:f3f702086a30	1518	plen = 0;
ClementBreteau	1:f3f702086a30	1519	}
ClementBreteau	1:f3f702086a30	1520	}
ClementBreteau	1:f3f702086a30	1521	else timeout++;
ClementBreteau	1:f3f702086a30	1522	break;
ClementBreteau	1:f3f702086a30	1523
ClementBreteau	1:f3f702086a30	1524	case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
ClementBreteau	1:f3f702086a30	1525	{
ClementBreteau	1:f3f702086a30	1526	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1527	timeout = 0;
ClementBreteau	1:f3f702086a30	1528	#ifdef AX12_DEBUG_WRITE
ClementBreteau	1:f3f702086a30	1529	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1530	#endif
ClementBreteau	1:f3f702086a30	1531	plen++;
ClementBreteau	1:f3f702086a30	1532	etat = checksum;
ClementBreteau	1:f3f702086a30	1533	}
ClementBreteau	1:f3f702086a30	1534	else timeout++;
ClementBreteau	1:f3f702086a30	1535
ClementBreteau	1:f3f702086a30	1536	case checksum: if (_ax12.readable()) // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write
ClementBreteau	1:f3f702086a30	1537	{
ClementBreteau	1:f3f702086a30	1538	Status[plen] = _ax12.getc();
ClementBreteau	1:f3f702086a30	1539	timeout = 0;
ClementBreteau	1:f3f702086a30	1540	flag_out = 1;
ClementBreteau	1:f3f702086a30	1541	etat = Header1;
ClementBreteau	1:f3f702086a30	1542	#ifdef AX12_DEBUG_WRITE
ClementBreteau	1:f3f702086a30	1543	printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1544	#endif
ClementBreteau	1:f3f702086a30	1545	}
ClementBreteau	1:f3f702086a30	1546	else timeout++;
ClementBreteau	1:f3f702086a30	1547	break;
ClementBreteau	1:f3f702086a30	1548	}
ClementBreteau	1:f3f702086a30	1549	}
ClementBreteau	1:f3f702086a30	1550
ClementBreteau	1:f3f702086a30	1551
ClementBreteau	1:f3f702086a30	1552	if ( Status[4] != 0 )
ClementBreteau	1:f3f702086a30	1553	{
ClementBreteau	1:f3f702086a30	1554	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1555	printf ("erreur ! \r\n");
ClementBreteau	1:f3f702086a30	1556	#endif
ClementBreteau	1:f3f702086a30	1557	for (int i = 0; i<5; i++)
ClementBreteau	1:f3f702086a30	1558	{
ClementBreteau	1:f3f702086a30	1559	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1560	printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
ClementBreteau	1:f3f702086a30	1561	#endif
ClementBreteau	1:f3f702086a30	1562	}
ClementBreteau	1:f3f702086a30	1563	}
ClementBreteau	1:f3f702086a30	1564
ClementBreteau	1:f3f702086a30	1565	if (timeout == MAX_TIMEOUT ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
ClementBreteau	1:f3f702086a30	1566	{
ClementBreteau	1:f3f702086a30	1567	#ifdef AX12_DEBUG
ClementBreteau	1:f3f702086a30	1568	printf ("timeout erreur\n\r");
ClementBreteau	1:f3f702086a30	1569	#endif
ClementBreteau	1:f3f702086a30	1570	return(-1);
ClementBreteau	1:f3f702086a30	1571	}
ClementBreteau	1:f3f702086a30	1572
ClementBreteau	1:f3f702086a30	1573	// Build the TxPacket first in RAM, then we'll send in one go
ClementBreteau	1:f3f702086a30	1574	}
ClementBreteau	1:f3f702086a30	1575
ClementBreteau	1:f3f702086a30	1576	return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
ClementBreteau	1:f3f702086a30	1577	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Created:	20 May 2017
Imports:	1
Forks:	1
Commits:	4
Dependents:	0
Dependencies:	0
Followers:	96

AX12/AX12.cpp@3:1bb26049bdd1, 2017-05-20 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning