Testing and evaluating Servos performance

by

(22-10-2010)

With the CM5 firmware ready, its now time to make a little program on the roboard, to test the servos.

This program will test the delay that the servos take moving between several angles, for example between 0º and 300º, or 0º to 100º, etc.

We will also run these tests on the  same servos, in several conditions, with different loads, different supply voltages, different maximum torques, and both at the same time.

We will also test the torques with different voltages .

The idea is to profile the servos as best we can so that they can accurately be emulated in EZPysics.

(27-10-2010)

At this point, we wrote a little program that sends the servos to a start point, and then to an end point, and repeats the process for 10 times.

During this process, the program saves to a file, every servomotor positions and make a timestamp with uSeconds precision, each time it receives a new packet from Roboard containing the servos data.

We reached the following results so far:

1) With 10 servos we get and average of 283 packets per second coming from the CM5

2) Some servos are slower than others.

3) around 1 second to go from 0 to 1023 which means 300º/sec at 12V in accordance with AX12 specs

4) the worst delay between any two servos was around 21º for completion of the 300º. This translates to around 7% error.

5) The old AX12 servos seem to be more problematic and some of the physical connectors seem to be faulty maybe due to so much messing around.

After this we just took the file from the roboard, using winscp, and imported it to Exel, elaborating a graphic, to easily visualize, the behaviour of the servos in time.

In the picture below you can see that some AX12 servos are faster than others. The servos are told to move from position 0 to position 1023 and then back. During the transition, the CM5 continuously captures the servo positions (at around 280 times per second) and sends those positions to the RoBoard, which in turn stores them in CSV file format which Excel then transforms into nice graphs.

The top graph shows the difference between the maximum and minimum values among the 10 servos sampled at 280/sec. The bottom graph shows the absolute positions of the fastest and slowest servo.

As you can see, the RoBoard waits for all the servos to reach 0 before sending them off to 1023, then waiting for all of them to reach that position and so on.

See the full article to view the C code running on the RoBoard Linux

Links to related files:

1) Source Code for RoBoard and CM5
2)  Excel Spreadsheet used for the above pretty pictures
3) CSV Output of the position samples as below

The output of the code below looks like this (first line has the servo IDs, right column has microsecond timestamp)

1	2	6	8	9	10	11	13	15	18	-

0	4	0	0	0	0	0	0	0	1	25334065
0	4	0	0	0	0	1	0	0	2	25338071
0	4	0	0	0	1	1	1	0	3	25342080
1	4	0	0	0	1	2	2	0	3	25347000
3	4	1	0	0	1	4	4	0	4	25351007
4	4	2	0	0	2	4	4	0	4	25355026
5	4	2	0	0	2	4	4	0	4	25359031
5	4	2	0	1	2	4	6	1	4	25363040
6	4	3	2	1	3	5	6	2	4	25367045
6	4	3	3	1	3	5	6	2	5	25371055
5	4	3	4	2	3	5	6	2	4	25375063
6	5	3	4	2	3	5	6	4	5	25379065
6	4	3	4	2	3	5	6	4	4	25383074
6	4	4	5	2	4	5	6	6	4	25387082
6	4	3	5	3	3	5	6	6	4	25392006
6	4	4	5	3	3	5	6	6	4	25396016
6	4	3	5	3	3	5	6	6	4	25400021
5	3	3	5	3	3	5	4	6	4	25404023
4	3	3	5	3	3	4	4	6	3	25408037
4	2	4	5	3	3	4	3	4	3	25412046
4	1	3	5	3	3	4	4	6	3	25416049
3	1	3	5	3	3	3	3	6	3	25420056
3	1	3	4	4	3	2	3	6	3	25424065
3	1	3	4	3	3	2	3	6	3	25428075
2	1	3	4	3	3	2	2	6	3	25432085
1	1	3	3	3	2	2	2	4	2	25436993
1	1	3	3	4	2	2	1	4	2	25441003
1	1	3	3	3	2	2	2	3	2	25445016
0	1	4	3	3	2	2	1	2	2	25449022
1	1	3	3	4	2	2	1	2	2	25453029
0	1	3	3	3	2	2	1	2	2	25457032
0	1	2	3	3	2	2	1	2	2	25461044
0	1	2	2	3	2	2	1	2	2	25465051
0	1	2	2	2	2	2	1	2	2	25469058
1	1	2	1	2	2	2	1	2	2	25473066
0	1	2	1	1	2	2	1	2	2	25477082
0	1	2	0	1	2	2	1	2	2	25481995
1	1	2	0	1	2	2	1	2	2	25484079
1	1	2	0	2	2	1	1	2	2	25489008
1	1	1	1	2	2	2	1	2	2	25493017
1	1	2	1	2	2	2	2	2	3	25497027
2	3	2	3	3	4	3	4	2	4	25501036
4	4	4	3	5	6	7	7	6	5	25505051
6	5	8	6	7	7	8	8	7	8	25509058
9	9	10	7	9	11	9	9	9	10	25513062
11	11	10	10	11	12	12	12	12	13	25517069
13	12	12	13	13	13	13	13	14	16	25521993
16	14	16	16	15	16	16	17	17	20	25526002
19	20	19	18	20	19	21	20	20	22	25530007
23	22	22	22	22	21	23	23	22	25	25534015
27	25	26	24	27	26	28	25	26	28	25538028
29	30	29	29	31	29	30	30	31	32	25542043
32	33	35	33	36	32	36	34	34	35	25546046
37	36	38	38	40	35	39	38	39	38	25550054
40	41	42	41	43	39	45	42	42	41	25554064
44	45	45	45	49	45	47	45	46	46	25558077
49	48	51	51	52	47	52	51	51	50	25562990
52	53	54	54	58	51	57	54	55	54	25567000
57	58	60	58	60	56	61	58	58	60	25571006
62	61	63	61	65	60	65	63	63	63	25575026
66	67	68	68	68	64	70	67	66	68	25579031
70	70	73	74	74	69	74	71	71	72	25583040
76	76	77	77	79	75	79	75	75	75	25587050
82	80	84	85	85	79	84	79	81	81	25592026
89	84	87	90	88	85	89	84	85	85	25596054
93	89	92	95	93	89	92	88	88	89	25600068
96	93	97	99	99	93	97	95	94	94	25604998
100	98	103	105	105	99	102	98	100	97	25609047
106	104	107	110	110	104	108	103	104	105	25613995
110	109	112	114	114	108	111	110	108	109	25618025
116	113	115	118	118	115	117	116	115	115	25622063
120	120	121	124	125	118	121	119	120	122	25627011
125	125	125	129	129	122	125	124	125	127	25631027
132	128	131	134	136	128	130	130	129	132	25635051
134	134	134	139	141	135	138	135	135	136	25640011
142	139	141	145	146	139	142	138	139	142	25644050
145	145	146	149	150	144	148	142	143	146	25648075
150	149	152	154	154	149	152	150	148	151	25652994
156	155	156	160	161	154	159	153	153	157	25657050
160	160	162	166	164	159	164	158	159	162	25661999
166	166	165	172	169	165	171	163	162	168	25666018
171	171	172	176	175	169	175	169	169	174	25670025
177	177	175	181	180	175	182	174	175	178	25674072
183	181	181	188	186	179	185	179	180	185	25679024
187	188	186	191	190	184	190	184	185	190	25683050
191	193	191	197	195	188	197	188	191	194	25687071
197	198	197	204	202	196	202	195	196	199	25692034
202	203	201	211	208	202	207	200	200	206	25696071
207	208	207	213	213	207	212	205	204	210	25701000
211	214	213	220	219	214	219	212	210	216	25705059
217	222	219	224	222	218	227	216	215	221	25710000
221	227	223	230	227	224	229	222	220	228	25714026
228	231	230	234	233	230	238	227	227	233	25718982
231	239	234	240	239	236	244	234	233	239	25723034
237	243	239	244	244	241	249	239	238	243	25727057
242	248	245	251	250	249	254	244	245	250	25732018
247	257	251	255	256	254	259	249	251	253	25736049
252	262	256	259	263	260	264	256	256	261	25740987
256	266	262	264	269	266	272	263	263	266	25745040
263	273	266	270	273	270	276	268	267	273	25749981
270	277	273	275	279	276	282	273	273	279	25753999
274	284	278	280	284	280	287	279	278	285	25758026
280	289	285	286	290	286	292	284	283	291	25762985
287	296	290	292	295	292	298	288	287	296	25767017
292	302	294	297	301	296	303	296	294	301	25771029
296	306	300	303	304	301	308	298	298	304	25775047
301	311	304	306	309	306	312	304	302	309	25779056
307	317	309	312	315	310	317	309	308	314	25783062
312	321	313	316	319	315	323	315	311	319	25787066
315	326	317	323	322	320	328	319	318	326	25792017
321	333	323	328	329	328	336	324	323	330	25796067
326	337	328	334	334	333	341	330	328	337	25800999
331	343	333	339	338	337	347	336	333	342	25805037
336	349	339	345	345	344	352	341	339	348	25810007
344	355	344	349	349	348	356	348	344	352	25814030
349	362	348	353	355	353	361	353	348	356	25818070
355	364	355	361	362	361	367	359	354	363	25823028
362	372	359	365	368	366	372	364	360	366	25827051
366	375	363	369	374	373	381	371	364	372	25832032
373	383	370	376	378	377	386	376	371	376	25836977
379	389	375	381	382	383	391	382	373	382	25841017
387	396	381	387	391	391	398	391	381	390	25845990
391	404	385	393	394	395	402	396	384	394	25850014
395	410	391	397	399	401	410	400	391	401	25855120
401	416	396	403	405	407	415	407	393	407	25859030
406	421	402	408	410	414	423	413	398	414	25863988
412	427	408	415	418	418	427	418	403	418	25868035
418	433	415	420	422	425	435	424	408	424	25872998
422	439	421	425	430	430	440	429	414	429	25877049
430	445	427	430	435	438	447	435	418	435	25882005
435	451	432	434	442	446	451	444	425	440	25886046
442	459	437	441	448	451	459	449	430	447	25891002
448	465	444	446	455	459	466	455	437	453	25895058
456	469	449	452	459	463	473	461	444	461	25900013
461	476	455	456	464	471	481	468	447	465	25904055
468	481	460	463	472	475	484	473	453	471	25909012
473	489	467	469	477	482	490	480	459	478	25913050
479	496	471	474	484	489	498	485	465	484	25918014
486	500	479	479	489	494	504	490	471	490	25922049
490	507	485	486	495	505	511	497	477	495	25926996
497	513	490	491	503	510	517	503	483	504	25931065
502	521	496	497	509	514	521	509	490	511	25935978
509	525	501	501	515	520	529	515	496	515	25939991
514	531	508	507	521	525	534	522	501	521	25944001
520	536	512	511	528	530	540	529	505	529	25948011
525	543	518	516	533	537	545	535	512	533	25952018
530	549	523	521	536	543	553	541	516	537	25956023
538	555	529	528	543	548	558	546	522	544	25960029
542	561	533	531	550	554	564	550	528	552	25964040
550	567	539	537	555	560	571	558	533	557	25968049
553	573	544	542	560	565	576	561	539	564	25972061
560	578	551	546	566	571	582	568	544	568	25976975
565	582	555	553	573	578	587	572	550	574	25980983
571	588	561	556	578	581	593	577	554	579	25984993
578	593	568	563	584	587	597	586	561	585	25988998
582	599	571	569	590	593	603	591	565	589	25993005
589	605	576	575	597	599	607	596	571	595	25997024
594	611	582	578	602	603	614	601	576	600	26001053
599	615	586	585	607	608	618	607	582	606	26005040
607	622	593	590	611	615	625	613	587	611	26009989
612	628	599	597	617	622	631	619	594	618	26014041
619	632	605	603	622	627	637	624	600	622	26018972
623	641	612	608	626	633	642	632	605	629	26023015
631	646	617	614	632	640	649	636	612	635	26027984
635	651	622	619	636	645	653	641	618	640	26031999
641	657	627	626	643	652	659	649	622	644	26036044
647	661	634	633	649	655	666	655	628	652	26041014
650	667	640	636	655	661	670	661	633	655	26045039
655	674	644	642	663	667	679	663	639	661	26049997
663	680	650	646	667	671	683	670	644	667	26054040
667	684	655	653	672	680	689	675	647	673	26058990
673	689	661	656	679	684	694	682	653	679	26063048
679	695	666	662	684	690	702	686	659	685	26067993
683	701	670	668	690	696	705	691	664	691	26072044
689	706	675	673	694	701	711	699	669	696	26076998
693	711	679	679	698	707	717	703	674	702	26081048
698	718	686	684	706	712	722	708	682	708	26086003
704	722	692	690	711	717	730	713	685	713	26090043
708	729	697	695	717	724	735	720	690	718	26095005
715	733	704	701	723	728	741	726	696	724	26099050
720	741	708	706	731	734	746	731	700	727	26103996
725	746	716	712	734	740	753	737	706	735	26108055
731	752	720	715	740	747	759	743	711	740	26113001
735	759	724	720	745	751	765	747	719	746	26117057
741	764	730	727	750	758	771	753	724	752	26122012
747	771	735	731	757	766	776	758	728	757	26126057
753	775	742	739	760	770	781	764	735	763	26131037
760	782	746	744	767	778	789	772	739	770	26135053
764	787	753	748	771	784	795	777	745	775	26140002
771	791	759	754	775	790	801	783	750	781	26144969
774	799	763	761	785	798	807	788	756	787	26149007
780	803	770	767	791	802	811	795	761	792	26153966
783	810	776	771	797	809	817	801	768	800	26158009
790	814	780	777	803	813	822	808	774	804	26162975
796	818	787	780	807	819	828	813	778	811	26167018
801	825	791	786	814	826	834	819	785	815	26171969
809	832	798	790	820	830	840	824	790	823	26176023
813	836	803	797	825	836	847	830	797	828	26180976
821	840	808	802	831	842	851	837	802	833	26185024
824	845	813	809	837	849	859	842	808	839	26189985
830	851	820	814	842	854	862	848	815	845	26194046
834	858	825	819	848	861	870	855	821	852	26198990
840	863	830	823	854	865	874	859	827	858	26203043
846	871	836	829	861	871	881	865	833	865	26208011
852	876	842	835	866	875	887	873	839	868	26212966
859	881	847	843	870	883	892	877	844	874	26217031
863	887	851	847	875	888	897	882	849	879	26221986
869	892	858	853	882	895	904	889	852	884	26226054
874	897	863	858	888	899	909	894	859	891	26231049
880	903	868	865	894	907	916	900	864	899	26235974
884	908	876	870	901	910	919	905	870	904	26240022
890	914	880	877	907	917	927	911	875	910	26245000
895	920	887	881	911	922	933	914	883	914	26249035
902	926	892	888	916	928	938	922	887	922	26254012
908	931	898	893	921	934	943	928	894	926	26258962
913	938	901	899	927	940	950	933	899	934	26263035
920	943	908	904	932	945	956	939	906	938	26268004
924	948	912	910	939	952	960	945	909	944	26272966
931	955	918	916	945	958	967	951	915	949	26277040
936	962	921	921	950	962	973	957	921	955	26282007
943	965	928	928	955	969	981	964	925	959	26286972
949	973	936	932	962	976	987	970	932	966	26291046
954	978	939	940	966	981	992	977	938	971	26295998
960	985	946	945	972	988	997	982	944	980	26300050
965	989	949	952	977	994	1005	989	949	983	26305012
971	995	957	957	983	999	1010	994	955	990	26309971
978	1001	962	964	989	1006	1015	1000	960	996	26314032
981	1008	969	970	994	1013	1020	1007	968	1004	26318995
989	1012	972	977	1001	1019	1023	1011	972	1009	26323034
994	1017	979	980	1008	1023	1023	1018	977	1015	26327996
1002	1023	984	988	1013	1023	1023	1022	983	1019	26332044
1005	1023	990	994	1016	1023	1023	1023	990	1023	26336978
1011	1023	994	998	1021	1023	1023	1023	995	1023	26341007
1016	1023	1000	1004	1023	1023	1023	1023	999	1023	26344998
1020	1023	1005	1008	1023	1023	1023	1023	1006	1023	26349005
1023	1023	1010	1014	1023	1023	1023	1023	1011	1023	26353018
1023	1023	1015	1019	1023	1023	1023	1023	1017	1023	26357998
1023	1023	1019	1022	1023	1023	1023	1023	1020	1023	26362018
1023	1023	1016	1023	1023	1023	1023	1023	1023	1023	26366031
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26370036
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26374960
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26378970
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26382974
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26386984
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26391015
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26395003
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26399005
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26403013
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26407021
1023	1023	1023	1023	1023	1023	1023	1023	1023	1023	26411054
1023	1023	1023	1023	1023	1023	1022	1023	1023	1023	26415041
1023	1023	1023	1023	1023	1023	1019	1023	1023	1023	26419955
1023	1020	1022	1023	1021	1019	1014	1022	1023	1020	26423976
1023	1017	1019	1021	1016	1015	1012	1020	1021	1018	26428002
1022	1012	1016	1019	1014	1011	1008	1015	1019	1013	26432030
1019	1010	1012	1014	1011	1006	1003	1011	1015	1009	26436988
1015	1004	1009	1011	1007	1001	1000	1008	1012	1005	26441033
1011	1000	1005	1006	1000	996	996	1005	1008	1002	26445037
1009	996	1002	1002	994	993	990	1000	1004	997	26449972
1002	990	995	997	991	987	986	994	999	991	26454021
998	987	993	994	987	983	981	989	994	987	26458954
994	983	988	988	982	978	976	986	990	982	26462980
988	978	981	983	978	975	970	980	985	977	26467038
983	974	979	979	973	969	966	975	982	972	26471975
979	968	973	973	968	963	960	970	975	967	26475995
974	964	969	969	964	959	956	964	972	963	26480018
969	957	962	965	960	955	950	959	965	956	26484985
964	953	959	958	955	950	947	955	961	953	26489017
960	948	953	955	950	944	941	951	957	949	26493043
954	944	949	949	943	940	937	945	952	945	26497973
949	939	943	943	939	934	931	939	946	939	26502034
944	932	937	938	934	928	927	934	939	934	26506968
941	929	933	932	927	923	922	932	936	931	26511002
934	924	929	928	924	918	917	925	932	925	26514997
930	917	921	923	919	915	909	920	925	918	26519966
923	913	919	918	915	909	906	915	922	913	26523993
919	908	913	913	908	905	901	911	918	909	26528022
914	904	909	908	907	899	897	906	913	903	26532041
908	897	903	903	901	893	891	899	907	898	26537016
903	892	900	898	894	889	886	895	902	892	26541061
896	887	893	893	887	883	880	890	898	885	26545964
892	882	889	889	884	879	875	886	892	879	26550027
885	878	883	883	875	872	870	880	887	874	26554971
883	872	879	877	872	867	865	875	882	870	26558992
878	867	873	871	868	863	860	869	875	866	26562999
871	861	867	866	863	858	853	864	871	861	26567966
866	855	861	862	857	853	848	858	865	855	26572009
861	849	857	856	852	846	843	852	861	849	26576023
856	845	851	851	847	840	838	848	856	844	26580031
851	839	848	846	841	836	834	843	851	839	26584953
845	834	842	840	836	831	828	838	848	834	26588969
840	828	837	835	832	825	822	831	842	827	26593038
834	824	830	829	826	820	818	826	836	822	26597978
830	821	826	824	821	814	812	822	833	817	26601990
825	814	820	820	816	810	808	817	826	813	26605992
821	810	815	815	811	805	805	813	822	807	26610009
813	804	810	810	806	799	798	806	813	802	26614950
809	799	805	803	799	793	792	799	809	795	26619011
803	792	800	799	793	790	787	795	803	791	26623032
798	788	796	793	789	782	781	788	798	784	26627964
790	781	789	788	782	776	775	783	792	778	26632029
786	775	783	785	775	770	769	776	786	772	26636968
780	773	778	779	770	766	765	772	782	768	26641006
775	764	771	774	767	758	758	765	773	760	26645961
770	759	765	769	758	752	753	758	768	755	26649990
764	755	760	763	753	748	746	753	761	749	26654039
760	748	754	757	749	740	741	747	756	742	26659005
752	743	748	752	743	734	735	744	750	737	26663960
748	733	743	746	739	728	730	737	745	731	26668030
741	729	736	740	733	724	722	732	739	727	26672983
735	723	730	735	728	717	717	725	735	719	26677949
730	719	724	729	722	713	712	720	727	715	26682000
724	712	720	723	717	707	709	714	723	709	26686963
717	707	714	718	710	703	702	708	716	705	26691042
713	701	708	712	705	696	696	703	711	698	26695987
707	696	704	709	698	690	690	697	705	694	26700033
703	689	697	703	692	685	684	691	700	686	26704996
697	686	691	698	689	680	679	686	694	681	26709957
691	680	685	693	681	671	674	682	689	675	26714017
687	673	678	689	679	666	667	675	684	668	26718979
682	665	674	681	672	661	663	670	680	662	26723947
674	661	670	676	667	657	656	662	674	656	26727994
671	654	663	670	660	650	651	659	668	651	26732965
664	651	657	665	655	645	645	651	662	646	26737007
659	644	653	659	649	640	639	649	658	642	26741981
653	639	648	654	644	633	635	640	651	634	26746024
648	632	643	647	638	626	629	635	647	629	26751001
642	626	636	643	632	620	622	630	641	622	26755029
638	620	630	637	627	614	617	622	635	618	26759977
632	613	624	633	622	609	611	618	632	613	26764032
625	607	617	626	617	603	605	612	625	606	26768995
620	600	613	621	613	598	600	607	620	600	26773956
612	597	606	615	608	592	594	599	613	592	26778010
608	589	601	608	599	587	588	594	609	586	26782979
599	583	593	604	595	580	582	585	601	581	26787023
594	577	587	596	588	573	578	579	596	576	26791977
587	572	583	592	584	567	572	573	588	568	26796025
580	566	576	586	576	561	564	568	583	564	26800996
575	558	571	578	569	556	557	561	578	559	26805023
568	553	563	573	565	550	552	556	571	552	26809976
561	545	559	567	557	544	545	549	565	545	26814023
555	538	553	562	552	537	539	545	560	537	26818960
549	534	545	556	546	530	532	537	553	532	26823014
542	525	540	550	541	525	526	532	549	526	26827971
538	521	534	544	533	519	520	526	543	520	26832013
531	513	529	537	530	511	513	519	537	513	26836973
524	508	522	534	522	506	509	510	532	507	26841023
517	502	515	528	517	499	501	503	525	501	26845959
511	493	509	521	509	493	495	498	519	492	26850003
507	488	505	516	504	484	488	490	514	487	26855055
498	481	498	510	497	479	483	486	505	479	26859029
492	476	490	506	490	473	477	480	501	473	26863991
487	468	486	499	484	466	471	475	494	467	26868950
480	463	477	493	478	461	463	467	488	462	26873011
474	455	472	487	472	454	457	461	481	455	26877975
468	448	467	482	465	447	449	455	473	448	26882026
461	442	459	476	459	440	445	448	468	441	26886991
455	438	454	470	455	434	438	443	462	437	26891959
448	430	449	464	449	427	434	435	457	431	26896010
441	424	441	459	442	424	426	429	450	425	26900996
435	417	436	454	435	416	422	424	445	421	26905027
427	411	430	449	430	411	415	416	439	414	26909998
422	405	424	442	422	405	409	411	434	410	26914959
416	399	416	437	419	400	401	407	427	402	26919010
409	391	410	432	411	393	398	400	423	396	26923982
405	386	404	426	405	386	390	395	415	389	26928951
400	378	398	420	399	380	384	388	412	382	26933004
393	372	392	416	394	374	378	381	405	377	26937972
389	365	388	409	389	368	372	373	402	372	26942017
382	362	381	406	385	362	366	370	396	366	26946988
376	355	376	399	380	358	361	362	391	362	26951937
371	350	370	394	374	352	356	358	385	357	26956002
364	344	364	387	368	346	352	350	381	353	26960984
357	337	359	382	362	342	347	344	375	348	26965020
352	333	354	378	356	336	342	340	371	341	26969965
344	326	349	370	352	330	335	336	365	336	26974022
339	322	342	366	346	324	329	330	361	329	26978971
332	315	338	361	338	318	322	323	354	325	26983948
327	310	332	354	336	312	317	319	350	318	26987990
321	304	327	349	329	308	311	311	344	313	26992977
317	298	320	345	323	302	307	308	339	308	26997025
310	291	314	337	319	298	300	299	332	303	27001996
306	285	309	333	315	292	297	296	328	296	27006947
299	280	305	327	308	287	290	290	321	291	27011937
294	273	301	322	303	281	285	284	317	284	27015992
287	266	293	316	298	275	279	279	310	278	27020987
283	259	288	310	293	270	273	273	306	273	27025023
277	254	283	305	288	266	266	268	300	265	27029982
269	248	276	297	283	260	261	262	294	258	27034977
264	242	270	292	276	253	256	256	288	253	27039959
256	236	263	287	269	246	251	248	283	247	27044012
252	230	256	281	264	240	246	243	276	241	27049011
246	224	252	274	256	234	238	236	272	236	27053983
241	217	244	269	250	228	233	232	266	229	27058942

----------------------------------------------------------------------------------------
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/types.h>
#include <time.h>
#include <linux/serial.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include "dynamixel.h"
#include <termios.h>
#include <unistd.h> 

int fdFTDI=-1; 

typedef struct SERVOSDATA
{
    unsigned char ID; //1st byte
    unsigned int Pos;//2nd byte
    unsigned char Error;
}ServosDataStructure_Type; 

typedef struct SERVOAVERAGE
{
    unsigned char ID;
    float Pos;
}ServosAverageStructure_Type; 

#define AverageCalcNumberOfPositions 20
#define MaxServos       20

ServosDataStructure_Type ServosInputData[MaxServos];
ServosDataStructure_Type ServosOutputData[MaxServos];
ServosDataStructure_Type  ServosEndPos[MaxServos]; 

unsigned long int ServosTimeCounter[MaxServos];

unsigned char New_buffer[150];
unsigned char New_buffer_count=0;
unsigned char New_buffer_ready=0; 

unsigned char u8CurrentInputArray=0;
unsigned char u8CurrentOutputArray=0;
unsigned char Readed=0; 

unsigned char CRC_Calc(unsigned char *Array, unsigned char length);
int usb_open(char *dev_name, float baudrate);

unsigned char NumberOfServos=0;
//unsigned int  NumberOfBytes=0;
//unsigned int  NumberOfGoodPackets=0;
unsigned int NumberOfPackesPerSec=0;
unsigned int PacksCounter=0;
unsigned int GoodPacks=0;
//unsigned int  NumberOfBadPackets=0;
//unsigned int  NumberOfFailedHeaders=0; 

#define TOTALWRITE 500 

FILE *fsFile; 

#define UseFile 1 

#define stiff 8 

long time1 = 0;
long time2 = 0; 

void nonblock(int state)
{
    struct termios ttystate;
    //get the terminal state
    tcgetattr(STDIN_FILENO, &ttystate); 

    if (state==1)
    {
        //turn off canonical mode
        ttystate.c_lflag &= ~ICANON;
        //minimum of number input read.
        ttystate.c_cc[VMIN] = 1;
    }
    else if (state==0)
    {
        //turn on canonical mode
        ttystate.c_lflag |= ICANON;
    }
    //set the terminal attributes.
    tcsetattr(STDIN_FILENO, TCSANOW, &ttystate);
} 

int main(int ac, unsigned char **args) /*FOLD00*/
{
    char FileName[100];
    unsigned int CompCounter=0;
    unsigned int state=0;
    unsigned char u8TestServoNumber=0;
    unsigned char pause=0;
    char KeyBoardChar=0;
    int KeyboardCount=0;
    int deviceNumber=0;
    float portSpeed=0;
    int temporary=0;
    char stringPort[20];
    struct termios termios_p;
    unsigned int j=0xffff;
    unsigned int i=0;
    unsigned long TimeCalcule=0;
    time_t       TimeCounterStartSec=0;
    time_t        TimeCounterEndSec=0;
    suseconds_t   TimeCounterStartuSec=0;
    suseconds_t   TimeCounterEnduSec=0;
    struct timeval detail_time;
    unsigned char fileHeaderPlaced=0;
    //unsigned char *broadcast1=&New_buffer[11]; 

    /******************************************************************************\
    |*   Variables to configure the file name, the servo to test(0 to all servos) *|
    |* the number of repetitions, etc                                             *|
    \******************************************************************************/ 

    #define BEGIN_ANGLE 0x0001
    #define END_ANGLE   0x03FF
    #define NUMBER_TESTS 20

    memcpy(FileName,"testingTimes.txt",17);// file name
    u8TestServoNumber=0;//test all servos 

    deviceNumber=(atoi(args[1]));
    portSpeed= atoi(args[2]); 

    sprintf(stringPort,"/dev/ttyUSB%d",deviceNumber); 

    printf("Opening Port %s at %f Baud\n\r", stringPort , portSpeed); 

    fdFTDI = usb_open( stringPort , portSpeed ); 

    /******************************************************************************
    fdFTDI = open(stringPort,O_NONBLOCK|O_RDWR|O_NOCTTY); 

    if(fdFTDI==-1)
    {
        //shit hapened
        puts("Error Opening");
    }
    puts("Port Open\n\rApply Settings"); 

    termios_p.c_iflag=IGNPAR;
    termios_p.c_oflag=0;
    termios_p.c_cflag= B115200|CREAD|CS8|CLOCAL;
    termios_p.c_lflag=0;
    termios_p.c_cc[VINTR]    = 0;     // Ctrl-c
    termios_p.c_cc[VQUIT]    = 0;     // Ctrl-\
    termios_p.c_cc[VERASE]   = 0;     // del
    termios_p.c_cc[VKILL]    = 0;     // @
    termios_p.c_cc[VEOF]     = 0;     // Ctrl-d
    termios_p.c_cc[VTIME]    = 0;     // inter-character timer unused
    termios_p.c_cc[VMIN]     = 0;     // blocking read until 1 character arrives
    termios_p.c_cc[VSWTC]    = 0;     // ''
    termios_p.c_cc[VSTART]   = 0;     // Ctrl-q
    termios_p.c_cc[VSTOP]    = 0;     // Ctrl-s
    termios_p.c_cc[VSUSP]    = 0;     // Ctrl-z
    termios_p.c_cc[VEOL]     = 0;     // ''
    termios_p.c_cc[VREPRINT] = 0;     // Ctrl-r
    termios_p.c_cc[VDISCARD] = 0;     // Ctrl-u
    termios_p.c_cc[VWERASE]  = 0;     // Ctrl-w
    termios_p.c_cc[VLNEXT]   = 0;     // Ctrl-v
    termios_p.c_cc[VEOL2]    = 0;     // ''  

    tcflush(fdFTDI, TCIFLUSH);
    if(tcsetattr(fdFTDI,TCSANOW, &termios_p)==-1)
    {
        puts("Error setting configurations");
        return 0;
    }
    /******************************************************************/ 

    if(fdFTDI<0)  // open port USB0 at   1 to 1Mbps   3 to 500Kbps
    {
         puts("Error Opening Port");
        return 0;
    } 

    puts("Port Open."); 

    time1 = time(0); 

    nonblock(1); 

    puts("Press 's' to start."); 

    j=0xFFFF; 

    while(1)
    {
        if((j==0xffff) || (pause==2))
        {
            read_servos();
        }
        else
        if (read_servos() == 1)
        {
            gettimeofday(&detail_time,NULL);
            TimeCounterEndSec = (detail_time.tv_sec);
            TimeCounterEnduSec = (detail_time.tv_usec);
            //printf("time: %ld\n",TimeCounterEnd-TimeCounterStart);
            switch (state)
            {
                case 0:
                    if(fileHeaderPlaced==0)
                    {
                        gettimeofday(&detail_time,NULL); 

                        TimeCounterStartSec=TimeCounterEndSec = (detail_time.tv_sec);
                        TimeCounterStartuSec=TimeCounterEnduSec = (detail_time.tv_usec); 

                        for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++)
                        {
                            fprintf(fsFile,"%d;",ServosInputData[CompCounter].ID);
                        }
                        fprintf(fsFile,"\n");
                        fileHeaderPlaced=1;
                        New_buffer_prepare();
                    } 

                    for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++)
                    {
                        ServosOutputData[CompCounter].ID=ServosInputData[CompCounter].ID;
                        ServosOutputData[CompCounter].Pos=BEGIN_ANGLE;
                        //ServosEndPos[CompCounter].Pos=1024;
                        ServosTimeCounter[CompCounter]=0;
                    } 

                    if(New_buffer_ready>0)
                        break; 

                    New_buffer_ready=1;
                    //write_servos();
                    i=0;
                    state = 1;
                    printf("\rTest number: %d\n",j+1); 

                case 1: 

                    for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++)
                    {
                        if((ServosInputData[CompCounter].Pos == BEGIN_ANGLE) && (ServosTimeCounter[CompCounter]==0) )
                        {
                            i++;
                            ServosTimeCounter[CompCounter]=1;
                        } 

                        if(i==NumberOfServos)
                        { 

                            for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++)
                            {
                                ServosOutputData[CompCounter].ID=ServosInputData[CompCounter].ID;
                                ServosOutputData[CompCounter].Pos=END_ANGLE;
                                //ServosEndPos[CompCounter].Pos=1024;
                                ServosTimeCounter[CompCounter]=0;
                            } 

                            if(New_buffer_ready==1)
                                break; 

                            New_buffer_ready=1;
                            //write_servos();
                            //puts("\nreset position");
                            state=2;
                            i=0;
                        }
                    }
                    break; 

                case 2: 

                    for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++)
                    { 

                        if(ServosInputData[CompCounter].Pos == END_ANGLE && ServosTimeCounter[CompCounter]==0)
                        {
                            //printf("i: %d ID:%d\n",i,ServosInputData[CompCounter].ID);
                            //printf("time: %ld\n",TimeCounterEnd-TimeCounterStart);
                            //ServosTimeCounter[CompCounter]=(TimeCounterEnd-TimeCounterStart);
                            i++;
                        } 

                        if(i==NumberOfServos)
                        {
                            state=3;
                        }
                    }
                break; 

                case 3:
                    //puts("never here");
                    /*for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++)
                    {
                        //printf("servo: %d",ServosTimeCounter[CompCounter]);
                        fprintf(fsFile,"%d;",ServosTimeCounter[CompCounter]);
                    }
                        fprintf(fsFile,"\n");*/ 

                        state =0; 

                    if(j==(NUMBER_TESTS-1))
                    {
                        fclose(fsFile);
                        puts("\rend");
                        j=0xffff;
                        //exit(1);
                    }
                    else
                    {
                        j++;
                        if(pause==1)
                        pause=2;
                    } 

                break;
            } 

            for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++)
            {
                //printf("servo: %d",ServosTimeCounter[CompCounter]);
                fprintf(fsFile,"%d;",ServosInputData[CompCounter].Pos);
            } 

            TimeCalcule = ((TimeCounterEndSec-TimeCounterStartSec)*1000000);
            TimeCalcule+= (TimeCounterEnduSec-TimeCounterStartuSec);
            fprintf(fsFile,"%d\n",TimeCalcule); 

        } 

        ioctl(0, FIONREAD, &KeyboardCount);
        if(KeyboardCount>0)
        {
            if (read (0, &KeyBoardChar, 1) > 0) //if(KeyboardCount>0)
            {
                //puts("dumass");
                //KeyBoardChar=getchar();
                if(KeyBoardChar=='x')
                { 

                    //fclose(fsFile);
                    puts("\rexit");
                    exit(1);
                }
                else
                if(KeyBoardChar=='g')
                {
                    printf("\rGoodPacks/s %d\n",NumberOfPackesPerSec);
                }
                else
                if(KeyBoardChar=='p')
                {
                    if((j<0xFFFF) && (pause==0))
                    {
                        puts("\rpause");
                        pause=1;
                    }
                    else
                    {
                        puts("\rNo pausing to do.");
                    }
                }
                else
                if(KeyBoardChar=='b' )
                {
                    if(j==0xFFFF)
                    {
                        puts("\rAlready stoped");
                    }
                    else
                    {
                        fprintf(fsFile,"break\n");
                        fclose(fsFile);
                        puts("\rbreak");
                        j=0xffff;
                    }
                }
                else
                if(KeyBoardChar=='s')
                {
                    if(pause==2)
                    {
                        puts("\rContinuing...");
                        pause=0;
                    }
                    else
                    if(pause==0)
                    {
                        if(j==0xFFFF)
                        {
                            puts("\r ");
                            fsFile = fopen(FileName,"a+");
                            if(fsFile==NULL)
                            {
                                puts("\rError Opening Log File");
                                return;
                            } 

                            fprintf(fsFile,"Start\n");
                            puts("\rStarted.");
                            fileHeaderPlaced=0;
                            state=0;
                            j=0;
                        }
                        else
                        {
                            puts("\rAlready running...");
                        }
                    }
                }
                else
                {
                    puts("\rUnknown Command");
                }
                /*else
                if(KeyBoardChar=='r')
                {
                    j=0;
                }*/
                KeyBoardChar=0;
            }
        }
        write_servos(); 

        time2=time(0);
        if(time2>time1)
        {
            NumberOfPackesPerSec=PacksCounter;
            PacksCounter=0;
            time1=time(0);
        }
        //write_terminal();
    } 

} 

New_buffer_prepare() {
    unsigned short i; 

    i = 7 + NumberOfServos*3 + 1;
    New_buffer[0] = 0xFA;
    New_buffer[1] = 0xFA;
    New_buffer[2] = i>>8; // msb
    New_buffer[3] = i&0xff; // lsb
    New_buffer[4] = 0xff;
    New_buffer[5] = 0xff;
    New_buffer[6] = 0xfe;
    New_buffer[7] = NumberOfServos*3+4; // sync_write packet length
    New_buffer[8] = 0x83; // broadcast SYNC_WRITE
    New_buffer[9] = 0x1E; // start table position of "goal position"
    New_buffer[10] = 2;  // data to be written

        New_buffer_count= (11 + (3*NumberOfServos) + 1 + 1);
} 

int write_terminal(void)
{
    //do nothing
    return 0;
} 

write_servos() {
    static unsigned char buf[512];
    static short buf_index = -2;
    static short buf_count;
    int i;
    unsigned int j; 

// superpacket: msbLen lsbLen [ ff ff fe buflen 83 start count id0 data0 id1 data1... checksum ] [ ff ff fe buflen 83 start.... ] .. superpacketChecksum

    if (New_buffer_ready==1)
    { 

        for(i=0;i<NumberOfServos;i++)
        {
                j = (unsigned short)ServosOutputData[i].Pos;
                New_buffer[i*3+0+11] = ServosOutputData[i].ID;
                New_buffer[i*3+1+11] = j&0xff;
                New_buffer[i*3+2+11] = j>>8;
        } 

        for(j=0,i=6;i<New_buffer_count-2;i++) //checksum
        {
            j += New_buffer[i];
        }
        New_buffer[New_buffer_count-2] = ((0xffffffff-j)& 0xff); 

        for(j=0,i=2;i<New_buffer_count-1;i++) // superpacketChecksum
        {
            j += New_buffer[i];
        }
        New_buffer[New_buffer_count-1] = ((0xffffffff-j)& 0xff); 

        /*
        if(New_buffer_ready == 0)
            New_buffer_ready = 1;
        */ 

        /*
        printf("\nwr: ");
        for(i=0;i<New_buffer_count; i++) printf("%02x ",New_buffer[i]);
        printf("\n");
        */
        bcopy (New_buffer, buf, New_buffer_count); // try also buf=new_buffer
        New_buffer_ready = 2;
        buf_index = 0;
        buf_count = New_buffer_count; 

        New_buffer_ready=2;
    }
    if(New_buffer_ready == 2)
    {
        switch (write(fdFTDI, &buf[buf_index], 1))
        {
            case  1:
                //printf("%02x ",buf[buf_index]);
                if (buf_index == (buf_count-1))
                {
                    buf_index = -1;
                    New_buffer_ready=0;
                    //puts("\n");
                }
                else
                {
                    buf_index++;
                }
                break;
            case 0:
                // still writing previous byte
                break;
            case -1:
                    // some error (or maybe still writing previous byte we dont know)
                break;
            default:;
        }
    }
    else
    {
        //do nothing
    } 

} 

int read_servos(void)
{
    unsigned char ReadC=0;
    static unsigned char cPackLength=0;
    static int  BytesCounter=0;
    unsigned char CalculatedCRC=0;
    //static int NumberOfPackesPerSec=0; 

    unsigned char GotCRC=0;
    static unsigned char FoundFF=0;
    static unsigned char ServosCounter=0;
    static unsigned char u8InputArray[150];

        //printf("in\n");
        if(read(fdFTDI,&ReadC,1)<1)
        {
            return -1;
        }
        //NumberOfBytes++;
        //printf("%x",ReadC);
        if(FoundFF<2)// looking for the header
        {
            if(ReadC==0xFF)
            {
                FoundFF++; 

                if(FoundFF == 2)
                {
                    //printf("rd: FFFF");
                }
            }
            else // if no header byte found, is a byte to trash
            {
                if(FoundFF==1)
                {
                   // NumberOfFailedHeaders+=2;
                //FailedBytes+=2;
                            FoundFF=0;
                }
                else
                {
                //NumberOfFailedHeaders++;
                            //FailedBytes++;
                }
            }
        }
        else
        if(FoundFF==2)
        { 

            cPackLength=ReadC;
            u8InputArray[0]=ReadC; 

            if(NumberOfServos>0)
            {
                if(cPackLength!=(NumberOfServos*3))
                {
                    FoundFF=0;
                    NumberOfServos=0;
                    //printf("LenERR\n\rfs");
                }
                else
                {
                    FoundFF=4;
                    BytesCounter=0;
                    ServosCounter=0;
                }
            }
            else
            {
                FoundFF=3;
                BytesCounter=0;
                NumberOfServos=(cPackLength/3);
                New_buffer_prepare();
                ServosCounter=0;
            }
        }
        else
        if(FoundFF==3 || FoundFF ==4)
        { 

            //printf(" %d",ReadC);
            if(BytesCounter==(3*NumberOfServos))
            {
                //Finish read all pack
                        //puts("comp CRC");
                GotCRC=ReadC;
                        u8InputArray[BytesCounter+1]=ReadC;
                CalculatedCRC=CRC_Calc(&u8InputArray[1],(3*NumberOfServos));
                //printf("\nCalc %d  Got: %d",CalculatedCRC,GotCRC);
                if(GotCRC!=CalculatedCRC)
                {
                //NumberOfBadPackets++;
                            //puts("PacketError");
                FoundFF=0;
                BytesCounter=0;
                return -1; 

                }
                else
                {   //good packet found
                            //puts("goodPack");
                    PacksCounter++;
                    /*
                    printf("\nrd: FF FF");
                    for(BytesCounter=0;BytesCounter< (NumberOfServos*3+2);BytesCounter++)
                    {
                        printf(" %02x", u8InputArray[BytesCounter]);
                    }
                    */
                    //printf("  GoodPacks/s %d\r",NumberOfPackesPerSec); 

        //            NumberOfGoodPackets++;
                    //NumberOfServos=(cPackLength/4);
                    FoundFF=0;
                    BytesCounter=0; 

                            // now lets build the array 

                return 1;
                }
            }
            else
                if(BytesCounter<(3*NumberOfServos))
            { 

                if((BytesCounter%3)==0 && BytesCounter>0)
                {
                    ServosCounter++;
                } 

                if((BytesCounter%3)==0)
                {
                    ServosInputData[ServosCounter].ID=ReadC;
                }
                else
                if((BytesCounter%3)==1)
                {
                    ServosInputData[ServosCounter].Pos=ReadC;
                }
                else
                if((BytesCounter%3)==2)
                {
                    ServosInputData[ServosCounter].Pos+=(ReadC<<8);
                }
                u8InputArray[BytesCounter+1]=ReadC; 

                BytesCounter++;
            }
            else
            {
                //printf("PackLen: %d  Bytes %d ",cPackLength,BytesCounter);
                FoundFF=0;
                BytesCounter=0;
            } 

        } 

    return 0;
} 

unsigned char CRC_Calc(unsigned char *Array, unsigned char length) /*FOLD00*/
{
    unsigned int CRCResult=0;
    unsigned int CounterCRC=0; 

    while(CounterCRC<length)
    {
        CRCResult+=Array[CounterCRC++];
    } 

    return ((0xFFFFFFFF-CRCResult)&0xFF);
} 

int fd;
int usb_open(char *dev_name, float baudrate) /*FOLD00*/
{
    struct termios newtio;
    struct serial_struct serinfo;
    //int fd; 

    memset(&newtio, 0, sizeof(newtio)); 

    if((fd = open(dev_name, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) {
        fprintf(stderr, "device open error: %s\n", dev_name);
    return (-1);
    } 

    newtio.c_cflag          = B38400|CS8|CLOCAL|CREAD;
    newtio.c_iflag          = IGNPAR;
    newtio.c_oflag          = 0;
    newtio.c_lflag          = 0;
    newtio.c_cc[VTIME]      = 0;
    newtio.c_cc[VMIN]       = 0;
    tcflush(fd, TCIFLUSH);
    tcsetattr(fd, TCSANOW, &newtio); 

    if(ioctl(fd, TIOCGSERIAL, &serinfo) < 0) {
        fprintf(stderr, "Cannot get %s serial info\n", dev_name);
        return (-1);
        } 

    serinfo.flags &= ~ASYNC_SPD_MASK;
    serinfo.flags |= ASYNC_SPD_CUST;
    serinfo.custom_divisor = serinfo.baud_base / baudrate;
    if(ioctl(fd, TIOCSSERIAL, &serinfo) < 0) {
        fprintf(stderr, "Cannot set %s serial info\n", dev_name);
        return (-1);
    } 

    return (fd); 

} 

int usb_close() /*FOLD00*/
{
    close(fd);
}
Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s


%d bloggers like this: