AX12_final - Debugged library

Team DIANA » Code » AX12_final

Team DIANA / AX12_final

Debugged library

Dependents: MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale

Fork of AX12_v2 by Team DIANA

AX12.cpp@1:93ad80f5fde7, 2011-03-30 (annotated)

Committer:: chris
Date:: Wed Mar 30 16:08:02 2011 +0000
Revision:: 1:93ad80f5fde7
Parent:: 0:be51952765ec
Child:: 2:5ea99c37a2d7
Child:: 4:36f451ee0a3c

Who changed what in which revision?

User	Revision	Line number	New contents of line
chris	0:be51952765ec	1	/* mbed AX-12+ Servo Library
chris	0:be51952765ec	2	*
chris	0:be51952765ec	3	* Copyright (c) 2010, cstyles (http://mbed.org)
chris	0:be51952765ec	4	*
chris	0:be51952765ec	5	* Permission is hereby granted, free of charge, to any person obtaining a copy
chris	0:be51952765ec	6	* of this software and associated documentation files (the "Software"), to deal
chris	0:be51952765ec	7	* in the Software without restriction, including without limitation the rights
chris	0:be51952765ec	8	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
chris	0:be51952765ec	9	* copies of the Software, and to permit persons to whom the Software is
chris	0:be51952765ec	10	* furnished to do so, subject to the following conditions:
chris	0:be51952765ec	11	*
chris	0:be51952765ec	12	* The above copyright notice and this permission notice shall be included in
chris	0:be51952765ec	13	* all copies or substantial portions of the Software.
chris	0:be51952765ec	14	*
chris	0:be51952765ec	15	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
chris	0:be51952765ec	16	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
chris	0:be51952765ec	17	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
chris	0:be51952765ec	18	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
chris	0:be51952765ec	19	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
chris	0:be51952765ec	20	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
chris	0:be51952765ec	21	* THE SOFTWARE.
chris	0:be51952765ec	22	*/
chris	0:be51952765ec	23
chris	0:be51952765ec	24	#include "AX12.h"
chris	0:be51952765ec	25	#include "mbed.h"
chris	0:be51952765ec	26
chris	0:be51952765ec	27	AX12::AX12(PinName tx, PinName rx, int ID)
chris	0:be51952765ec	28	: _ax12(tx,rx) {
chris	0:be51952765ec	29
chris	0:be51952765ec	30	_ax12.baud(1000000);
chris	0:be51952765ec	31	_ID = ID;
chris	0:be51952765ec	32	}
chris	0:be51952765ec	33
chris	1:93ad80f5fde7	34	// Set the mode of the servo
chris	1:93ad80f5fde7	35	// 0 = Positional (0-300 degrees)
chris	1:93ad80f5fde7	36	// 1 = Rotational -1 to 1 speed
chris	1:93ad80f5fde7	37	int AX12::SetMode(int mode) {
chris	1:93ad80f5fde7	38
chris	1:93ad80f5fde7	39	if (mode == 1) { // set CR
chris	1:93ad80f5fde7	40	SetCWLimit(0);
chris	1:93ad80f5fde7	41	SetCCWLimit(0);
chris	1:93ad80f5fde7	42	SetCRSpeed(0.0);
chris	1:93ad80f5fde7	43	} else {
chris	1:93ad80f5fde7	44	SetCWLimit(0);
chris	1:93ad80f5fde7	45	SetCCWLimit(300);
chris	1:93ad80f5fde7	46	SetCRSpeed(0.0);
chris	1:93ad80f5fde7	47	}
chris	1:93ad80f5fde7	48	return(0);
chris	1:93ad80f5fde7	49	}
chris	1:93ad80f5fde7	50
chris	1:93ad80f5fde7	51
chris	1:93ad80f5fde7	52	// if flag[0] is set, were blocking
chris	1:93ad80f5fde7	53	// if flag[1] is set, we're registering
chris	1:93ad80f5fde7	54	// they are mutually exclusive operations
chris	1:93ad80f5fde7	55	int AX12::SetGoal(int degrees, int flags) {
chris	0:be51952765ec	56
chris	1:93ad80f5fde7	57	char reg_flag = 0;
chris	1:93ad80f5fde7	58	char data[2];
chris	1:93ad80f5fde7	59
chris	1:93ad80f5fde7	60	// set the flag is only the register bit is set in the flag
chris	1:93ad80f5fde7	61	if (flags == 0x2) {
chris	1:93ad80f5fde7	62	reg_flag = 1;
chris	1:93ad80f5fde7	63	}
chris	1:93ad80f5fde7	64
chris	1:93ad80f5fde7	65	// 1023 / 300 * degrees
chris	1:93ad80f5fde7	66	short goal = (1023 * degrees) / 300;
chris	1:93ad80f5fde7	67	if (AX12_DEBUG) {
chris	1:93ad80f5fde7	68	printf("SetGoal to 0x%x\n",goal);
chris	1:93ad80f5fde7	69	}
chris	1:93ad80f5fde7	70
chris	1:93ad80f5fde7	71	data[0] = goal & 0xff; // bottom 8 bits
chris	1:93ad80f5fde7	72	data[1] = goal >> 8; // top 8 bits
chris	1:93ad80f5fde7	73
chris	1:93ad80f5fde7	74	// write the packet, return the error code
chris	1:93ad80f5fde7	75	int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
chris	1:93ad80f5fde7	76
chris	1:93ad80f5fde7	77	if (flags == 1) {
chris	1:93ad80f5fde7	78	// block until it comes to a halt
chris	1:93ad80f5fde7	79	while (isMoving()) {}
chris	1:93ad80f5fde7	80	}
chris	1:93ad80f5fde7	81	return(rVal);
chris	0:be51952765ec	82	}
chris	0:be51952765ec	83
chris	1:93ad80f5fde7	84
chris	0:be51952765ec	85	// Set continuous rotation speed from -1 to 1
chris	0:be51952765ec	86	int AX12::SetCRSpeed(float speed) {
chris	0:be51952765ec	87
chris	0:be51952765ec	88	// bit 10 = direction, 0 = CCW, 1=CW
chris	0:be51952765ec	89	// bits 9-0 = Speed
chris	0:be51952765ec	90	char data[2];
chris	0:be51952765ec	91
chris	0:be51952765ec	92	int goal = (0x3ff * abs(speed));
chris	0:be51952765ec	93
chris	0:be51952765ec	94	// Set direction CW if we have a negative speed
chris	0:be51952765ec	95	if (speed < 0) {
chris	0:be51952765ec	96	goal \|= (0x1 << 10);
chris	0:be51952765ec	97	}
chris	0:be51952765ec	98
chris	0:be51952765ec	99	data[0] = goal & 0xff; // bottom 8 bits
chris	0:be51952765ec	100	data[1] = goal >> 8; // top 8 bits
chris	0:be51952765ec	101
chris	0:be51952765ec	102	// write the packet, return the error code
chris	0:be51952765ec	103	int rVal = write(_ID, 0x20, 2, data);
chris	0:be51952765ec	104
chris	0:be51952765ec	105	return(rVal);
chris	0:be51952765ec	106	}
chris	0:be51952765ec	107
chris	1:93ad80f5fde7	108
chris	1:93ad80f5fde7	109	int AX12::SetCWLimit (int degrees) {
chris	1:93ad80f5fde7	110
chris	1:93ad80f5fde7	111	char data[2];
chris	1:93ad80f5fde7	112
chris	1:93ad80f5fde7	113	// 1023 / 300 * degrees
chris	1:93ad80f5fde7	114	short limit = (1023 * degrees) / 300;
chris	0:be51952765ec	115
chris	1:93ad80f5fde7	116	if (AX12_DEBUG) {
chris	1:93ad80f5fde7	117	printf("SetCWLimit to 0x%x\n",limit);
chris	0:be51952765ec	118	}
chris	1:93ad80f5fde7	119
chris	1:93ad80f5fde7	120	data[0] = limit & 0xff; // bottom 8 bits
chris	1:93ad80f5fde7	121	data[1] = limit >> 8; // top 8 bits
chris	1:93ad80f5fde7	122
chris	1:93ad80f5fde7	123	// write the packet, return the error code
chris	1:93ad80f5fde7	124	return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
chris	1:93ad80f5fde7	125
chris	0:be51952765ec	126	}
chris	0:be51952765ec	127
chris	1:93ad80f5fde7	128	int AX12::SetCCWLimit (int degrees) {
chris	1:93ad80f5fde7	129
chris	1:93ad80f5fde7	130	char data[2];
chris	1:93ad80f5fde7	131
chris	1:93ad80f5fde7	132	// 1023 / 300 * degrees
chris	1:93ad80f5fde7	133	short limit = (1023 * degrees) / 300;
chris	0:be51952765ec	134
chris	0:be51952765ec	135	if (AX12_DEBUG) {
chris	1:93ad80f5fde7	136	printf("SetCCWLimit to 0x%x\n",limit);
chris	0:be51952765ec	137	}
chris	1:93ad80f5fde7	138
chris	1:93ad80f5fde7	139	data[0] = limit & 0xff; // bottom 8 bits
chris	1:93ad80f5fde7	140	data[1] = limit >> 8; // top 8 bits
chris	1:93ad80f5fde7	141
chris	1:93ad80f5fde7	142	// write the packet, return the error code
chris	1:93ad80f5fde7	143	return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
chris	0:be51952765ec	144	}
chris	0:be51952765ec	145
chris	1:93ad80f5fde7	146
chris	1:93ad80f5fde7	147	int AX12::SetID (int CurrentID, int NewID) {
chris	1:93ad80f5fde7	148
chris	0:be51952765ec	149	char data[1];
chris	1:93ad80f5fde7	150	data[0] = NewID;
chris	1:93ad80f5fde7	151	if (AX12_DEBUG) {
chris	1:93ad80f5fde7	152	printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
chris	1:93ad80f5fde7	153	}
chris	1:93ad80f5fde7	154	return (write(CurrentID, AX12_REG_ID, 1, data));
chris	1:93ad80f5fde7	155
chris	0:be51952765ec	156	}
chris	0:be51952765ec	157
chris	1:93ad80f5fde7	158
chris	1:93ad80f5fde7	159	// return 1 is the servo is still in flight
chris	1:93ad80f5fde7	160	int AX12::isMoving(void) {
chris	1:93ad80f5fde7	161
chris	1:93ad80f5fde7	162	char data[1];
chris	1:93ad80f5fde7	163	read(_ID,AX12_REG_MOVING,1,data);
chris	1:93ad80f5fde7	164	return(data[0]);
chris	1:93ad80f5fde7	165	}
chris	1:93ad80f5fde7	166
chris	1:93ad80f5fde7	167
chris	0:be51952765ec	168	void AX12::trigger(void) {
chris	0:be51952765ec	169
chris	0:be51952765ec	170	char TxBuf[16];
chris	0:be51952765ec	171	char sum = 0;
chris	0:be51952765ec	172
chris	0:be51952765ec	173	if (AX12_TRIGGER_DEBUG) {
chris	0:be51952765ec	174	printf("\nTriggered\n");
chris	0:be51952765ec	175	}
chris	0:be51952765ec	176
chris	0:be51952765ec	177	// Build the TxPacket first in RAM, then we'll send in one go
chris	0:be51952765ec	178	if (AX12_TRIGGER_DEBUG) {
chris	0:be51952765ec	179	printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
chris	0:be51952765ec	180	}
chris	0:be51952765ec	181
chris	0:be51952765ec	182	TxBuf[0] = 0xFF;
chris	0:be51952765ec	183	TxBuf[1] = 0xFF;
chris	0:be51952765ec	184
chris	0:be51952765ec	185	// ID - Broadcast
chris	0:be51952765ec	186	TxBuf[2] = 0xFE;
chris	0:be51952765ec	187	sum += TxBuf[2];
chris	0:be51952765ec	188
chris	0:be51952765ec	189	if (AX12_TRIGGER_DEBUG) {
chris	0:be51952765ec	190	printf(" ID : %d\n",TxBuf[2]);
chris	0:be51952765ec	191	}
chris	0:be51952765ec	192
chris	0:be51952765ec	193	// Length
chris	0:be51952765ec	194	TxBuf[3] = 0x02;
chris	0:be51952765ec	195	sum += TxBuf[3];
chris	0:be51952765ec	196	if (AX12_TRIGGER_DEBUG) {
chris	0:be51952765ec	197	printf(" Length %d\n",TxBuf[3]);
chris	0:be51952765ec	198	}
chris	0:be51952765ec	199
chris	0:be51952765ec	200	// Instruction - ACTION
chris	0:be51952765ec	201	TxBuf[4] = 0x04;
chris	0:be51952765ec	202	sum += TxBuf[4];
chris	0:be51952765ec	203	if (AX12_TRIGGER_DEBUG) {
chris	0:be51952765ec	204	printf(" Instruction 0x%X\n",TxBuf[5]);
chris	0:be51952765ec	205	}
chris	0:be51952765ec	206
chris	0:be51952765ec	207	// Checksum
chris	0:be51952765ec	208	TxBuf[5] = 0xFF - sum;
chris	0:be51952765ec	209	if (AX12_TRIGGER_DEBUG) {
chris	0:be51952765ec	210	printf(" Checksum 0x%X\n",TxBuf[5]);
chris	0:be51952765ec	211	}
chris	0:be51952765ec	212
chris	0:be51952765ec	213	// Transmit the packet in one burst with no pausing
chris	0:be51952765ec	214	for (int i = 0; i < 6 ; i++) {
chris	0:be51952765ec	215	_ax12.putc(TxBuf[i]);
chris	0:be51952765ec	216	}
chris	1:93ad80f5fde7	217
chris	0:be51952765ec	218	// This is a broadcast packet, so there will be no reply
chris	0:be51952765ec	219
chris	0:be51952765ec	220	return;
chris	0:be51952765ec	221	}
chris	0:be51952765ec	222
chris	0:be51952765ec	223
chris	0:be51952765ec	224	float AX12::GetPosition(void) {
chris	0:be51952765ec	225
chris	0:be51952765ec	226	if (AX12_DEBUG) {
chris	0:be51952765ec	227	printf("\nGetPosition(%d)",_ID);
chris	0:be51952765ec	228	}
chris	0:be51952765ec	229
chris	0:be51952765ec	230	char data[2];
chris	0:be51952765ec	231
chris	0:be51952765ec	232	int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
chris	0:be51952765ec	233	short position = data[0] + (data[1] << 8);
chris	0:be51952765ec	234	float angle = (position * 300)/1024;
chris	0:be51952765ec	235
chris	0:be51952765ec	236	return (angle);
chris	0:be51952765ec	237	}
chris	0:be51952765ec	238
chris	0:be51952765ec	239
chris	1:93ad80f5fde7	240	float AX12::GetTemp (void) {
chris	0:be51952765ec	241
chris	0:be51952765ec	242	if (AX12_DEBUG) {
chris	1:93ad80f5fde7	243	printf("\nGetTemp(%d)",_ID);
chris	0:be51952765ec	244	}
chris	1:93ad80f5fde7	245	char data[1];
chris	1:93ad80f5fde7	246	int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
chris	1:93ad80f5fde7	247	float temp = data[0];
chris	1:93ad80f5fde7	248	return(temp);
chris	0:be51952765ec	249	}
chris	0:be51952765ec	250
chris	0:be51952765ec	251
chris	1:93ad80f5fde7	252	float AX12::GetVolts (void) {
chris	0:be51952765ec	253	if (AX12_DEBUG) {
chris	1:93ad80f5fde7	254	printf("\nGetVolts(%d)",_ID);
chris	0:be51952765ec	255	}
chris	0:be51952765ec	256	char data[1];
chris	1:93ad80f5fde7	257	int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
chris	1:93ad80f5fde7	258	float volts = data[0]/10.0;
chris	1:93ad80f5fde7	259	return(volts);
chris	0:be51952765ec	260	}
chris	0:be51952765ec	261
chris	0:be51952765ec	262
chris	0:be51952765ec	263	int AX12::read(int ID, int start, int bytes, char* data) {
chris	0:be51952765ec	264
chris	0:be51952765ec	265	char PacketLength = 0x4;
chris	0:be51952765ec	266	char TxBuf[16];
chris	0:be51952765ec	267	char sum = 0;
chris	0:be51952765ec	268	char Status[16];
chris	0:be51952765ec	269
chris	0:be51952765ec	270	Status[4] = 0xFE; // return code
chris	0:be51952765ec	271
chris	0:be51952765ec	272	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	273	printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
chris	0:be51952765ec	274	}
chris	0:be51952765ec	275
chris	0:be51952765ec	276	// Build the TxPacket first in RAM, then we'll send in one go
chris	0:be51952765ec	277	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	278	printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
chris	0:be51952765ec	279	}
chris	0:be51952765ec	280
chris	0:be51952765ec	281	TxBuf[0] = 0xff;
chris	0:be51952765ec	282	TxBuf[1] = 0xff;
chris	0:be51952765ec	283
chris	0:be51952765ec	284	// ID
chris	0:be51952765ec	285	TxBuf[2] = ID;
chris	0:be51952765ec	286	sum += TxBuf[2];
chris	0:be51952765ec	287	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	288	printf(" ID : %d\n",TxBuf[2]);
chris	0:be51952765ec	289	}
chris	0:be51952765ec	290
chris	0:be51952765ec	291	// Packet Length
chris	0:be51952765ec	292	TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
chris	0:be51952765ec	293	sum += TxBuf[3]; // Accululate the packet sum
chris	0:be51952765ec	294	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	295	printf(" Length : 0x%x\n",TxBuf[3]);
chris	0:be51952765ec	296	}
chris	0:be51952765ec	297
chris	0:be51952765ec	298	// Instruction - Read
chris	0:be51952765ec	299	TxBuf[4] = 0x2;
chris	0:be51952765ec	300	sum += TxBuf[4];
chris	0:be51952765ec	301	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	302	printf(" Instruction : 0x%x\n",TxBuf[4]);
chris	0:be51952765ec	303	}
chris	0:be51952765ec	304
chris	0:be51952765ec	305	// Start Address
chris	0:be51952765ec	306	TxBuf[5] = start;
chris	0:be51952765ec	307	sum += TxBuf[5];
chris	0:be51952765ec	308	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	309	printf(" Start Address : 0x%x\n",TxBuf[5]);
chris	0:be51952765ec	310	}
chris	0:be51952765ec	311
chris	0:be51952765ec	312	// Bytes to read
chris	0:be51952765ec	313	TxBuf[6] = bytes;
chris	0:be51952765ec	314	sum += TxBuf[6];
chris	0:be51952765ec	315	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	316	printf(" No bytes : 0x%x\n",TxBuf[6]);
chris	0:be51952765ec	317	}
chris	0:be51952765ec	318
chris	0:be51952765ec	319	// Checksum
chris	0:be51952765ec	320	TxBuf[7] = 0xFF - sum;
chris	0:be51952765ec	321	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	322	printf(" Checksum : 0x%x\n",TxBuf[7]);
chris	0:be51952765ec	323	}
chris	0:be51952765ec	324
chris	0:be51952765ec	325	// Transmit the packet in one burst with no pausing
chris	0:be51952765ec	326	for (int i = 0; i<8 ; i++) {
chris	0:be51952765ec	327	_ax12.putc(TxBuf[i]);
chris	0:be51952765ec	328	}
chris	0:be51952765ec	329
chris	0:be51952765ec	330	// Wait for the bytes to be transmitted
chris	0:be51952765ec	331	wait (0.00002);
chris	0:be51952765ec	332
chris	0:be51952765ec	333	// Skip if the read was to the broadcast address
chris	0:be51952765ec	334	if (_ID != 0xFE) {
chris	0:be51952765ec	335
chris	0:be51952765ec	336	// Receive the Status packet 6+ number of bytes read
chris	0:be51952765ec	337	for (int i=0; i<(6+bytes) ; i++) {
chris	0:be51952765ec	338	Status[i] = _ax12.getc();
chris	0:be51952765ec	339	}
chris	0:be51952765ec	340
chris	0:be51952765ec	341	// Copy the data from Status into data for return
chris	0:be51952765ec	342	for (int i=0; i < Status[3]-2 ; i++) {
chris	0:be51952765ec	343	data[i] = Status[5+i];
chris	0:be51952765ec	344	}
chris	0:be51952765ec	345
chris	0:be51952765ec	346	if (AX12_READ_DEBUG) {
chris	0:be51952765ec	347	printf("\nStatus Packet\n");
chris	0:be51952765ec	348	printf(" Header : 0x%x\n",Status[0]);
chris	0:be51952765ec	349	printf(" Header : 0x%x\n",Status[1]);
chris	0:be51952765ec	350	printf(" ID : 0x%x\n",Status[2]);
chris	0:be51952765ec	351	printf(" Length : 0x%x\n",Status[3]);
chris	0:be51952765ec	352	printf(" Error Code : 0x%x\n",Status[4]);
chris	0:be51952765ec	353
chris	0:be51952765ec	354	for (int i=0; i < Status[3]-2 ; i++) {
chris	0:be51952765ec	355	printf(" Data : 0x%x\n",Status[5+i]);
chris	0:be51952765ec	356	}
chris	0:be51952765ec	357
chris	0:be51952765ec	358	printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
chris	0:be51952765ec	359	}
chris	0:be51952765ec	360
chris	0:be51952765ec	361	} // if (ID!=0xFE)
chris	0:be51952765ec	362
chris	0:be51952765ec	363	return(Status[4]);
chris	0:be51952765ec	364	}
chris	0:be51952765ec	365
chris	1:93ad80f5fde7	366
chris	0:be51952765ec	367	int AX12:: write(int ID, int start, int bytes, char* data, int flag) {
chris	0:be51952765ec	368	// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
chris	0:be51952765ec	369
chris	0:be51952765ec	370	char TxBuf[16];
chris	0:be51952765ec	371	char sum = 0;
chris	0:be51952765ec	372	char Status[6];
chris	0:be51952765ec	373
chris	0:be51952765ec	374	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	375	printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
chris	0:be51952765ec	376	}
chris	0:be51952765ec	377
chris	0:be51952765ec	378	// Build the TxPacket first in RAM, then we'll send in one go
chris	0:be51952765ec	379	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	380	printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
chris	0:be51952765ec	381	}
chris	0:be51952765ec	382
chris	0:be51952765ec	383	TxBuf[0] = 0xff;
chris	0:be51952765ec	384	TxBuf[1] = 0xff;
chris	0:be51952765ec	385
chris	0:be51952765ec	386	// ID
chris	0:be51952765ec	387	TxBuf[2] = ID;
chris	0:be51952765ec	388	sum += TxBuf[2];
chris	0:be51952765ec	389
chris	0:be51952765ec	390	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	391	printf(" ID : %d\n",TxBuf[2]);
chris	0:be51952765ec	392	}
chris	0:be51952765ec	393
chris	0:be51952765ec	394	// packet Length
chris	0:be51952765ec	395	TxBuf[3] = 3+bytes;
chris	0:be51952765ec	396	sum += TxBuf[3];
chris	0:be51952765ec	397
chris	0:be51952765ec	398	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	399	printf(" Length : %d\n",TxBuf[3]);
chris	0:be51952765ec	400	}
chris	0:be51952765ec	401
chris	0:be51952765ec	402	// Instruction
chris	0:be51952765ec	403	if (flag == 1) {
chris	0:be51952765ec	404	TxBuf[4]=0x04;
chris	0:be51952765ec	405	sum += TxBuf[4];
chris	0:be51952765ec	406	} else {
chris	0:be51952765ec	407	TxBuf[4]=0x03;
chris	0:be51952765ec	408	sum += TxBuf[4];
chris	0:be51952765ec	409	}
chris	0:be51952765ec	410
chris	0:be51952765ec	411	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	412	printf(" Instruction : 0x%x\n",TxBuf[4]);
chris	0:be51952765ec	413	}
chris	0:be51952765ec	414
chris	0:be51952765ec	415	// Start Address
chris	0:be51952765ec	416	TxBuf[5] = start;
chris	0:be51952765ec	417	sum += TxBuf[5];
chris	0:be51952765ec	418	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	419	printf(" Start : 0x%x\n",TxBuf[5]);
chris	0:be51952765ec	420	}
chris	0:be51952765ec	421
chris	0:be51952765ec	422	// data
chris	0:be51952765ec	423	for (char i=0; i<bytes ; i++) {
chris	0:be51952765ec	424	TxBuf[6+i] = data[i];
chris	0:be51952765ec	425	sum += TxBuf[6+i];
chris	0:be51952765ec	426	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	427	printf(" Data : 0x%x\n",TxBuf[6+i]);
chris	0:be51952765ec	428	}
chris	0:be51952765ec	429	}
chris	0:be51952765ec	430
chris	0:be51952765ec	431	// checksum
chris	0:be51952765ec	432	TxBuf[6+bytes] = 0xFF - sum;
chris	0:be51952765ec	433	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	434	printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
chris	0:be51952765ec	435	}
chris	0:be51952765ec	436
chris	0:be51952765ec	437	// Transmit the packet in one burst with no pausing
chris	0:be51952765ec	438	for (int i = 0; i < (7 + bytes) ; i++) {
chris	0:be51952765ec	439	_ax12.putc(TxBuf[i]);
chris	0:be51952765ec	440	}
chris	0:be51952765ec	441
chris	0:be51952765ec	442	// Wait for data to transmit
chris	0:be51952765ec	443	wait (0.00002);
chris	0:be51952765ec	444
chris	0:be51952765ec	445	// make sure we have a valid return
chris	0:be51952765ec	446	Status[4]=0x00;
chris	0:be51952765ec	447
chris	0:be51952765ec	448	// we'll only get a reply if it was not broadcast
chris	0:be51952765ec	449	if (_ID!=0xFE) {
chris	0:be51952765ec	450
chris	0:be51952765ec	451	// response is always 6 bytes
chris	0:be51952765ec	452	// 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
chris	0:be51952765ec	453	for (int i=0; i < 6 ; i++) {
chris	0:be51952765ec	454	Status[i] = _ax12.getc();
chris	0:be51952765ec	455	}
chris	0:be51952765ec	456
chris	0:be51952765ec	457	// Build the TxPacket first in RAM, then we'll send in one go
chris	0:be51952765ec	458	if (AX12_WRITE_DEBUG) {
chris	0:be51952765ec	459	printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
chris	0:be51952765ec	460	printf(" ID : %d\n",Status[2]);
chris	0:be51952765ec	461	printf(" Length : %d\n",Status[3]);
chris	0:be51952765ec	462	printf(" Error : 0x%x\n",Status[4]);
chris	0:be51952765ec	463	printf(" Checksum : 0x%x\n",Status[5]);
chris	0:be51952765ec	464	}
chris	0:be51952765ec	465
chris	0:be51952765ec	466
chris	0:be51952765ec	467	}
chris	0:be51952765ec	468
chris	0:be51952765ec	469	return(Status[4]); // return error code
chris	0:be51952765ec	470	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	29 Apr 2016
Imports:	3
Forks:	0
Commits:	16
Dependents:	3
Dependencies:	0
Followers:	24

AX12.cpp@1:93ad80f5fde7, 2011-03-30 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning