command_AX12_petit_robot_V4tempo - code ax12 petit robot 12/05/2017

CRAC Team » Code » command_AX12_petit_robot_V4tempo

CRAC Team / command_AX12_petit_robot_V4tempo

code ax12 petit robot 12/05/2017

Fork of command_AX12_petit_robot_V3 by CRAC Team

AX12/AX12.cpp@7:ad4a19e26b84, 2017-05-12 (annotated)

Committer:: ClementBreteau
Date:: Fri May 12 14:35:09 2017 +0000
Revision:: 7:ad4a19e26b84
Parent:: 4:888b32bc5b63

position ax12

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Created:	12 May 2017
Imports:	1
Forks:	0
Commits:	8
Dependents:	0
Dependencies:	0
Followers:	96

AX12/AX12.cpp@7:ad4a19e26b84, 2017-05-12 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning